CMAKE_C_FLAGS="-Wall -Wextra -Wabi -O2"
CMAKE_CXX_FLAGS="-Wall -Wextra -Wabi -O2"
+DOWNLOAD_DIR=$HOME/download
+
+export FLANN_ROOT=$HOME/flann
+export VTK_DIR=$HOME/vtk
+export QHULL_ROOT=$HOME/qhull
+export DOXYGEN_DIR=$HOME/doxygen
+
function build ()
{
case $CC in
{
# Configure
mkdir $BUILD_DIR && cd $BUILD_DIR
- cmake -DCMAKE_C_FLAGS=$CMAKE_C_FLAGS -DCMAKE_CXX_FLAGS=$CMAKE_CXX_FLAGS -DPCL_ONLY_CORE_POINT_TYPES=ON -DBUILD_global_tests=ON -DPCL_NO_PRECOMPILE=ON $PCL_DIR
+ cmake -DCMAKE_C_FLAGS=$CMAKE_C_FLAGS \
+ -DCMAKE_CXX_FLAGS=$CMAKE_CXX_FLAGS \
+ -DPCL_ONLY_CORE_POINT_TYPES=ON \
+ -DBUILD_global_tests=ON \
+ -DPCL_NO_PRECOMPILE=ON \
+ $PCL_DIR
# Build and run tests
- make pcl_filters -j3
- make test_filters
- make pcl_registration -j3
- make test_registration
- make test_registration_api
- make tests -j3
+ make tests
}
function doc ()
{
# Do not generate documentation for pull requests
if [[ $TRAVIS_PULL_REQUEST != 'false' ]]; then exit; fi
- # Install doxygen and sphinx
- sudo apt-get install doxygen doxygen-latex graphviz python-pip
- sudo pip install sphinx sphinxcontrib-doxylink
+ # Add installed doxygen to path and install sphinx
+ export PATH=$DOXYGEN_DIR/bin:$PATH
+ pip install --user sphinx sphinxcontrib-doxylink
# Configure
mkdir $BUILD_DIR && cd $BUILD_DIR
cmake -DDOXYGEN_USE_SHORT_NAMES=OFF \
-DSPHINX_HTML_FILE_SUFFIX=php \
- -DWITH_DOCS=1 \
- -DWITH_TUTORIALS=1 \
+ -DWITH_DOCS=ON \
+ -DWITH_TUTORIALS=ON \
$PCL_DIR
git config --global user.email "documentation@pointclouds.org"
# Commit and push
cd $DOC_DIR
git add --all
- git commit --amend -m "Documentation for commit $TRAVIS_COMMIT"
+ git commit --amend -m "Documentation for commit $TRAVIS_COMMIT" -q
git push --force
else
exit 2
fi
}
-case $TASK in
+function install_flann()
+{
+ local pkg_ver=1.8.4
+ local pkg_file="flann-${pkg_ver}-src"
+ local pkg_url="http://people.cs.ubc.ca/~mariusm/uploads/FLANN/${pkg_file}.zip"
+ local pkg_md5sum="a0ecd46be2ee11a68d2a7d9c6b4ce701"
+ local FLANN_DIR=$HOME/flann
+ local config=$FLANN_DIR/include/flann/config.h
+ echo "Installing FLANN ${pkg_ver}"
+ if [[ -d $FLANN_DIR ]]; then
+ if [[ -e ${config} ]]; then
+ local version=`grep -Po "(?<=FLANN_VERSION_ \").*(?=\")" ${config}`
+ if [[ "$version" = "$pkg_ver" ]]; then
+ local modified=`stat -c %y ${config} | cut -d ' ' -f1`
+ echo " > Found cached installation of FLANN"
+ echo " > Version ${pkg_ver}, built on ${modified}"
+ return 0
+ fi
+ fi
+ fi
+ download ${pkg_url} ${pkg_md5sum}
+ if [[ $? -ne 0 ]]; then
+ return $?
+ fi
+ unzip -qq pkg
+ cd ${pkg_file}
+ mkdir build && cd build
+ cmake .. \
+ -DCMAKE_BUILD_TYPE=Release \
+ -DCMAKE_INSTALL_PREFIX=$FLANN_DIR \
+ -DBUILD_MATLAB_BINDINGS=OFF \
+ -DBUILD_PYTHON_BINDINGS=OFF \
+ -DBUILD_CUDA_LIB=OFF \
+ -DBUILD_C_BINDINGS=OFF \
+ -DUSE_OPENMP=OFF
+ make -j2 && make install && touch ${config}
+ return $?
+}
+
+function install_vtk()
+{
+ local pkg_ver=5.10.1
+ local pkg_file="vtk-${pkg_ver}"
+ local pkg_url="http://www.vtk.org/files/release/${pkg_ver:0:4}/${pkg_file}.tar.gz"
+ local pkg_md5sum="264b0052e65bd6571a84727113508789"
+ local VTK_DIR=$HOME/vtk
+ local config=$VTK_DIR/include/vtk-${pkg_ver:0:4}/vtkConfigure.h
+ echo "Installing VTK ${pkg_ver}"
+ if [[ -d $VTK_DIR ]]; then
+ if [[ -e ${config} ]]; then
+ local version=`grep -Po "(?<=VTK_VERSION \").*(?=\")" ${config}`
+ if [[ "$version" = "$pkg_ver" ]]; then
+ local modified=`stat -c %y ${config} | cut -d ' ' -f1`
+ echo " > Found cached installation of VTK"
+ echo " > Version ${pkg_ver}, built on ${modified}"
+ return 0
+ fi
+ fi
+ fi
+ download ${pkg_url} ${pkg_md5sum}
+ if [[ $? -ne 0 ]]; then
+ return $?
+ fi
+ tar xzf pkg
+ cd "VTK${pkg_ver}"
+ mkdir build && cd build
+ cmake .. \
+ -Wno-dev \
+ -DCMAKE_BUILD_TYPE=Release \
+ -DBUILD_SHARED_LIBS=ON \
+ -DCMAKE_INSTALL_PREFIX=$VTK_DIR \
+ -DBUILD_DOCUMENTATION=OFF \
+ -DBUILD_EXAMPLES=OFF \
+ -DBUILD_TESTING=OFF \
+ -DVTK_USE_BOOST=ON \
+ -DVTK_USE_CHARTS=ON \
+ -DVTK_USE_VIEWS=ON \
+ -DVTK_USE_RENDERING=ON \
+ -DVTK_USE_CHEMISTRY=OFF \
+ -DVTK_USE_HYBRID=OFF \
+ -DVTK_USE_PARALLEL=OFF \
+ -DVTK_USE_PATENTED=OFF \
+ -DVTK_USE_INFOVIS=ON \
+ -DVTK_USE_GL2PS=OFF \
+ -DVTK_USE_MYSQL=OFF \
+ -DVTK_USE_FFMPEG_ENCODER=OFF \
+ -DVTK_USE_TEXT_ANALYSIS=OFF \
+ -DVTK_WRAP_JAVA=OFF \
+ -DVTK_WRAP_PYTHON=OFF \
+ -DVTK_WRAP_TCL=OFF \
+ -DVTK_USE_QT=OFF \
+ -DVTK_USE_GUISUPPORT=OFF \
+ -DVTK_USE_SYSTEM_ZLIB=ON \
+ -DCMAKE_CXX_FLAGS="-D__STDC_CONSTANT_MACROS"
+ make -j2 && make install && touch ${config}
+ return $?
+}
+
+function install_qhull()
+{
+ local pkg_ver=2012.1
+ local pkg_file="qhull-${pkg_ver}"
+ local pkg_url="http://www.qhull.org/download/${pkg_file}-src.tgz"
+ local pkg_md5sum="d0f978c0d8dfb2e919caefa56ea2953c"
+ local QHULL_DIR=$HOME/qhull
+ local announce=$QHULL_DIR/share/doc/qhull/Announce.txt
+ echo "Installing QHull ${pkg_ver}"
+ if [[ -d $QHULL_DIR ]]; then
+ if [[ -e ${announce} ]]; then
+ local version=`grep -Po "(?<=Qhull )[0-9.]*(?= )" ${announce}`
+ if [[ "$version" = "$pkg_ver" ]]; then
+ local modified=`stat -c %y ${announce} | cut -d ' ' -f1`
+ echo " > Found cached installation of QHull"
+ echo " > Version ${pkg_ver}, built on ${modified}"
+ return 0
+ fi
+ fi
+ fi
+ download ${pkg_url} ${pkg_md5sum}
+ if [[ $? -ne 0 ]]; then
+ return $?
+ fi
+ tar xzf pkg
+ cd ${pkg_file}
+ mkdir -p build && cd build
+ cmake .. \
+ -DCMAKE_BUILD_TYPE=Release \
+ -DCMAKE_CXX_FLAGS=-fPIC \
+ -DCMAKE_C_FLAGS=-fPIC \
+ -DCMAKE_INSTALL_PREFIX=$QHULL_DIR
+ make -j2 && make install && touch ${announce}
+ return $?
+}
+
+function install_doxygen()
+{
+ local pkg_ver=1.8.9.1
+ local pkg_file="doxygen-${pkg_ver}"
+ local pkg_url="http://ftp.stack.nl/pub/users/dimitri/${pkg_file}.src.tar.gz"
+ local pkg_md5sum="3d1a5c26bef358c10a3894f356a69fbc"
+ local DOXYGEN_EXE=$DOXYGEN_DIR/bin/doxygen
+ echo "Installing Doxygen ${pkg_ver}"
+ if [[ -d $DOXYGEN_DIR ]]; then
+ if [[ -e $DOXYGEN_EXE ]]; then
+ local version=`$DOXYGEN_EXE --version`
+ if [[ "$version" = "$pkg_ver" ]]; then
+ local modified=`stat -c %y $DOXYGEN_EXE | cut -d ' ' -f1`
+ echo " > Found cached installation of Doxygen"
+ echo " > Version ${pkg_ver}, built on ${modified}"
+ return 0
+ fi
+ fi
+ fi
+ download ${pkg_url} ${pkg_md5sum}
+ if [[ $? -ne 0 ]]; then
+ return $?
+ fi
+ tar xzf pkg
+ cd ${pkg_file}
+ ./configure --prefix $DOXYGEN_DIR
+ make -j2 && make install
+ return $?
+}
+
+function install_dependencies()
+{
+ install_flann
+ install_vtk
+ install_qhull
+ install_doxygen
+}
+
+function download()
+{
+ mkdir -p $DOWNLOAD_DIR && cd $DOWNLOAD_DIR && rm -rf *
+ wget --output-document=pkg $1
+ if [[ $? -ne 0 ]]; then
+ return $?
+ fi
+ if [[ $# -ge 2 ]]; then
+ echo "$2 pkg" > "md5"
+ md5sum -c "md5" --quiet --status
+ if [[ $? -ne 0 ]]; then
+ echo "MD5 mismatch"
+ return 1
+ fi
+ fi
+ return 0
+}
+
+case $1 in
+ install ) install_dependencies;;
build ) build;;
test ) test;;
doc ) doc;;
+sudo: false
language: cpp
compiler:
- gcc
- secure: PZivWbaCWFA2BFFY8n3UMxdEWjz7rBh568u9LF5LH3HgWADnfiwWzNriACqX9fhe7tSmDru5Bk978s+xPPAY9v24cfiDEX5a5MQ/XVr2rP48n3vlUDWERDhIodJ73F9F9GGZXToGdNz0MBUAHgiv7Lb0GYUfmOYzUJjWghngLBw=
matrix:
include:
- - compiler: clang
+ - compiler: gcc
env: TASK="test"
- env: TASK="doc"
+addons:
+ apt:
+ sources:
+ - kalakris-cmake
+ - boost-latest
+ - kubuntu-backports
+ packages:
+ - cmake
+ - libboost1.55-all-dev
+ - libeigen3-dev
+ - libgtest-dev
+ - doxygen-latex
+ - dvipng
+ - libusb-1.0-0-dev
+cache:
+ directories:
+ - $HOME/flann
+ - $HOME/vtk
+ - $HOME/qhull
+ - $HOME/doxygen
before_install:
- - sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl -y
- - sudo add-apt-repository ppa:apokluda/boost1.53 -y
- - sudo add-apt-repository ppa:yade-users/external -y
- - sudo add-apt-repository ppa:libreoffice/ppa -y
- - sudo apt-get update -d
-install:
- - sudo apt-get install libvtk5-qt4-dev libflann-dev libeigen3-dev libopenni-dev libqhull-dev libboost-filesystem1.53-dev libboost-iostreams1.53-dev libboost-thread1.53-dev libboost-chrono1.53-dev libusb-1.0-0-dev libgtest-dev
+ - bash .travis.sh install
script:
- - bash .travis.sh
+ - bash .travis.sh $TASK
--- /dev/null
+set(SUBSYS_NAME 2d)
+set(SUBSYS_DESC "Point cloud 2d")
+set(SUBSYS_DEPS common io filters)
+
+set(build TRUE)
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS vtk)
+
+PCL_ADD_DOC("${SUBSYS_NAME}")
+
+if(build)
+
+ set(srcs
+ src/convolution_2d.cpp
+ )
+
+ set(incs
+ "include/pcl/${SUBSYS_NAME}/convolution.h"
+ "include/pcl/${SUBSYS_NAME}/kernel.h"
+ "include/pcl/${SUBSYS_NAME}/edge.h"
+ "include/pcl/${SUBSYS_NAME}/morphology.h"
+ )
+
+ set(impl_incs
+ "include/pcl/${SUBSYS_NAME}/impl/convolution.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/edge.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/morphology.hpp"
+ )
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+
+ if(${VTK_FOUND})
+ set(VTK_USE_FILE "${VTK_USE_FILE}" CACHE INTERNAL "VTK_USE_FILE")
+ include("${VTK_USE_FILE}")
+ set(VTK_IO_TARGET_LINK_LIBRARIES vtkCommon vtkWidgets vtkIO vtkImaging)
+ endif(${VTK_FOUND})
+
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include" ${VTK_INCLUDE_DIRECTORIES})
+ PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs})
+ link_directories(${VTK_LINK_DIRECTORIES})
+ target_link_libraries("${LIB_NAME}" ${VTK_LIBRARIES} pcl_io)
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "")
+
+ #Install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs})
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs})
+
+endif(build)
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_2D_CONVOLUTION_H
+#define PCL_2D_CONVOLUTION_H
+
+#include <pcl/pcl_base.h>
+#include <pcl/filters/filter.h>
+#include <pcl/point_types.h>
+
+namespace pcl
+{
+ /**
+ * This typedef is used to represent a point cloud containing edge information
+ */
+ struct PointXYZIEdge
+ {
+ PCL_ADD_POINT4D; // preferred way of adding a XYZ+padding
+ float magnitude;
+ float direction;
+ float magnitude_x;
+ float magnitude_y;
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
+ } EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment
+
+ /** \brief A 2D convolution class. */
+ template <typename PointT>
+ class Convolution : public Filter<PointT>
+ {
+ public:
+ using Filter<PointT>::input_;
+
+ /**
+ * Extra pixels are added to the input image so that convolution can be performed over the entire image.
+ *
+ * (kernel_height/2) rows are added before the first row and after the last row
+ * (kernel_width/2) columns are added before the first column and after the last column
+ * border options define what values are set for these extra rows and columns
+ *
+ * Assume that the three rows of right edge of the image looks like this:
+ * .. 3 2 1
+ * .. 6 5 4
+ * .. 9 8 7
+ *
+ * BOUNDARY_OPTION_CLAMP : the extra pixels are set to the pixel value of the boundary pixel
+ * This option makes it seem as if it were:
+ * .. 3 2 1| 1 1 1 ..
+ * .. 6 5 4| 4 4 4 ..
+ * .. 9 8 7| 7 7 7 ..
+ *
+ * BOUNDARY_OPTION_MIRROR : the input image is mirrored at the boundary.
+ * This option makes it seem as if it were:
+ * .. 3 2 1| 1 2 3 ..
+ * .. 6 5 4| 4 5 6 ..
+ * .. 9 8 7| 7 8 9 ..
+ *
+ * BOUNDARY_OPTION_ZERO_PADDING : the extra pixels are simply set to 0
+ * This option makes it seem as if it were:
+ * .. 3 2 1| 0 0 0 ..
+ * .. 6 5 4| 0 0 0 ..
+ * .. 9 8 7| 0 0 0 ..
+ *
+ * Note that the input image is not actually extended in size. Instead, based on these options,
+ * the convolution is performed differently at the border pixels.
+ */
+ enum BOUNDARY_OPTIONS_ENUM
+ {
+ BOUNDARY_OPTION_CLAMP,
+ BOUNDARY_OPTION_MIRROR,
+ BOUNDARY_OPTION_ZERO_PADDING
+ };
+
+ Convolution ()
+ {
+ boundary_options_ = BOUNDARY_OPTION_CLAMP;
+ }
+
+ /** \brief Sets the kernel to be used for convolution
+ * \param[in] kernel convolution kernel passed by reference
+ */
+ inline void
+ setKernel (const pcl::PointCloud<PointT> &kernel)
+ {
+ kernel_ = kernel;
+ }
+
+ /**
+ * \param[in] boundary_options enum indicating the boundary options to be used for convolution
+ */
+ inline void
+ setBoundaryOptions (BOUNDARY_OPTIONS_ENUM boundary_options)
+ {
+ boundary_options_ = boundary_options;
+ }
+
+ /** \brief Performs 2D convolution of the input point cloud with the kernel.
+ * Uses clamp as the default boundary option.
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ filter (pcl::PointCloud<PointT> &output);
+
+ protected:
+ /** \brief This is an over-ride function for the pcl::Filter interface. */
+ void
+ applyFilter (pcl::PointCloud<PointT> &) {}
+
+ private:
+ BOUNDARY_OPTIONS_ENUM boundary_options_;
+ pcl::PointCloud<PointT> kernel_;
+ };
+}
+
+#include <pcl/2d/impl/convolution.hpp>
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXYZIEdge,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (float, magnitude, magnitude)
+ (float, direction, direction)
+ (float, magnitude_x, magnitude_x)
+ (float, magnitude_y, magnitude_y)
+)
+#endif // PCL_2D_CONVOLUTION_2D_H
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_2D_EDGE_H
+#define PCL_2D_EDGE_H
+
+#include <pcl/pcl_base.h>
+#include <pcl/2d/convolution.h>
+#include <pcl/2d/kernel.h>
+
+namespace pcl
+{
+ template <typename PointInT, typename PointOutT>
+ class Edge
+ {
+ private:
+ typedef typename pcl::PointCloud<PointInT> PointCloudIn;
+ typedef typename PointCloudIn::Ptr PointCloudInPtr;
+
+ PointCloudInPtr input_;
+ pcl::Convolution<PointInT> convolution_;
+ kernel<PointInT> kernel_;
+
+ /** \brief This function performs edge tracing for Canny Edge detector.
+ *
+ * \param[in] rowOffset row offset for direction in which the edge is to be traced
+ * \param[in] colOffset column offset for direction in which the edge is to be traced
+ * \param[in] row row location of the edge point
+ * \param[in] col column location of the edge point
+ * \param[out] maxima point cloud containing the edge information in the magnitude channel
+ */
+ inline void
+ cannyTraceEdge (int rowOffset, int colOffset, int row, int col,
+ pcl::PointCloud<pcl::PointXYZI> &maxima);
+
+ /** \brief This function discretizes the edge directions in steps of 22.5 degrees.
+ * \param thet point cloud containing the edge information in the direction channel
+ */
+ void
+ discretizeAngles (pcl::PointCloud<PointOutT> &thet);
+
+ /** \brief This function suppresses the edges which don't form a local maximum
+ * in the edge direction.
+ * \param[in] edges point cloud containing all the edges
+ * \param[out] maxima point cloud containing the non-max supressed edges
+ * \param[in] tLow
+ */
+ void
+ suppressNonMaxima (const pcl::PointCloud<PointXYZIEdge> &edges,
+ pcl::PointCloud<pcl::PointXYZI> &maxima, float tLow);
+
+ public:
+ typedef boost::shared_ptr<Edge> Ptr;
+ typedef boost::shared_ptr<const Edge> ConstPtr;
+
+ enum OUTPUT_TYPE
+ {
+ OUTPUT_Y,
+ OUTPUT_X,
+ OUTPUT_X_Y,
+ OUTPUT_MAGNITUDE,
+ OUTPUT_DIRECTION,
+ OUTPUT_MAGNITUDE_DIRECTION,
+ OUTPUT_ALL
+ };
+
+ enum DETECTOR_KERNEL_TYPE
+ {
+ CANNY,
+ SOBEL,
+ PREWITT,
+ ROBERTS,
+ LOG,
+ DERIVATIVE_CENTRAL,
+ DERIVATIVE_FORWARD,
+ DERIVATIVE_BACKWARD
+ };
+
+ private:
+ OUTPUT_TYPE output_type_;
+ DETECTOR_KERNEL_TYPE detector_kernel_type_;
+ bool non_maximal_suppression_;
+ bool hysteresis_thresholding_;
+
+ float hysteresis_threshold_low_;
+ float hysteresis_threshold_high_;
+ float non_max_suppression_radius_x_;
+ float non_max_suppression_radius_y_;
+
+ public:
+ Edge () :
+ output_type_ (OUTPUT_X),
+ detector_kernel_type_ (SOBEL),
+ non_maximal_suppression_ (false),
+ hysteresis_thresholding_ (false),
+ hysteresis_threshold_low_ (20),
+ hysteresis_threshold_high_ (80),
+ non_max_suppression_radius_x_ (3),
+ non_max_suppression_radius_y_ (3)
+ {
+ }
+
+ /** \brief Set the output type.
+ * \param[in] output_type the output type
+ */
+ void
+ setOutputType (OUTPUT_TYPE output_type)
+ {
+ output_type_ = output_type;
+ }
+
+ void
+ setHysteresisThresholdLow (float threshold)
+ {
+ hysteresis_threshold_low_ = threshold;
+ }
+
+ void
+ setHysteresisThresholdHigh (float threshold)
+ {
+ hysteresis_threshold_high_ = threshold;
+ }
+
+ /**
+ * \param[in] input_x
+ * \param[in] input_y
+ * \param[out] output
+ */
+ void
+ sobelMagnitudeDirection (const pcl::PointCloud<PointInT> &input_x,
+ const pcl::PointCloud<PointInT> &input_y,
+ pcl::PointCloud<PointOutT> &output);
+
+
+ /** \brief Perform Canny edge detection with two separated input images for
+ * horizontal and vertical derivatives.
+ * All edges of magnitude above t_high are always classified as edges. All edges
+ * below t_low are discarded. Edge values between t_low and t_high are classified
+ * as edges only if they are connected to edges having magnitude > t_high and are
+ * located in a direction perpendicular to that strong edge.
+ *
+ * \param[in] input_x Input point cloud passed by reference for the first derivative in the horizontal direction
+ * \param[in] input_y Input point cloud passed by reference for the first derivative in the vertical direction
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ canny (const pcl::PointCloud<PointInT> &input_x,
+ const pcl::PointCloud<PointInT> &input_y,
+ pcl::PointCloud<PointOutT> &output);
+
+ /** \brief This is a convenience function which performs edge detection based on
+ * the variable detector_kernel_type_
+ * \param[out] output
+ */
+ void
+ detectEdge (pcl::PointCloud<PointOutT> &output);
+
+ /** \brief All edges of magnitude above t_high are always classified as edges.
+ * All edges below t_low are discarded.
+ * Edge values between t_low and t_high are classified as edges only if they are
+ * connected to edges having magnitude > t_high and are located in a direction
+ * perpendicular to that strong edge.
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ detectEdgeCanny (pcl::PointCloud<PointOutT> &output);
+
+ /** \brief Uses the Sobel kernel for edge detection.
+ * This function does NOT include a smoothing step.
+ * The image should be smoothed before using this function to reduce noise.
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ detectEdgeSobel (pcl::PointCloud<PointOutT> &output);
+
+ /** \brief Uses the Prewitt kernel for edge detection.
+ * This function does NOT include a smoothing step.
+ * The image should be smoothed before using this function to reduce noise.
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ detectEdgePrewitt (pcl::PointCloud<PointOutT> &output);
+
+ /** \brief Uses the Roberts kernel for edge detection.
+ * This function does NOT include a smoothing step.
+ * The image should be smoothed before using this function to reduce noise.
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ detectEdgeRoberts (pcl::PointCloud<PointOutT> &output);
+
+ /** \brief Uses the LoG kernel for edge detection.
+ * Zero crossings of the Laplacian operator applied on an image indicate edges.
+ * Gaussian kernel is used to smoothen the image prior to the Laplacian.
+ * This is because Laplacian uses the second order derivative of the image and hence, is very sensitive to noise.
+ * The implementation is not two-step but rather applies the LoG kernel directly.
+ *
+ * \param[in] kernel_sigma variance of the LoG kernel used.
+ * \param[in] kernel_size a LoG kernel of dimensions kernel_size x kernel_size is used.
+ * \param[out] output Output point cloud passed by reference.
+ */
+ void
+ detectEdgeLoG (const float kernel_sigma, const float kernel_size,
+ pcl::PointCloud<PointOutT> &output);
+
+ /** \brief Computes the image derivatives in X direction using the kernel kernel::derivativeYCentralKernel.
+ * This function does NOT include a smoothing step.
+ * The image should be smoothed before using this function to reduce noise.
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ computeDerivativeXCentral (pcl::PointCloud<PointOutT> &output);
+
+ /** \brief Computes the image derivatives in Y direction using the kernel kernel::derivativeYCentralKernel.
+ * This function does NOT include a smoothing step.
+ * The image should be smoothed before using this function to reduce noise.
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ computeDerivativeYCentral (pcl::PointCloud<PointOutT> &output);
+
+ /** \brief Computes the image derivatives in X direction using the kernel kernel::derivativeYForwardKernel.
+ * This function does NOT include a smoothing step.
+ * The image should be smoothed before using this function to reduce noise.
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ computeDerivativeXForward (pcl::PointCloud<PointOutT> &output);
+
+ /** \brief Computes the image derivatives in Y direction using the kernel kernel::derivativeYForwardKernel.
+ * This function does NOT include a smoothing step.
+ * The image should be smoothed before using this function to reduce noise.
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ computeDerivativeYForward (pcl::PointCloud<PointOutT> &output);
+
+ /** \brief Computes the image derivatives in X direction using the kernel kernel::derivativeXBackwardKernel.
+ * This function does NOT include a smoothing step.
+ * The image should be smoothed before using this function to reduce noise.
+ * \param output Output point cloud passed by reference
+ */
+ void
+ computeDerivativeXBackward (pcl::PointCloud<PointOutT> &output);
+
+ /** \brief Computes the image derivatives in Y direction using the kernel kernel::derivativeYBackwardKernel.
+ * This function does NOT include a smoothing step.
+ * The image should be smoothed before using this function to reduce noise.
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ computeDerivativeYBackward (pcl::PointCloud<PointOutT> &output);
+
+ /** \brief Override function to implement the pcl::Filter interface
+ */
+ void
+ applyFilter (pcl::PointCloud<PointOutT>& /*output*/) {}
+
+ /** \brief Set the input point cloud pointer
+ * \param[in] input pointer to input point cloud
+ */
+ void
+ setInputCloud (PointCloudInPtr input)
+ {
+ input_ = input;
+ }
+ };
+}
+#include <pcl/2d/impl/edge.hpp>
+
+#endif // PCL_2D_EDGE_H
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_2D_CONVOLUTION_IMPL_HPP
+#define PCL_2D_CONVOLUTION_IMPL_HPP
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Convolution<PointT>::filter (pcl::PointCloud<PointT> &output)
+{
+ int input_row = 0;
+ int input_col = 0;
+ // default boundary option : zero padding
+ output = *input_;
+
+ int iw = static_cast<int> (input_->width),
+ ih = static_cast<int> (input_->height),
+ kw = static_cast<int> (kernel_.width),
+ kh = static_cast<int> (kernel_.height);
+ switch (boundary_options_)
+ {
+ default:
+ case BOUNDARY_OPTION_CLAMP:
+ {
+ for (int i = 0; i < ih; i++)
+ {
+ for (int j = 0; j < iw; j++)
+ {
+ float intensity = 0;
+ for (int k = 0; k < kh; k++)
+ {
+ for (int l = 0; l < kw; l++)
+ {
+ int ikkh = i + k - kh / 2, jlkw = j + l - kw / 2;
+ if (ikkh < 0)
+ input_row = 0;
+ else if (ikkh >= ih)
+ input_row = ih - 1;
+ else
+ input_row = ikkh;
+
+ if (jlkw < 0)
+ input_col = 0;
+ else if (jlkw >= iw)
+ input_col = iw - 1;
+ else
+ input_col = jlkw;
+
+ intensity += kernel_ (l, k).intensity * (*input_)(input_col, input_row).intensity;
+ }
+ }
+ output (j, i).intensity = intensity;
+ }
+ }
+ break;
+ }
+
+ case BOUNDARY_OPTION_MIRROR:
+ {
+ for (int i = 0; i < ih; i++)
+ {
+ for (int j = 0; j < iw; j++)
+ {
+ float intensity = 0;
+ for (int k = 0; k < kh; k++)
+ {
+ for (int l = 0; l < kw; l++)
+ {
+ int ikkh = i + k - kh / 2, jlkw = j + l - kw / 2;
+ if (ikkh < 0)
+ input_row = -ikkh - 1;
+ else if (ikkh >= ih)
+ input_row = 2 * ih - 1 - ikkh;
+ else
+ input_row = ikkh;
+
+ if (jlkw < 0)
+ input_col = -jlkw - 1;
+ else if (jlkw >= iw)
+ input_col = 2 * iw - 1 - jlkw;
+ else
+ input_col = jlkw;
+
+ intensity += kernel_ (l, k).intensity * ((*input_)(input_col, input_row).intensity);
+ }
+ }
+ output (j, i).intensity = intensity;
+ }
+ }
+ break;
+ }
+
+ case BOUNDARY_OPTION_ZERO_PADDING:
+ {
+ for (int i = 0; i < ih; i++)
+ {
+ for (int j = 0; j < iw; j++)
+ {
+ float intensity = 0;
+ for (int k = 0; k < kh; k++)
+ {
+ for (int l = 0; l < kw; l++)
+ {
+ int ikkh = i + k - kh / 2, jlkw = j + l - kw / 2;
+ if (ikkh < 0 || ikkh >= ih || jlkw < 0 || jlkw >= iw)
+ continue;
+ else
+ intensity += kernel_ (l, k).intensity * ((*input_)(jlkw, ikkh).intensity);
+ }
+ }
+ output (j, i).intensity = intensity;
+ }
+ }
+ break;
+ }
+ } // switch
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_2D_EDGE_IMPL_HPP
+#define PCL_2D_EDGE_IMPL_HPP
+
+#include <pcl/2d/convolution.h>
+#include <pcl/common/common_headers.h> // rad2deg()
+#include <pcl/console/time.h>
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::detectEdgeSobel (
+ pcl::PointCloud<PointOutT> &output)
+{
+ //pcl::console::TicToc tt;
+ //tt.tic ();
+ convolution_.setInputCloud (input_);
+ pcl::PointCloud<PointXYZI>::Ptr kernel_x (new pcl::PointCloud<PointXYZI>);
+ pcl::PointCloud<PointXYZI>::Ptr magnitude_x (new pcl::PointCloud<PointXYZI>);
+ kernel_.setKernelType (kernel<PointXYZI>::SOBEL_X);
+ kernel_.fetchKernel (*kernel_x);
+ convolution_.setKernel (*kernel_x);
+ convolution_.filter (*magnitude_x);
+ //PCL_ERROR ("Convolve X: %g\n", tt.toc ()); tt.tic ();
+
+ pcl::PointCloud<PointXYZI>::Ptr kernel_y (new pcl::PointCloud<PointXYZI>);
+ pcl::PointCloud<PointXYZI>::Ptr magnitude_y (new pcl::PointCloud<PointXYZI>);
+ kernel_.setKernelType (kernel<PointXYZI>::SOBEL_Y);
+ kernel_.fetchKernel (*kernel_y);
+ convolution_.setKernel (*kernel_y);
+ convolution_.filter (*magnitude_y);
+ //PCL_ERROR ("Convolve Y: %g\n", tt.toc ()); tt.tic ();
+
+ const int height = input_->height;
+ const int width = input_->width;
+
+ output.resize (height * width);
+ output.height = height;
+ output.width = width;
+
+ for (size_t i = 0; i < output.size (); ++i)
+ {
+ output[i].magnitude_x = (*magnitude_x)[i].intensity;
+ output[i].magnitude_y = (*magnitude_y)[i].intensity;
+ output[i].magnitude =
+ sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
+ (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
+ output[i].direction =
+ atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
+ }
+ //PCL_ERROR ("Rest: %g\n", tt.toc ());
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::sobelMagnitudeDirection (
+ const pcl::PointCloud<PointInT> &input_x,
+ const pcl::PointCloud<PointInT> &input_y,
+ pcl::PointCloud<PointOutT> &output)
+{
+ convolution_.setInputCloud (input_x.makeShared());
+ pcl::PointCloud<PointXYZI>::Ptr kernel_x (new pcl::PointCloud<PointXYZI>);
+ pcl::PointCloud<PointXYZI>::Ptr magnitude_x (new pcl::PointCloud<PointXYZI>);
+ kernel_.setKernelType (kernel<PointXYZI>::SOBEL_X);
+ kernel_.fetchKernel (*kernel_x);
+ convolution_.setKernel (*kernel_x);
+ convolution_.filter (*magnitude_x);
+
+ convolution_.setInputCloud (input_y.makeShared());
+ pcl::PointCloud<PointXYZI>::Ptr kernel_y (new pcl::PointCloud<PointXYZI>);
+ pcl::PointCloud<PointXYZI>::Ptr magnitude_y (new pcl::PointCloud<PointXYZI>);
+ kernel_.setKernelType (kernel<PointXYZI>::SOBEL_Y);
+ kernel_.fetchKernel (*kernel_y);
+ convolution_.setKernel (*kernel_y);
+ convolution_.filter (*magnitude_y);
+
+ const int height = input_x.height;
+ const int width = input_x.width;
+
+ output.resize (height * width);
+ output.height = height;
+ output.width = width;
+
+ for (size_t i = 0; i < output.size (); ++i)
+ {
+ output[i].magnitude_x = (*magnitude_x)[i].intensity;
+ output[i].magnitude_y = (*magnitude_y)[i].intensity;
+ output[i].magnitude =
+ sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
+ (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
+ output[i].direction =
+ atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::detectEdgePrewitt (pcl::PointCloud<PointOutT> &output)
+{
+ convolution_.setInputCloud (input_);
+
+ pcl::PointCloud<PointXYZI>::Ptr kernel_x (new pcl::PointCloud<PointXYZI>);
+ pcl::PointCloud<PointXYZI>::Ptr magnitude_x (new pcl::PointCloud<PointXYZI>);
+ kernel_.setKernelType (kernel<PointXYZI>::PREWITT_X);
+ kernel_.fetchKernel (*kernel_x);
+ convolution_.setKernel (*kernel_x);
+ convolution_.filter (*magnitude_x);
+
+ pcl::PointCloud<PointXYZI>::Ptr kernel_y (new pcl::PointCloud<PointXYZI>);
+ pcl::PointCloud<PointXYZI>::Ptr magnitude_y (new pcl::PointCloud<PointXYZI>);
+ kernel_.setKernelType (kernel<PointXYZI>::PREWITT_Y);
+ kernel_.fetchKernel (*kernel_y);
+ convolution_.setKernel (*kernel_y);
+ convolution_.filter (*magnitude_y);
+
+ const int height = input_->height;
+ const int width = input_->width;
+
+ output.resize (height * width);
+ output.height = height;
+ output.width = width;
+
+ for (size_t i = 0; i < output.size (); ++i)
+ {
+ output[i].magnitude_x = (*magnitude_x)[i].intensity;
+ output[i].magnitude_y = (*magnitude_y)[i].intensity;
+ output[i].magnitude =
+ sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
+ (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
+ output[i].direction =
+ atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::detectEdgeRoberts (pcl::PointCloud<PointOutT> &output)
+{
+ convolution_.setInputCloud (input_);
+
+ pcl::PointCloud<PointXYZI>::Ptr kernel_x (new pcl::PointCloud<PointXYZI>);
+ pcl::PointCloud<PointXYZI>::Ptr magnitude_x (new pcl::PointCloud<PointXYZI>);
+ kernel_.setKernelType (kernel<PointXYZI>::ROBERTS_X);
+ kernel_.fetchKernel (*kernel_x);
+ convolution_.setKernel (*kernel_x);
+ convolution_.filter (*magnitude_x);
+
+ pcl::PointCloud<PointXYZI>::Ptr kernel_y (new pcl::PointCloud<PointXYZI>);
+ pcl::PointCloud<PointXYZI>::Ptr magnitude_y (new pcl::PointCloud<PointXYZI>);
+ kernel_.setKernelType (kernel<PointXYZI>::ROBERTS_Y);
+ kernel_.fetchKernel (*kernel_y);
+ convolution_.setKernel (*kernel_y);
+ convolution_.filter (*magnitude_y);
+
+ const int height = input_->height;
+ const int width = input_->width;
+
+ output.resize (height * width);
+ output.height = height;
+ output.width = width;
+
+ for (size_t i = 0; i < output.size (); ++i)
+ {
+ output[i].magnitude_x = (*magnitude_x)[i].intensity;
+ output[i].magnitude_y = (*magnitude_y)[i].intensity;
+ output[i].magnitude =
+ sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
+ (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
+ output[i].direction =
+ atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::cannyTraceEdge (
+ int rowOffset, int colOffset, int row, int col,
+ pcl::PointCloud<PointXYZI> &maxima)
+{
+ int newRow = row + rowOffset;
+ int newCol = col + colOffset;
+ PointXYZI &pt = maxima (newCol, newRow);
+
+ if (newRow > 0 && newRow < static_cast<int> (maxima.height) && newCol > 0 && newCol < static_cast<int> (maxima.width))
+ {
+ if (pt.intensity == 0.0f || pt.intensity == std::numeric_limits<float>::max ())
+ return;
+
+ pt.intensity = std::numeric_limits<float>::max ();
+ cannyTraceEdge ( 1, 0, newRow, newCol, maxima);
+ cannyTraceEdge (-1, 0, newRow, newCol, maxima);
+ cannyTraceEdge ( 1, 1, newRow, newCol, maxima);
+ cannyTraceEdge (-1, -1, newRow, newCol, maxima);
+ cannyTraceEdge ( 0, -1, newRow, newCol, maxima);
+ cannyTraceEdge ( 0, 1, newRow, newCol, maxima);
+ cannyTraceEdge (-1, 1, newRow, newCol, maxima);
+ cannyTraceEdge ( 1, -1, newRow, newCol, maxima);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::discretizeAngles (pcl::PointCloud<PointOutT> &thet)
+{
+ const int height = thet.height;
+ const int width = thet.width;
+ float angle;
+ for (int i = 0; i < height; i++)
+ {
+ for (int j = 0; j < width; j++)
+ {
+ angle = pcl::rad2deg (thet (j, i).direction);
+ if (((angle <= 22.5) && (angle >= -22.5)) || (angle >= 157.5) || (angle <= -157.5))
+ thet (j, i).direction = 0;
+ else
+ if (((angle > 22.5) && (angle < 67.5)) || ((angle < -112.5) && (angle > -157.5)))
+ thet (j, i).direction = 45;
+ else
+ if (((angle >= 67.5) && (angle <= 112.5)) || ((angle <= -67.5) && (angle >= -112.5)))
+ thet (j, i).direction = 90;
+ else
+ if (((angle > 112.5) && (angle < 157.5)) || ((angle < -22.5) && (angle > -67.5)))
+ thet (j, i).direction = 135;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::suppressNonMaxima (
+ const pcl::PointCloud<PointXYZIEdge> &edges,
+ pcl::PointCloud<PointXYZI> &maxima, float tLow)
+{
+ const int height = edges.height;
+ const int width = edges.width;
+
+ maxima.height = height;
+ maxima.width = width;
+ maxima.resize (height * width);
+
+ for (size_t i = 0; i < maxima.size (); ++i)
+ maxima[i].intensity = 0.0f;
+
+ // tHigh and non-maximal supression
+ for (int i = 1; i < height - 1; i++)
+ {
+ for (int j = 1; j < width - 1; j++)
+ {
+ const PointXYZIEdge &ptedge = edges (j, i);
+ PointXYZI &ptmax = maxima (j, i);
+
+ if (ptedge.magnitude < tLow)
+ continue;
+
+ //maxima (j, i).intensity = 0;
+
+ switch (int (ptedge.direction))
+ {
+ case 0:
+ {
+ if (ptedge.magnitude >= edges (j - 1, i).magnitude &&
+ ptedge.magnitude >= edges (j + 1, i).magnitude)
+ ptmax.intensity = ptedge.magnitude;
+ break;
+ }
+ case 45:
+ {
+ if (ptedge.magnitude >= edges (j - 1, i - 1).magnitude &&
+ ptedge.magnitude >= edges (j + 1, i + 1).magnitude)
+ ptmax.intensity = ptedge.magnitude;
+ break;
+ }
+ case 90:
+ {
+ if (ptedge.magnitude >= edges (j, i - 1).magnitude &&
+ ptedge.magnitude >= edges (j, i + 1).magnitude)
+ ptmax.intensity = ptedge.magnitude;
+ break;
+ }
+ case 135:
+ {
+ if (ptedge.magnitude >= edges (j + 1, i - 1).magnitude &&
+ ptedge.magnitude >= edges (j - 1, i + 1).magnitude)
+ ptmax.intensity = ptedge.magnitude;
+ break;
+ }
+ }
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::detectEdgeCanny (pcl::PointCloud<PointOutT> &output)
+{
+ float tHigh = hysteresis_threshold_high_;
+ float tLow = hysteresis_threshold_low_;
+ const int height = input_->height;
+ const int width = input_->width;
+
+ output.resize (height * width);
+ output.height = height;
+ output.width = width;
+
+ //pcl::console::TicToc tt;
+ //tt.tic ();
+
+ // Noise reduction using gaussian blurring
+ pcl::PointCloud<PointXYZI>::Ptr gaussian_kernel (new pcl::PointCloud<PointXYZI>);
+ PointCloudInPtr smoothed_cloud (new PointCloudIn);
+ kernel_.setKernelSize (3);
+ kernel_.setKernelSigma (1.0);
+ kernel_.setKernelType (kernel<PointXYZI>::GAUSSIAN);
+ kernel_.fetchKernel (*gaussian_kernel);
+ convolution_.setKernel (*gaussian_kernel);
+ convolution_.setInputCloud (input_);
+ convolution_.filter (*smoothed_cloud);
+ //PCL_ERROR ("Gaussian blur: %g\n", tt.toc ()); tt.tic ();
+
+ // Edge detection usign Sobel
+ pcl::PointCloud<PointXYZIEdge>::Ptr edges (new pcl::PointCloud<PointXYZIEdge>);
+ setInputCloud (smoothed_cloud);
+ detectEdgeSobel (*edges);
+ //PCL_ERROR ("Sobel: %g\n", tt.toc ()); tt.tic ();
+
+ // Edge discretization
+ discretizeAngles (*edges);
+ //PCL_ERROR ("Discretize: %g\n", tt.toc ()); tt.tic ();
+
+ // tHigh and non-maximal supression
+ pcl::PointCloud<PointXYZI>::Ptr maxima (new pcl::PointCloud<PointXYZI>);
+ suppressNonMaxima (*edges, *maxima, tLow);
+ //PCL_ERROR ("NM suppress: %g\n", tt.toc ()); tt.tic ();
+
+ // Edge tracing
+ for (int i = 0; i < height; i++)
+ {
+ for (int j = 0; j < width; j++)
+ {
+ if ((*maxima)(j, i).intensity < tHigh || (*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
+ continue;
+
+ (*maxima)(j, i).intensity = std::numeric_limits<float>::max ();
+ cannyTraceEdge ( 1, 0, i, j, *maxima);
+ cannyTraceEdge (-1, 0, i, j, *maxima);
+ cannyTraceEdge ( 1, 1, i, j, *maxima);
+ cannyTraceEdge (-1, -1, i, j, *maxima);
+ cannyTraceEdge ( 0, -1, i, j, *maxima);
+ cannyTraceEdge ( 0, 1, i, j, *maxima);
+ cannyTraceEdge (-1, 1, i, j, *maxima);
+ cannyTraceEdge ( 1, -1, i, j, *maxima);
+ }
+ }
+ //PCL_ERROR ("Edge tracing: %g\n", tt.toc ());
+
+ // Final thresholding
+ for (size_t i = 0; i < input_->size (); ++i)
+ {
+ if ((*maxima)[i].intensity == std::numeric_limits<float>::max ())
+ output[i].magnitude = 255;
+ else
+ output[i].magnitude = 0;
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::canny (
+ const pcl::PointCloud<PointInT> &input_x,
+ const pcl::PointCloud<PointInT> &input_y,
+ pcl::PointCloud<PointOutT> &output)
+{
+ float tHigh = hysteresis_threshold_high_;
+ float tLow = hysteresis_threshold_low_;
+ const int height = input_x.height;
+ const int width = input_x.width;
+
+ output.resize (height * width);
+ output.height = height;
+ output.width = width;
+
+ // Noise reduction using gaussian blurring
+ pcl::PointCloud<PointXYZI>::Ptr gaussian_kernel (new pcl::PointCloud<PointXYZI>);
+ kernel_.setKernelSize (3);
+ kernel_.setKernelSigma (1.0);
+ kernel_.setKernelType (kernel<PointXYZI>::GAUSSIAN);
+ kernel_.fetchKernel (*gaussian_kernel);
+ convolution_.setKernel (*gaussian_kernel);
+
+ PointCloudIn smoothed_cloud_x;
+ convolution_.setInputCloud (input_x.makeShared());
+ convolution_.filter (smoothed_cloud_x);
+
+ PointCloudIn smoothed_cloud_y;
+ convolution_.setInputCloud (input_y.makeShared());
+ convolution_.filter (smoothed_cloud_y);
+
+
+ // Edge detection usign Sobel
+ pcl::PointCloud<PointXYZIEdge>::Ptr edges (new pcl::PointCloud<PointXYZIEdge>);
+ sobelMagnitudeDirection (smoothed_cloud_x, smoothed_cloud_y, *edges.get ());
+
+ // Edge discretization
+ discretizeAngles (*edges);
+
+ pcl::PointCloud<PointXYZI>::Ptr maxima (new pcl::PointCloud<PointXYZI>);
+ suppressNonMaxima (*edges, *maxima, tLow);
+
+ // Edge tracing
+ for (int i = 0; i < height; i++)
+ {
+ for (int j = 0; j < width; j++)
+ {
+ if ((*maxima)(j, i).intensity < tHigh || (*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
+ continue;
+
+ (*maxima)(j, i).intensity = std::numeric_limits<float>::max ();
+ cannyTraceEdge ( 1, 0, i, j, *maxima);
+ cannyTraceEdge (-1, 0, i, j, *maxima);
+ cannyTraceEdge ( 1, 1, i, j, *maxima);
+ cannyTraceEdge (-1, -1, i, j, *maxima);
+ cannyTraceEdge ( 0, -1, i, j, *maxima);
+ cannyTraceEdge ( 0, 1, i, j, *maxima);
+ cannyTraceEdge (-1, 1, i, j, *maxima);
+ cannyTraceEdge ( 1, -1, i, j, *maxima);
+ }
+ }
+
+ // Final thresholding
+ for (int i = 0; i < height; i++)
+ {
+ for (int j = 0; j < width; j++)
+ {
+ if ((*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
+ output (j, i).magnitude = 255;
+ else
+ output (j, i).magnitude = 0;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointInT, typename PointOutT> void
+pcl::Edge<PointInT, PointOutT>::detectEdgeLoG (
+ const float kernel_sigma, const float kernel_size,
+ pcl::PointCloud<PointOutT> &output)
+{
+ convolution_.setInputCloud (input_);
+
+ pcl::PointCloud<PointXYZI>::Ptr log_kernel (new pcl::PointCloud<PointXYZI>);
+ kernel_.setKernelType (kernel<PointXYZI>::LOG);
+ kernel_.setKernelSigma (kernel_sigma);
+ kernel_.setKernelSize (kernel_size);
+ kernel_.fetchKernel (*log_kernel);
+ convolution_.setKernel (*log_kernel);
+ convolution_.filter (output);
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_2D_KERNEL_IMPL_HPP
+#define PCL_2D_KERNEL_IMPL_HPP
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::fetchKernel (pcl::PointCloud<PointT> &kernel)
+{
+ switch (kernel_type_)
+ {
+ case SOBEL_X:
+ {
+ sobelKernelX (kernel);
+ break;
+ }
+ case SOBEL_Y:
+ {
+ sobelKernelY (kernel);
+ break;
+ }
+ case PREWITT_X:
+ {
+ prewittKernelX (kernel);
+ break;
+ }
+ case PREWITT_Y:
+ {
+ prewittKernelY (kernel);
+ break;
+ }
+ case ROBERTS_X:
+ {
+ robertsKernelX (kernel);
+ break;
+ }
+ case ROBERTS_Y:
+ {
+ robertsKernelY (kernel);
+ break;
+ }
+ case LOG:
+ {
+ loGKernel (kernel);
+ break;
+ }
+ case DERIVATIVE_CENTRAL_X:
+ {
+ derivativeXCentralKernel (kernel);
+ break;
+ }
+ case DERIVATIVE_FORWARD_X:
+ {
+ derivativeXForwardKernel (kernel);
+ break;
+ }
+ case DERIVATIVE_BACKWARD_X:
+ {
+ derivativeXBackwardKernel (kernel);
+ break;
+ }
+ case DERIVATIVE_CENTRAL_Y:
+ {
+ derivativeYCentralKernel (kernel);
+ break;
+ }
+ case DERIVATIVE_FORWARD_Y:
+ {
+ derivativeYForwardKernel (kernel);
+ break;
+ }
+ case DERIVATIVE_BACKWARD_Y:
+ {
+ derivativeYBackwardKernel (kernel);
+ break;
+ }
+ case GAUSSIAN:
+ {
+ gaussianKernel (kernel);
+ break;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::gaussianKernel (pcl::PointCloud<PointT> &kernel)
+{
+ float sum = 0;
+ kernel.resize (kernel_size_ * kernel_size_);
+ kernel.height = kernel_size_;
+ kernel.width = kernel_size_;
+
+ double sigma_sqr = 2 * sigma_ * sigma_;
+
+ for (int i = 0; i < kernel_size_; i++)
+ {
+ for (int j = 0; j < kernel_size_; j++)
+ {
+ int iks = (i - kernel_size_ / 2);
+ int jks = (j - kernel_size_ / 2);
+ kernel (j, i).intensity = expf (float (- double (iks * iks + jks * jks) / sigma_sqr));
+ sum += float (kernel (j, i).intensity);
+ }
+ }
+
+ // Normalizing the kernel
+ for (size_t i = 0; i < kernel.size (); ++i)
+ kernel[i].intensity /= sum;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointT> void
+pcl::kernel<PointT>::loGKernel (pcl::PointCloud<PointT> &kernel)
+{
+ float sum = 0;
+ float temp = 0;
+ kernel.resize (kernel_size_ * kernel_size_);
+ kernel.height = kernel_size_;
+ kernel.width = kernel_size_;
+
+ double sigma_sqr = 2 * sigma_ * sigma_;
+
+ for (int i = 0; i < kernel_size_; i++)
+ {
+ for (int j = 0; j < kernel_size_; j++)
+ {
+ int iks = (i - kernel_size_ / 2);
+ int jks = (j - kernel_size_ / 2);
+ temp = float (double (iks * iks + jks * jks) / sigma_sqr);
+ kernel (j, i).intensity = (1.0f - temp) * expf (-temp);
+ sum += kernel (j, i).intensity;
+ }
+ }
+
+ // Normalizing the kernel
+ for (size_t i = 0; i < kernel.size (); ++i)
+ kernel[i].intensity /= sum;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::sobelKernelX (pcl::PointCloud<PointT> &kernel)
+{
+ kernel.resize (9);
+ kernel.height = 3;
+ kernel.width = 3;
+ kernel (0, 0).intensity = -1; kernel (1, 0).intensity = 0; kernel (2, 0).intensity = 1;
+ kernel (0, 1).intensity = -2; kernel (1, 1).intensity = 0; kernel (2, 1).intensity = 2;
+ kernel (0, 2).intensity = -1; kernel (1, 2).intensity = 0; kernel (2, 2).intensity = 1;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::prewittKernelX (pcl::PointCloud<PointT> &kernel)
+{
+ kernel.resize (9);
+ kernel.height = 3;
+ kernel.width = 3;
+ kernel (0, 0).intensity = -1; kernel (1, 0).intensity = 0; kernel (2, 0).intensity = 1;
+ kernel (0, 1).intensity = -1; kernel (1, 1).intensity = 0; kernel (2, 1).intensity = 1;
+ kernel (0, 2).intensity = -1; kernel (1, 2).intensity = 0; kernel (2, 2).intensity = 1;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::robertsKernelX (pcl::PointCloud<PointT> &kernel)
+{
+ kernel.resize (4);
+ kernel.height = 2;
+ kernel.width = 2;
+ kernel (0, 0).intensity = 1; kernel (1, 0).intensity = 0;
+ kernel (0, 1).intensity = 0; kernel (1, 1).intensity = -1;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::sobelKernelY (pcl::PointCloud<PointT> &kernel)
+{
+ kernel.resize (9);
+ kernel.height = 3;
+ kernel.width = 3;
+ kernel (0, 0).intensity = -1; kernel (1, 0).intensity = -2; kernel (2, 0).intensity = -1;
+ kernel (0, 1).intensity = 0; kernel (1, 1).intensity = 0; kernel (2, 1).intensity = 0;
+ kernel (0, 2).intensity = 1; kernel (1, 2).intensity = 2; kernel (2, 2).intensity = 1;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::prewittKernelY (pcl::PointCloud<PointT> &kernel)
+{
+ kernel.resize (9);
+ kernel.height = 3;
+ kernel.width = 3;
+ kernel (0, 0).intensity = 1; kernel (1, 0).intensity = 1; kernel (2, 0).intensity = 1;
+ kernel (0, 1).intensity = 0; kernel (1, 1).intensity = 0; kernel (2, 1).intensity = 0;
+ kernel (0, 2).intensity = -1; kernel (1, 2).intensity = -1; kernel (2, 2).intensity = -1;
+}
+
+template <typename PointT> void
+pcl::kernel<PointT>::robertsKernelY (pcl::PointCloud<PointT> &kernel)
+{
+ kernel.resize (4);
+ kernel.height = 2;
+ kernel.width = 2;
+ kernel (0, 0).intensity = 0; kernel (1, 0).intensity = 1;
+ kernel (0, 1).intensity = -1; kernel (1, 1).intensity = 0;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::derivativeXCentralKernel (pcl::PointCloud<PointT> &kernel)
+{
+ kernel.resize (3);
+ kernel.height = 1;
+ kernel.width = 3;
+ kernel (0, 0).intensity = -1; kernel (1, 0).intensity = 0; kernel (2, 0).intensity = 1;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::derivativeXForwardKernel (pcl::PointCloud<PointT> &kernel)
+{
+ kernel.resize (3);
+ kernel.height = 1;
+ kernel.width = 3;
+ kernel (0, 0).intensity = 0; kernel (1, 0).intensity = -1; kernel (2, 0).intensity = 1;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::derivativeXBackwardKernel (pcl::PointCloud<PointT> &kernel)
+{
+ kernel.resize (3);
+ kernel.height = 1;
+ kernel.width = 3;
+ kernel (0, 0).intensity = -1; kernel (1, 0).intensity = 1; kernel (2, 0).intensity = 0;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::derivativeYCentralKernel (pcl::PointCloud<PointT> &kernel)
+{
+ kernel.resize (3);
+ kernel.height = 3;
+ kernel.width = 1;
+ kernel (0, 0).intensity = -1; kernel (0, 1).intensity = 0; kernel (0, 2).intensity = 1;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::derivativeYForwardKernel (pcl::PointCloud<PointT> &kernel)
+{
+ kernel.resize (3);
+ kernel.height = 3;
+ kernel.width = 1;
+ kernel (0, 0).intensity = 0; kernel (0, 1).intensity = -1; kernel (0, 2).intensity = 1;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::derivativeYBackwardKernel (pcl::PointCloud<PointT> &kernel)
+{
+ kernel.resize (3);
+ kernel.height = 3;
+ kernel.width = 1;
+ kernel (0, 0).intensity = -1; kernel (0, 1).intensity = 1; kernel (0, 2).intensity = 0;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::setKernelType (KERNEL_ENUM kernel_type)
+{
+ kernel_type_ = kernel_type;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::setKernelSize (int kernel_size)
+{
+ kernel_size_ = kernel_size;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::kernel<PointT>::setKernelSigma (float kernel_sigma)
+{
+ sigma_ = kernel_sigma;
+}
+
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * keypoint.hpp
+ *
+ * Created on: May 28, 2012
+ * Author: somani
+ */
+
+#ifndef PCL_2D_KEYPOINT_HPP_
+#define PCL_2D_KEYPOINT_HPP_
+
+#include <pcl/2d/edge.h>
+#include <pcl/2d/convolution.h>
+#include <limits>
+
+//////////////////////////////////////////////////////////////////////////////
+void
+pcl::keypoint::harrisCorner (ImageType &output, ImageType &input, const float sigma_d, const float sigma_i, const float alpha, const float thresh){
+
+ /*creating the gaussian kernels*/
+ ImageType kernel_d;
+ ImageType kernel_i;
+ conv_2d.gaussianKernel (5, sigma_d, kernel_d);
+ conv_2d.gaussianKernel (5, sigma_i, kernel_i);
+
+ /*scaling the image with differentiation scale*/
+ ImageType smoothed_image;
+ conv_2d.convolve (smoothed_image, kernel_d, input);
+
+ /*image derivatives*/
+ ImageType I_x, I_y;
+ edge_detection.ComputeDerivativeXCentral (I_x, smoothed_image);
+ edge_detection.ComputeDerivativeYCentral (I_y, smoothed_image);
+
+ /*second moment matrix*/
+ ImageType I_x2, I_y2, I_xI_y;
+ imageElementMultiply (I_x2, I_x, I_x);
+ imageElementMultiply (I_y2, I_y, I_y);
+ imageElementMultiply (I_xI_y, I_x, I_y);
+
+ /*scaling second moment matrix with integration scale*/
+ ImageType M00, M10, M11;
+ conv_2d.convolve (M00, kernel_i, I_x2);
+ conv_2d.convolve (M10, kernel_i, I_xI_y);
+ conv_2d.convolve (M11, kernel_i, I_y2);
+
+ /*harris function*/
+ const size_t height = input.size ();
+ const size_t width = input[0].size ();
+ output.resize (height);
+ for (size_t i = 0; i < height; i++)
+ {
+ output[i].resize (width);
+ for (size_t j = 0; j < width; j++)
+ {
+ output[i][j] = M00[i][j] * M11[i][j] - (M10[i][j] * M10[i][j]) - alpha * ((M00[i][j] + M11[i][j]) * (M00[i][j] + M11[i][j]));
+ if (thresh != 0)
+ {
+ if (output[i][j] < thresh)
+ output[i][j] = 0;
+ else
+ output[i][j] = 255;
+ }
+ }
+ }
+
+ /*local maxima*/
+ for (size_t i = 1; i < height - 1; i++)
+ {
+ for (size_t j = 1; j < width - 1; j++)
+ {
+ if (output[i][j] > output[i - 1][j - 1] && output[i][j] > output[i - 1][j] && output[i][j] > output[i - 1][j + 1] &&
+ output[i][j] > output[i][j - 1] && output[i][j] > output[i][j + 1] &&
+ output[i][j] > output[i + 1][j - 1] && output[i][j] > output[i + 1][j] && output[i][j] > output[i + 1][j + 1])
+ ;
+ else
+ output[i][j] = 0;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+void
+pcl::keypoint::hessianBlob (ImageType &output, ImageType &input, const float sigma, bool SCALED){
+ /*creating the gaussian kernels*/
+ ImageType kernel, cornerness;
+ conv_2d.gaussianKernel (5, sigma, kernel);
+
+ /*scaling the image with differentiation scale*/
+ ImageType smoothed_image;
+ conv_2d.convolve (smoothed_image, kernel, input);
+
+ /*image derivatives*/
+ ImageType I_x, I_y;
+ edge_detection.ComputeDerivativeXCentral (I_x, smoothed_image);
+ edge_detection.ComputeDerivativeYCentral (I_y, smoothed_image);
+
+ /*second moment matrix*/
+ ImageType I_xx, I_yy, I_xy;
+ edge_detection.ComputeDerivativeXCentral (I_xx, I_x);
+ edge_detection.ComputeDerivativeYCentral (I_xy, I_x);
+ edge_detection.ComputeDerivativeYCentral (I_yy, I_y);
+ /*Determinant of Hessian*/
+ const size_t height = input.size ();
+ const size_t width = input[0].size ();
+ float min = std::numeric_limits<float>::max();
+ float max = std::numeric_limits<float>::min();
+ cornerness.resize (height);
+ for (size_t i = 0; i < height; i++)
+ {
+ cornerness[i].resize (width);
+ for (size_t j = 0; j < width; j++)
+ {
+ cornerness[i][j] = sigma*sigma*(I_xx[i][j]+I_yy[i][j]-I_xy[i][j]*I_xy[i][j]);
+ if(SCALED){
+ if(cornerness[i][j] < min)
+ min = cornerness[i][j];
+ if(cornerness[i][j] > max)
+ max = cornerness[i][j];
+ }
+ }
+
+ /*local maxima*/
+ output.resize (height);
+ output[0].resize (width);
+ output[height-1].resize (width);
+ for (size_t i = 1; i < height - 1; i++)
+ {
+ output[i].resize (width);
+ for (size_t j = 1; j < width - 1; j++)
+ {
+ if(SCALED)
+ output[i][j] = ((cornerness[i][j]-min)/(max-min));
+ else
+ output[i][j] = cornerness[i][j];
+ }
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+void
+pcl::keypoint::hessianBlob (ImageType &output, ImageType &input, const float start_scale, const float scaling_factor, const int num_scales){
+ const size_t height = input.size();
+ const size_t width = input[0].size();
+ const int local_search_radius = 1;
+ float scale = start_scale;
+ std::vector<ImageType> cornerness;
+ cornerness.resize(num_scales);
+ for(int i = 0;i < num_scales;i++){
+ hessianBlob(cornerness[i], input, scale, false);
+ scale *= scaling_factor;
+ }
+ bool non_max_flag = false;
+ float scale_max, local_max;
+ for(size_t i = 0;i < height;i++){
+ for(size_t j = 0;j < width;j++){
+ scale_max = std::numeric_limits<float>::min();
+ /*default output in case of no blob at the current point is 0*/
+ output[i][j] = 0;
+ for(int k = 0;k < num_scales;k++){
+ /*check if the current point (k,i,j) is a maximum in the defined search radius*/
+ non_max_flag = false;
+ local_max = cornerness[k][i][j];
+ for(int n = -local_search_radius; n <= local_search_radius;n++){
+ if(n+k < 0 || n+k >= num_scales)
+ continue;
+ for(int l = -local_search_radius;l <= local_search_radius;l++){
+ if(l+i < 0 || l+i >= height)
+ continue;
+ for(int m = -local_search_radius; m <= local_search_radius;m++){
+ if(m+j < 0 || m+j >= width)
+ continue;
+ if(cornerness[n+k][l+i][m+j] > local_max){
+ non_max_flag = true;
+ break;
+ }
+ }
+ if(non_max_flag)
+ break;
+ }
+ if(non_max_flag)
+ break;
+ }
+ /*if the current point is a point of local maximum, check if it is a maximum point across scales*/
+ if(!non_max_flag){
+ if(cornerness[k][i][j] > scale_max){
+ scale_max = cornerness[k][i][j];
+ /*output indicates the scale at which the blob is found at the current location in the image*/
+ output[i][i] = start_scale*pow(scaling_factor, k);
+ }
+ }
+ }
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+void
+pcl::keypoint::imageElementMultiply (ImageType &output, ImageType &input1, ImageType &input2){
+ const size_t height = input1.size ();
+ const size_t width = input1[0].size ();
+ output.resize (height);
+ for (size_t i = 0; i < height; i++)
+ {
+ output[i].resize (width);
+ for (size_t j = 0; j < width; j++)
+ {
+ output[i][j] = input1[i][j] * input2[i][j];
+ }
+ }
+}
+
+#endif // PCL_2D_KEYPOINT_HPP_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_2D_MORPHOLOGY_HPP_
+#define PCL_2D_MORPHOLOGY_HPP_
+
+//////////////////////////////////////////////////////////////////////////////
+// Assumes input, kernel and output images have 0's and 1's only
+template<typename PointT> void
+pcl::Morphology<PointT>::erosionBinary (pcl::PointCloud<PointT> &output)
+{
+ const int height = input_->height;
+ const int width = input_->width;
+ const int kernel_height = structuring_element_->height;
+ const int kernel_width = structuring_element_->width;
+ bool mismatch_flag;
+
+ output.width = width;
+ output.height = height;
+ output.resize (width * height);
+
+ for (int i = 0; i < height; i++)
+ {
+ for (int j = 0; j < width; j++)
+ {
+ // Operation done only at 1's
+ if ((*input_)(j, i).intensity == 0)
+ {
+ output (j, i).intensity = 0;
+ continue;
+ }
+ mismatch_flag = false;
+ for (int k = 0; k < kernel_height; k++)
+ {
+ if (mismatch_flag)
+ break;
+ for (int l = 0; l < kernel_width; l++)
+ {
+ // We only check for 1's in the kernel
+ if ((*structuring_element_)(l, k).intensity == 0)
+ continue;
+ if ((i + k - kernel_height / 2) < 0 || (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || (j + l - kernel_width / 2) >= width)
+ {
+ continue;
+ }
+ // If one of the elements of the kernel and image dont match,
+ // the output image is 0. So, move to the next point.
+ if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity != 1)
+ {
+ output (j, i).intensity = 0;
+ mismatch_flag = true;
+ break;
+ }
+ }
+ }
+ // Assign value according to mismatch flag
+ output (j, i).intensity = (mismatch_flag) ? 0 : 1;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Assumes input, kernel and output images have 0's and 1's only
+template <typename PointT> void
+pcl::Morphology<PointT>::dilationBinary (pcl::PointCloud<PointT> &output)
+{
+ const int height = input_->height;
+ const int width = input_->width;
+ const int kernel_height = structuring_element_->height;
+ const int kernel_width = structuring_element_->width;
+ bool match_flag;
+
+ output.width = width;
+ output.height = height;
+ output.resize (width * height);
+
+ for (int i = 0; i < height; i++)
+ {
+ for (int j = 0; j < width; j++)
+ {
+ match_flag = false;
+ for (int k = 0; k < kernel_height; k++)
+ {
+ if (match_flag)
+ break;
+ for (int l = 0; l < kernel_width; l++)
+ {
+ // We only check for 1's in the kernel
+ if ((*structuring_element_)(l, k).intensity == 0)
+ continue;
+ if ((i + k - kernel_height / 2) < 0 || (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || (j + l - kernel_width / 2) >= height)
+ {
+ continue;
+ }
+ // If any position where kernel is 1 and image is also one is detected,
+ // matching occurs
+ if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity == 1)
+ {
+ match_flag = true;
+ break;
+ }
+ }
+ }
+ // Assign value according to match flag
+ output (j, i).intensity = (match_flag) ? 1 : 0;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Assumes input, kernel and output images have 0's and 1's only
+template <typename PointT> void
+pcl::Morphology<PointT>::openingBinary (pcl::PointCloud<PointT> &output)
+{
+ PointCloudInPtr intermediate_output (new PointCloudIn);
+ erosionBinary (*intermediate_output);
+ this->setInputCloud (intermediate_output);
+ dilationBinary (output);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Assumes input, kernel and output images have 0's and 1's only
+template <typename PointT> void
+pcl::Morphology<PointT>::closingBinary (pcl::PointCloud<PointT> &output)
+{
+ PointCloudInPtr intermediate_output (new PointCloudIn);
+ dilationBinary (*intermediate_output);
+ this->setInputCloud (intermediate_output);
+ erosionBinary (output);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::erosionGray (pcl::PointCloud<PointT> &output)
+{
+ const int height = input_->height;
+ const int width = input_->width;
+ const int kernel_height = structuring_element_->height;
+ const int kernel_width = structuring_element_->width;
+ float min;
+ output.resize (width * height);
+ output.width = width;
+ output.height = height;
+
+ for (int i = 0; i < height; i++)
+ {
+ for (int j = 0; j < width; j++)
+ {
+ min = -1;
+ for (int k = 0; k < kernel_height; k++)
+ {
+ for (int l = 0; l < kernel_width; l++)
+ {
+ // We only check for 1's in the kernel
+ if ((*structuring_element_)(l, k).intensity == 0)
+ continue;
+ if ((i + k - kernel_height / 2) < 0 || (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || (j + l - kernel_width / 2) >= width)
+ {
+ continue;
+ }
+ // If one of the elements of the kernel and image dont match,
+ // the output image is 0. So, move to the next point.
+ if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity < min || min == -1)
+ {
+ min = (*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity;
+ }
+ }
+ }
+ // Assign value according to mismatch flag
+ output (j, i).intensity = min;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::dilationGray (pcl::PointCloud<PointT> &output)
+{
+ const int height = input_->height;
+ const int width = input_->width;
+ const int kernel_height = structuring_element_->height;
+ const int kernel_width = structuring_element_->width;
+ float max;
+
+ output.resize (width * height);
+ output.width = width;
+ output.height = height;
+
+ for (int i = 0; i < height; i++)
+ {
+ for (int j = 0; j < width; j++)
+ {
+ max = -1;
+ for (int k = 0; k < kernel_height; k++)
+ {
+ for (int l = 0; l < kernel_width; l++)
+ {
+ // We only check for 1's in the kernel
+ if ((*structuring_element_)(l, k).intensity == 0)
+ continue;
+ if ((i + k - kernel_height / 2) < 0 || (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || (j + l - kernel_width / 2) >= width)
+ {
+ continue;
+ }
+ // If one of the elements of the kernel and image dont match,
+ // the output image is 0. So, move to the next point.
+ if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity > max || max == -1)
+ {
+ max = (*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity;
+ }
+ }
+ }
+ // Assign value according to mismatch flag
+ output (j, i).intensity = max;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::openingGray (pcl::PointCloud<PointT> &output)
+{
+ PointCloudInPtr intermediate_output (new PointCloudIn);
+ erosionGray (*intermediate_output);
+ this->setInputCloud (intermediate_output);
+ dilationGray (output);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::closingGray (pcl::PointCloud<PointT> &output)
+{
+ PointCloudInPtr intermediate_output (new PointCloudIn);
+ dilationGray (*intermediate_output);
+ this->setInputCloud (intermediate_output);
+ erosionGray (output);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::subtractionBinary (
+ pcl::PointCloud<PointT> &output,
+ const pcl::PointCloud<PointT> &input1,
+ const pcl::PointCloud<PointT> &input2)
+{
+ const int height = (input1.height < input2.hieght) ? input1.height : input2.height;
+ const int width = (input1.width < input2.width) ? input1.width : input2.width;
+ output.width = width;
+ output.height = height;
+ output.resize (height * width);
+
+ for (size_t i = 0; i < output.size (); ++i)
+ {
+ if (input1[i].intensity == 1 && input2[i].intensity == 0)
+ output[i].intensity = 1;
+ else
+ output[i].intensity = 0;
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::unionBinary (
+ pcl::PointCloud<PointT> &output,
+ const pcl::PointCloud<PointT> &input1,
+ const pcl::PointCloud<PointT> &input2)
+{
+ const int height = (input1.height < input2.hieght) ? input1.height : input2.height;
+ const int width = (input1.width < input2.width) ? input1.width : input2.width;
+ output.width = width;
+ output.height = height;
+ output.resize (height * width);
+
+ for (size_t i = 0; i < output.size (); ++i)
+ {
+ if (input1[i].intensity == 1 || input2[i].intensity == 1)
+ output[i].intensity = 1;
+ else
+ output[i].intensity = 0;
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::intersectionBinary (
+ pcl::PointCloud<PointT> &output,
+ const pcl::PointCloud<PointT> &input1,
+ const pcl::PointCloud<PointT> &input2)
+{
+ const int height = (input1.height < input2.height) ? input1.height : input2.height;
+ const int width = (input1.width < input2.width) ? input1.width : input2.width;
+ output.width = width;
+ output.height = height;
+ output.resize (height * width);
+
+ for (size_t i = 0; i < output.size (); ++i)
+ {
+ if (input1[i].intensity == 1 && input2[i].intensity == 1)
+ output[i].intensity = 1;
+ else
+ output[i].intensity = 0;
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::structuringElementCircular (
+ pcl::PointCloud<PointT> &kernel, const int radius)
+{
+ const int dim = 2 * radius;
+ kernel.height = dim;
+ kernel.width = dim;
+ kernel.resize (dim * dim);
+
+ for (int i = 0; i < dim; i++)
+ {
+ for (int j = 0; j < dim; j++)
+ {
+ if (((i - radius) * (i - radius) + (j - radius) * (j - radius)) < radius * radius)
+ kernel (j, i).intensity = 1;
+ else
+ kernel (j, i).intensity = 0;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::structuringElementRectangle (
+ pcl::PointCloud<PointT> &kernel, const int height, const int width)
+{
+ kernel.height = height;
+ kernel.width = width;
+ kernel.resize (height * width);
+ for (size_t i = 0; i < kernel.size (); ++i)
+ kernel[i].intensity = 1;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Morphology<PointT>::setStructuringElement (const PointCloudInPtr &structuring_element)
+{
+ structuring_element_ = structuring_element;
+}
+
+#endif // PCL_2D_MORPHOLOGY_HPP_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_2D_KERNEL_H_
+#define PCL_2D_KERNEL_H_
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+namespace pcl
+{
+ template<typename PointT>
+ class kernel
+ {
+ public:
+
+ /**
+ * enumerates the different types of kernels available.
+ */
+ enum KERNEL_ENUM
+ {
+ SOBEL_X, //!< SOBEL_X
+ SOBEL_Y, //!< SOBEL_Y
+ PREWITT_X, //!< PREWITT_X
+ PREWITT_Y, //!< PREWITT_Y
+ ROBERTS_X, //!< ROBERTS_X
+ ROBERTS_Y, //!< ROBERTS_Y
+ LOG, //!< LOG
+ DERIVATIVE_CENTRAL_X, //!< DERIVATIVE_CENTRAL_X
+ DERIVATIVE_FORWARD_X, //!< DERIVATIVE_FORWARD_X
+ DERIVATIVE_BACKWARD_X,//!< DERIVATIVE_BACKWARD_X
+ DERIVATIVE_CENTRAL_Y, //!< DERIVATIVE_CENTRAL_Y
+ DERIVATIVE_FORWARD_Y, //!< DERIVATIVE_FORWARD_Y
+ DERIVATIVE_BACKWARD_Y,//!< DERIVATIVE_BACKWARD_Y
+ GAUSSIAN //!< GAUSSIAN
+ };
+
+ int kernel_size_;
+ float sigma_;
+ KERNEL_ENUM kernel_type_;
+
+ kernel () :
+ kernel_size_ (3),
+ sigma_ (1.0),
+ kernel_type_ (GAUSSIAN)
+ {
+
+ }
+
+ /**
+ *
+ * @param kernel Kernel point cloud passed by reference
+ *
+ * Helper function which returns the kernel selected by the kernel_type_ enum
+ */
+ void fetchKernel (pcl::PointCloud<PointT> &kernel);
+
+ /**
+ *
+ * @param kernel Kernel point cloud passed by reference
+ *
+ * Gaussian kernel with size (kernel_size_ x kernel_size_) and variance sigma_
+ */
+
+ void gaussianKernel (pcl::PointCloud<PointT> &kernel);
+
+ /**
+ *
+ * @param kernel Kernel point cloud passed by reference
+ *
+ * Laplacian of Gaussian kernel with size (kernel_size_ x kernel_size_) and variance sigma_
+ */
+
+ void loGKernel (pcl::PointCloud<PointT> &kernel);
+
+ /**
+ *
+ * @param kernel Kernel point cloud passed by reference
+ *
+ * 3x3 Sobel kernel in the X direction
+ */
+
+ void sobelKernelX (pcl::PointCloud<PointT> &kernel);
+
+ /**
+ *
+ * @param kernel Kernel point cloud passed by reference
+ *
+ * 3x3 Prewitt kernel in the X direction
+ */
+
+ void prewittKernelX (pcl::PointCloud<PointT> &kernel);
+
+ /**
+ *
+ * @param kernel Kernel point cloud passed by reference
+ *
+ * 2x2 Roberts kernel in the X direction
+ */
+
+ void robertsKernelX (pcl::PointCloud<PointT> &kernel);
+
+ /**
+ *
+ * @param kernel Kernel point cloud passed by reference
+ *
+ * 3x3 Sobel kernel in the Y direction
+ */
+
+ void sobelKernelY (pcl::PointCloud<PointT> &kernel);
+
+ /**
+ *
+ * @param kernel Kernel point cloud passed by reference
+ *
+ * 3x3 Prewitt kernel in the Y direction
+ */
+
+ void prewittKernelY (pcl::PointCloud<PointT> &kernel);
+
+ /**
+ *
+ * @param kernel Kernel point cloud passed by reference
+ *
+ * 2x2 Roberts kernel in the Y direction
+ */
+
+ void robertsKernelY (pcl::PointCloud<PointT> &kernel);
+
+ /**
+ *
+ * @param kernel Kernel point cloud passed by reference
+ *
+ * kernel [-1 0 1]
+ */
+
+ void derivativeXCentralKernel (pcl::PointCloud<PointT> &kernel);
+
+ /**
+ *
+ * @param kernel Kernel point cloud passed by reference
+ *
+ * kernel [-1 0 1]'
+ */
+
+ void derivativeYCentralKernel (pcl::PointCloud<PointT> &kernel);
+
+ /**
+ *
+ * @param kernel Kernel point cloud passed by reference
+ *
+ * kernel [0 -1 1]
+ */
+
+ void derivativeXForwardKernel (pcl::PointCloud<PointT> &kernel);
+
+ /**
+ *
+ * @param kernel Kernel point cloud passed by reference
+ *
+ * kernel [0 -1 1]'
+ */
+
+ void derivativeYForwardKernel (pcl::PointCloud<PointT> &kernel);
+
+ /**
+ *
+ * @param kernel Kernel point cloud passed by reference
+ *
+ * kernel [-1 1 0]
+ */
+
+ void derivativeXBackwardKernel (pcl::PointCloud<PointT> &kernel);
+
+ /**
+ *
+ * @param kernel Kernel point cloud passed by reference
+ *
+ * kernel [-1 1 0]'
+ */
+
+ void derivativeYBackwardKernel (PointCloud<PointT> &kernel);
+
+ /**
+ *
+ * @param kernel_type enum indicating the kernel type wanted
+ *
+ * select the kernel type.
+ */
+ void setKernelType (KERNEL_ENUM kernel_type);
+
+ /**
+ *
+ * @param kernel_size kernel of size kernel_size x kernel_size is created(LoG and Gaussian only)
+ *
+ * Setter function for kernel_size_
+ */
+ void setKernelSize (int kernel_size);
+
+ /**
+ *
+ * @param kernel_sigma variance of the Gaussian or LoG kernels.
+ *
+ * Setter function for kernel_sigma_
+ */
+ void setKernelSigma (float kernel_sigma);
+ };
+}
+
+#include <pcl/2d/impl/kernel.hpp>
+
+#endif // PCL_2D_KERNEL_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * keypoint.h
+ *
+ * Created on: May 28, 2012
+ * Author: somani
+ */
+
+#ifndef PCL_2D_KEYPOINT_H_
+#define PCL_2D_KEYPOINT_H_
+
+#include <pcl/2d/edge.h>
+
+namespace pcl
+{
+ class Keypoint
+ {
+ private:
+ Edge edge_detection;
+ Convolution conv_2d;
+ public:
+ Keypoint ()
+ {
+ }
+
+ void
+ harrisCorner (ImageType &output, ImageType &input, const float sigma_d, const float sigma_i, const float alpha, const float thresh);
+
+ void
+ hessianBlob (ImageType &output, ImageType &input, const float sigma, bool SCALE);
+
+ void
+ hessianBlob (ImageType &output, ImageType &input, const float start_scale, const float scaling_factor, const int num_scales);
+
+ void
+ imageElementMultiply (ImageType &output, ImageType &input1, ImageType &input2);
+ };
+}
+
+#include <pcl/2d/impl/keypoint.hpp>
+
+#endif // PCL_2D_KEYPOINT_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_2D_MORPHOLOGY_H_
+#define PCL_2D_MORPHOLOGY_H_
+
+#include <pcl/pcl_base.h>
+
+namespace pcl
+{
+ template <typename PointT>
+ class Morphology : public PCLBase<PointT>
+ {
+ private:
+ typedef typename pcl::PointCloud<PointT> PointCloudIn;
+ typedef typename PointCloudIn::Ptr PointCloudInPtr;
+
+ PointCloudInPtr structuring_element_;
+
+ public:
+ using PCLBase<PointT>::input_;
+
+ Morphology () {}
+
+ /** \brief This function performs erosion followed by dilation.
+ * It is useful for removing noise in the form of small blobs and patches.
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ openingBinary (pcl::PointCloud<PointT> &output);
+
+ /** \brief This function performs dilation followed by erosion.
+ * It is useful for filling up (holes/cracks/small discontinuities) in a binary
+ * segmented region
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ closingBinary (pcl::PointCloud<PointT> &output);
+
+ /** \brief Binary dilation is similar to a logical disjunction of sets.
+ * At each pixel having value 1, if for all pixels in the structuring element having value 1, the corresponding
+ * pixels in the input image are also 1, the center pixel is set to 1. Otherwise, it is set to 0.
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ erosionBinary (pcl::PointCloud<PointT> &output);
+
+ /** \brief Binary erosion is similar to a logical addition of sets.
+ * At each pixel having value 1, if at least one pixel in the structuring element is 1 and the corresponding point
+ * in the input image is 1, the center pixel is set to 1. Otherwise, it is set to 0.
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ dilationBinary (pcl::PointCloud<PointT> &output);
+
+ /** \brief Grayscale erosion followed by dilation.
+ * This is used to remove small bright artifacts from the image. Large bright objects are relatively undisturbed.
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ openingGray (pcl::PointCloud<PointT> &output);
+
+ /** \brief Grayscale dilation followed by erosion.
+ * This is used to remove small dark artifacts from the image. Bright features or large dark features are relatively undisturbed.
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ closingGray (pcl::PointCloud<PointT> &output);
+
+ /** \brief Takes the min of the pixels where kernel is 1
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ erosionGray (pcl::PointCloud<PointT> &output);
+
+ /** \brief Takes the max of the pixels where kernel is 1
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ dilationGray (pcl::PointCloud<PointT> &output);
+
+ /** \brief Set operation
+ * output = input1 - input2
+ * \param[out] output Output point cloud passed by reference
+ * \param[in] input1
+ * \param[in] input2
+ */
+ void
+ subtractionBinary (pcl::PointCloud<PointT> &output,
+ const pcl::PointCloud<PointT> &input1,
+ const pcl::PointCloud<PointT> &input2);
+
+ /** \brief Set operation
+ * \f$output = input1 \cup input2\f$
+ * \param[out] output Output point cloud passed by reference
+ * \param[in] input1
+ * \param[in] input2
+ */
+ void
+ unionBinary (pcl::PointCloud<PointT> &output,
+ const pcl::PointCloud<PointT> &input1,
+ const pcl::PointCloud<PointT> &input2);
+
+ /** \brief Set operation \f$ output = input1 \cap input2 \f$
+ * \param[out] output Output point cloud passed by reference
+ * \param[in] input1
+ * \param[in] input2
+ */
+ void
+ intersectionBinary (pcl::PointCloud<PointT> &output,
+ const pcl::PointCloud<PointT> &input1,
+ const pcl::PointCloud<PointT> &input2);
+
+ /** \brief Creates a circular structing element. The size of the kernel created is 2*radius x 2*radius.
+ * Center of the structuring element is the center of the circle.
+ * All values lying on the circle are 1 and the others are 0.
+ *
+ * \param[out] kernel structuring element kernel passed by reference
+ * \param[in] radius Radius of the circular structuring element.
+ */
+ void
+ structuringElementCircular (pcl::PointCloud<PointT> &kernel, const int radius);
+
+ /** \brief Creates a rectangular structing element of size height x width. *
+ * All values are 1.
+ *
+ * \param[out] kernel structuring element kernel passed by reference
+ * \param[in] height height number of rows in the structuring element
+ * \param[in] width number of columns in the structuring element
+ *
+ */
+ void
+ structuringElementRectangle (pcl::PointCloud<PointT> &kernel,
+ const int height, const int width);
+
+ enum MORPHOLOGICAL_OPERATOR_TYPE
+ {
+ EROSION_GRAY,
+ DILATION_GRAY,
+ OPENING_GRAY,
+ CLOSING_GRAY,
+ EROSION_BINARY,
+ DILATION_BINARY,
+ OPENING_BINARY,
+ CLOSING_BINARY
+ };
+
+ MORPHOLOGICAL_OPERATOR_TYPE operator_type;
+
+ /**
+ * \param[out] output Output point cloud passed by reference
+ */
+ void
+ applyMorphologicalOperation (pcl::PointCloud<PointT> &output);
+
+ /**
+ * \param[in] structuring_element The structuring element to be used for the morphological operation
+ */
+ void
+ setStructuringElement (const PointCloudInPtr &structuring_element);
+ };
+}
+
+#include <pcl/2d/impl/morphology.hpp>
+
+#endif // PCL_2D_MORPHOLOGY_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+// Instantiations of specific point types
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ */
+
+#include <pcl/point_types.h>
+
+#include <pcl/2d/Convolution.h>
+#include <pcl/2d/Edge.h>
+#include <pcl/2d/Kernel.h>
+#include <pcl/2d/Morphology.h>
+#include <pcl/pcl_base.h>
+
+using namespace pcl;
+
+void example_edge ()
+{
+ Edge<pcl::PointXYZRGB> edge;
+
+ /*dummy clouds*/
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+
+ /*example 1*/
+ edge.output_type_ = Edge<pcl::PointXYZRGB>::OUTPUT_X_Y;
+ edge.detectEdgeRoberts (*output_cloud, *input_cloud);
+
+ /*example 2*/
+ edge.hysteresis_threshold_low_ = 20;
+ edge.hysteresis_threshold_high_ = 80;
+ edge.non_max_suppression_radius_x_ = 3;
+ edge.non_max_suppression_radius_y_ = 3;
+ edge.detectEdgeCanny (*output_cloud, *input_cloud);
+
+ /*example 3*/
+ edge.detector_kernel_type_ = Edge<pcl::PointXYZRGB>::PREWITT;
+ edge.hysteresis_thresholding_ = true;
+ edge.hysteresis_threshold_low_ = 20;
+ edge.hysteresis_threshold_high_ = 80;
+ edge.non_maximal_suppression_ = true;
+ edge.non_max_suppression_radius_x_ = 1;
+ edge.non_max_suppression_radius_y_ = 1;
+ edge.output_type_ = Edge<pcl::PointXYZRGB>::OUTPUT_X_Y;
+ edge.detectEdge (*output_cloud, *input_cloud);
+}
+
+void example_convolution ()
+{
+ Kernel<pcl::PointXYZRGB> kernel;
+ Convolution<pcl::PointXYZRGB> convolution;
+
+ /*dummy clouds*/
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr kernel_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+
+ /*example 1 : Gaussian Smoothing*/
+ kernel.sigma_ = 2.0;
+ kernel.kernel_size_ = 3;
+ kernel.gaussianKernel (*kernel_cloud);
+ convolution.kernel_ = *kernel_cloud;
+ convolution.convolve (*output_cloud, *input_cloud);
+
+ /*example 2 : forward derivative in X direction*/
+ kernel.kernel_type_ = Kernel<pcl::PointXYZRGB>::DERIVATIVE_FORWARD_X;
+ kernel.fetchKernel (*kernel_cloud);
+ convolution.kernel_ = *kernel_cloud;
+ convolution.convolve (*output_cloud, *input_cloud);
+
+ /*example 3*/
+ kernel.kernel_type_ = Kernel<pcl::PointXYZRGB>::DERIVATIVE_FORWARD_X;
+ kernel.fetchKernel (convolution.kernel_);
+ convolution.convolve (*output_cloud, *input_cloud);
+}
+
+void example_morphology ()
+{
+ Morphology<pcl::PointXYZRGB> morphology;
+
+ /*dummy clouds*/
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr structuring_element_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+
+ /*example 1 : Gaussian Smoothing*/
+ morphology.structuringElementCircular (*structuring_element_cloud, 3);
+ morphology.structuring_element_ = *structuring_element_cloud;
+ morphology.operator_type_ = Morphology<pcl::PointXYZRGB>::EROSION_GRAY;
+ morphology.applyMorphologicalOperation (*output_cloud, *input_cloud);
+
+ /*example 2 : forward derivative in X direction*/
+ morphology.structuringElementCircular (morphology.structuring_element_, 3);
+ morphology.operator_type_ = Morphology<pcl::PointXYZRGB>::EROSION_GRAY;
+ morphology.applyMorphologicalOperation (*output_cloud, *input_cloud);
+
+}
+
+int main(char *args, int argv)
+{
+ return 0;
+}
-PCL is a large collaborative effort, and it would not exist without the
-contributions of several people. Please see http://dev.pointclouds.org for a
-complete list of developers.
+PCL is a large collaborative effort, and it would not exist without the contributions of several people.
+ Please see https://github.com/PointCloudLibrary/pcl/graphs/contributors for a complete list of developers.
# ChangeList
+## *= 1.8.0 (14.06.2016) =*
+
+* Added missing `Eigen::aligned_allocator` in vectors and maps that contain
+ vectorizable Eigen where appropriate
+ [[#1034]](https://github.com/PointCloudLibrary/pcl/pull/1034)
+ [[#1052]](https://github.com/PointCloudLibrary/pcl/pull/1052)
+ [[#1068]](https://github.com/PointCloudLibrary/pcl/pull/1068)
+ [[#1182]](https://github.com/PointCloudLibrary/pcl/pull/1182)
+ [[#1497]](https://github.com/PointCloudLibrary/pcl/pull/1497)
+* Fixed compilation errors/warning when compiling in C++11 mode
+ [[#1179]](https://github.com/PointCloudLibrary/pcl/pull/1179)
+* Added a configuration option to choose between Qt4 and Qt5; the default is
+ changed to be Qt5
+ [[#1217]](https://github.com/PointCloudLibrary/pcl/pull/1217)
+* Improved compatibility with recent Eigen versions
+ [[#1261]](https://github.com/PointCloudLibrary/pcl/pull/1261)
+ [[#1298]](https://github.com/PointCloudLibrary/pcl/pull/1298)
+ [[#1316]](https://github.com/PointCloudLibrary/pcl/pull/1316)
+ [[#1369]](https://github.com/PointCloudLibrary/pcl/pull/1369)
+* Added support for VTK compiled with OpenGL2 backend (was introduced in VTK
+ 6.3, became default in VTK 7.0)
+ [[#1534]](https://github.com/PointCloudLibrary/pcl/pull/1534)
+
+### `libpcl_common:`
+
+* Added `copy_all_fields` option to the family of transformPointCloudXXX()
+ functions
+ [[#805]](https://github.com/PointCloudLibrary/pcl/pull/805)
+* Added a color lookup table consisting of 256 colors structured in a maximally
+ discontinuous manner (Glasbey colors)
+ [[#849]](https://github.com/PointCloudLibrary/pcl/pull/849)
+* Added a helper class `EventFrequency` to measure frequency of a certain event
+ [[#850]](https://github.com/PointCloudLibrary/pcl/pull/850)
+* Added a new `UniqueShapeContext1960` point type
+ [[#856]](https://github.com/PointCloudLibrary/pcl/pull/856)
+* Added a function `transformPointWithNormal()`
+ [[#908]](https://github.com/PointCloudLibrary/pcl/pull/908)
+* Fixed index-out-of-range error in `copyPointCloud()` for empty clouds
+ [[#933]](https://github.com/PointCloudLibrary/pcl/pull/933)
+* Fixed errors when compiling library with Boost 1.56 and Qt4
+ [[#938]](https://github.com/PointCloudLibrary/pcl/pull/938)
+* Created a new point type `PointXYZLNormal` with position, normal, and label
+ fields
+ [[#962]](https://github.com/PointCloudLibrary/pcl/pull/962)
+* Created a new point type `PointDEM` to represent Digital Elevation Maps
+ [[#1021]](https://github.com/PointCloudLibrary/pcl/pull/1021)
+* Fixed angle convexity calculation for parallel and anti-parallel normals,
+ where a rounding error occasionally caused NaN angles in `getAngle3D()`
+ [[#1035]](https://github.com/PointCloudLibrary/pcl/pull/1035)
+* Fixed undefined behavior when using multiple instances of `TimeTrigger`
+ [[#1074]](https://github.com/PointCloudLibrary/pcl/pull/1074)
+* Fixed starvation bug in `TimeTrigger` on Windows with Boost < 1.55
+ [[#1086]](https://github.com/PointCloudLibrary/pcl/pull/1086)
+* Removed unnecessary mutex locking in `TimeTrigger::registerCallback`
+ [[#1087]](https://github.com/PointCloudLibrary/pcl/pull/1087)
+* Updated PCL exception types to have nothrow copy constructor and copy
+ assigment operator
+ [[#1119]](https://github.com/PointCloudLibrary/pcl/pull/1119)
+* Fixed a bug with `PCA` not triggering recomputation when input indices are
+ changed
+ [[#1167]](https://github.com/PointCloudLibrary/pcl/pull/1167)
+* Added missing XYZ coordinate copying in `PointXYZRGBAtoXYZHSV` and
+ `PointXYZRGBtoXYZHSV` conversion functions
+ [[#1273]](https://github.com/PointCloudLibrary/pcl/pull/1273)
+* Added `const` qualifiers where appropriate in point type conversion functions
+ [[#1274]](https://github.com/PointCloudLibrary/pcl/pull/1274)
+* Fixed assignment operator in `PCA`
+ [[#1328]](https://github.com/PointCloudLibrary/pcl/pull/1328)
+* Added `PointWithRange` to the list of core point types
+ [[#1352]](https://github.com/PointCloudLibrary/pcl/pull/1352)
+* Fixed a bug in `getMaxDistance()` (this affected computation of OUR-CVFH
+ features)
+ [[#1449]](https://github.com/PointCloudLibrary/pcl/pull/1449)
+* Added `operator==` to `PCLHeader` class
+ [[#1508]](https://github.com/PointCloudLibrary/pcl/pull/1508)
+
+### `libpcl_features:`
+
+* Fixed default parameters of the USC descriptor to match the values proposed in
+ the original paper
+ [[#856]](https://github.com/PointCloudLibrary/pcl/pull/856)
+* Fixed the L1 normalization of the `ROPSEstimation` feature
+ [[#993]](https://github.com/PointCloudLibrary/pcl/pull/993)
+* Fixed default angle step in `ROPSEstimation`
+ [[#1000]](https://github.com/PointCloudLibrary/pcl/pull/1000)
+* Fixed a bug in `CRHEstimation` where internal spatial data vector was not
+ zero-initialized
+ [[#1151]](https://github.com/PointCloudLibrary/pcl/pull/1151)
+* Updated `NormalEstimation` to mark cloud as non-dense when normal computation
+ fails
+ [[#1239]](https://github.com/PointCloudLibrary/pcl/pull/1239)
+* Added new functions to compute approximate surface normals on a mesh and
+ approximate covariance matrices
+ [[#1262]](https://github.com/PointCloudLibrary/pcl/pull/1262)
+* Fixed histogram computation in `computePointPFHRGBSignature()`
+ [[#1331]](https://github.com/PointCloudLibrary/pcl/pull/1331)
+* Fixed wrong erasing order in feature cache in `PFHEstimation`
+ [[#1335]](https://github.com/PointCloudLibrary/pcl/pull/1335)
+
+### `libpcl_filters:`
+
+* Improved `RadiusOutlierRemoval` performance by using nearest-K search when the
+ input point cloud is dense
+ [[#709]](https://github.com/PointCloudLibrary/pcl/pull/709)
+* Fixed the signature of `BoxClipper3D::clipPlanarPolygon3D()`
+ [[#911]](https://github.com/PointCloudLibrary/pcl/pull/911)
+* Updated base `Filter` class to allow using same point cloud as input and
+ output (effective for every filtering algorithm)
+ [[#1042]](https://github.com/PointCloudLibrary/pcl/pull/1042)
+* Improved `CropBox` performance by caching the result of transform matrix
+ identity test
+ [[#1210]](https://github.com/PointCloudLibrary/pcl/pull/1210)
+* Updated `PassThrough` filter to write a user-supplied value in place of bad
+ points
+ [[#1290]](https://github.com/PointCloudLibrary/pcl/pull/1290)
+* Fixed handling of color fields in `VoxelGrid` centroid computation
+ [[#1415]](https://github.com/PointCloudLibrary/pcl/pull/1415)
+* Updated `ExtractIndices` (for `PCLPointCloud2` cloud type) to respect
+ `keep_organized_` flag
+ [[#1462]](https://github.com/PointCloudLibrary/pcl/pull/1462)
+* Fixed OpenMP support on MSVC in `Convolution3D`
+ [[#1527]](https://github.com/PointCloudLibrary/pcl/pull/1527)
+* BugFix: Filters used applyFilter twice.
+ [[#1572]](https://github.com/PointCloudLibrary/pcl/pull/1572)
+
+### `libpcl_gpu:`
+
+* Added a function `hasShifted()` in KinFu large scale
+ [[#944]](https://github.com/PointCloudLibrary/pcl/pull/944)
+* Fixed empty "View3D" window bug when using registration mode with `-pcd` flag
+ in KinFu app
+ [[#1018]](https://github.com/PointCloudLibrary/pcl/pull/1018)
+* Fixed uninitialized loop variable in `PeoplePCDApp::convertProbToRGB()`
+ [[#1104]](https://github.com/PointCloudLibrary/pcl/pull/1104)
+* Fixed compilation errors in `gpu_people`
+ [[#1194]](https://github.com/PointCloudLibrary/pcl/pull/1194)
+* Fixed compilation error in `kinfu_large_scale` with CUDA ≥ 6.0
+ [[#1225]](https://github.com/PointCloudLibrary/pcl/pull/1225)
+* Fixed volume size computation in `kinfu_large_scale`
+ [[#1233]](https://github.com/PointCloudLibrary/pcl/pull/1233)
+* Fixed sporadical out-of-bounds memory accesses in `kinfu_large_scale` kernels
+ [[#1263]](https://github.com/PointCloudLibrary/pcl/pull/1263)
+* Fixed `plot_camera_poses.m` script in KinFu project
+ [[#1311]](https://github.com/PointCloudLibrary/pcl/pull/1311)
+* Fixed runtime exceptions related to `--viz` flag in KinFu
+* Fix compilation on Mac OSX
+ [[#1586]](https://github.com/PointCloudLibrary/pcl/pull/1586)
+
+### `libpcl_io:`
+
+* Added a grabber for IDS Imaging Ensenso devices
+ [[#888]](https://github.com/PointCloudLibrary/pcl/pull/888)
+* Updated `RobotEyeGrabber` class to handle new packet format
+ [[#982]](https://github.com/PointCloudLibrary/pcl/pull/982)
+* Fixed focal length computation in `OpenNI2Grabber`
+ [[#992]](https://github.com/PointCloudLibrary/pcl/pull/992)
+* Updated `OpenNIGrabber` to use depth camera parameters instead of color camera
+ parameters for point reprojection
+ [[#994]](https://github.com/PointCloudLibrary/pcl/pull/994)
+* Made PCD reader case insensitive with respect to nan/NaN/NAN values
+ [[#1004]](https://github.com/PointCloudLibrary/pcl/pull/1004)
+* Added support for saving normal and curvature fields in `savePLYFile` and
+ `savePLYFileBinary`
+ [[#1057]](https://github.com/PointCloudLibrary/pcl/pull/1057)
+ [[#1058]](https://github.com/PointCloudLibrary/pcl/pull/1058)
+* Fixed alpha value of bad points in `OpenNIGrabber`
+ [[#1090]](https://github.com/PointCloudLibrary/pcl/pull/1090)
+* Fixed a bug in `OpenNIGrabber` destructor where wrong callback handle was
+ unregistered
+ [[#1094]](https://github.com/PointCloudLibrary/pcl/pull/1094)
+* Fixed a bug in `PCDGrabber` destructor
+ [[#1127]](https://github.com/PointCloudLibrary/pcl/pull/1127)
+* Fixed point coordinate computation in `HDLGrabber`
+ [[#1130]](https://github.com/PointCloudLibrary/pcl/pull/1130)
+* Improved the PLY parser to work around some issues on Mac OSX
+ [[#1165]](https://github.com/PointCloudLibrary/pcl/pull/1165)
+* Added a family of data buffer classes useful for temporal filtering in
+ grabbers
+ [[#1212]](https://github.com/PointCloudLibrary/pcl/pull/1212)
+* Added a grabber for davidSDK devices
+ [[#1216]](https://github.com/PointCloudLibrary/pcl/pull/1216)
+* Added a grabber and viewer for DepthSense SDK devices
+ [[#1230]](https://github.com/PointCloudLibrary/pcl/pull/1230)
+* Fixed stride computation and alpha values in
+ `OpenNI2Grabber::convertToXYZRGBPointCloud()`
+ [[#1248]](https://github.com/PointCloudLibrary/pcl/pull/1248)
+* Changed type and semantics of return values in polygon saving functions based
+ on VTK
+ [[#1279]](https://github.com/PointCloudLibrary/pcl/pull/1279)
+* Moved implementations of `pcl::io::load()` and `pcl::io::save()` to a new file
+ "io/auto_io.h"
+ [[#1294]](https://github.com/PointCloudLibrary/pcl/pull/1294)
+* Fixed compilation of `OpenNI2Grabber` on _msvc14_
+ [[#1310]](https://github.com/PointCloudLibrary/pcl/pull/1310)
+* Added a callback signal for the filename of grabbed PCD file in `PCDGrabber`
+ [[#1354]](https://github.com/PointCloudLibrary/pcl/pull/1354)
+* Added support for both 'CRLF' and 'LF' line endings in PLY reader
+ [[#1370]](https://github.com/PointCloudLibrary/pcl/pull/1370)
+* Updated OpenNI2 grabber to support devices without color stream
+ [[#1372]](https://github.com/PointCloudLibrary/pcl/pull/1372)
+* Updated `PCDWriter` to not fail when the filesystem does not support setting
+ file permissions
+ [[#1374]](https://github.com/PointCloudLibrary/pcl/pull/1374)
+* Fixed a bug in `MTLReader` reading function
+ [[#1380]](https://github.com/PointCloudLibrary/pcl/pull/1380)
+* Removed `PXCGrabber` (superseded by `DepthSenseGrabber`)
+ [[#1395]](https://github.com/PointCloudLibrary/pcl/pull/1395)
+* Added a grabber and viewer for RealSense SDK devices
+ [[#1401]](https://github.com/PointCloudLibrary/pcl/pull/1401)
+* Updated `loadPLYFile()` to support NaN values
+ [[#1433]](https://github.com/PointCloudLibrary/pcl/pull/1433)
+* Fixed parsing of `char` and `uchar` scalars in PLY files
+ [[#1443]](https://github.com/PointCloudLibrary/pcl/pull/1443)
+* Fixed ASCII file support in `savePolygonFile*` functions
+ [[#1445]](https://github.com/PointCloudLibrary/pcl/pull/1445)
+* Added a grabber and viewer for Velodyne VLP
+ [[#1452]](https://github.com/PointCloudLibrary/pcl/pull/1452)
+* Fix compilation when WITH_VTK=OFF
+ [[#1585]](https://github.com/PointCloudLibrary/pcl/pull/1585)
+
+### `libpcl_keypoints:`
+
+* Fixed invalid array allocation in `ISSKeypoint3D`
+ [[#1022]](https://github.com/PointCloudLibrary/pcl/pull/1022)
+* Removed superfluous parameter in 'TrajkovicKeypoint3D::getNormals()'
+ [[#1096]](https://github.com/PointCloudLibrary/pcl/pull/1096)
+* Moved `UniformSampling` to the `filters` module
+ [[#1411]](https://github.com/PointCloudLibrary/pcl/pull/1411)
+* Fixed OpenMP support in `HarrisKeypoint2D`
+ [[#1501]](https://github.com/PointCloudLibrary/pcl/pull/1501)
+* Updated `SIFTKeypoint` to preserve point cloud viewpoint
+ [[#1508]](https://github.com/PointCloudLibrary/pcl/pull/1508)
+
+### `libpcl_octree:`
+
+* Added `const` qualifiers in `OctreePointCloud::getVoxelBounds()`
+ [[#1016]](https://github.com/PointCloudLibrary/pcl/pull/1016)
+* Updated `Octree` iterator to use `unsigned long`s in key computations to
+ reduce chance of overflows
+ [[#1297]](https://github.com/PointCloudLibrary/pcl/pull/1297)
+* Fixed compilation of `OctreePointCloudOccupancy` on _gcc_
+ [[#1461]](https://github.com/PointCloudLibrary/pcl/pull/1461)
+
+### `libpcl_outofcore:`
+
+* Fixed compilation errors with C++11 standard
+ [[#1386]](https://github.com/PointCloudLibrary/pcl/pull/1386)
+
+### `libpcl_people:`
+
+* Fixed undefined behavior in `HOG` (use `new`/`delete` consistently)
+ [[#1099]](https://github.com/PointCloudLibrary/pcl/pull/1099)
+
+### `libpcl_recognition:`
+
+* Fixed multiple includes in `recognition` module
+ [[#1109]](https://github.com/PointCloudLibrary/pcl/pull/1109)
+ [[#1110]](https://github.com/PointCloudLibrary/pcl/pull/1110)
+* Fixed "index out of bounds" error in `LineRGBD::refineDetectionsAlongDepth()`
+ [[#1117]](https://github.com/PointCloudLibrary/pcl/pull/1117)
+* Fixed a memory leak in `LINEMOD::detectTemplatesSemiScaleInvariant()`
+ [[#1184]](https://github.com/PointCloudLibrary/pcl/pull/1184)
+
+### `libpcl_registration:`
+
+* Updated `GeneralizedIterativeClosestPoint` to return _transformed_ input point
+ cloud after alignment
+ [[#887]](https://github.com/PointCloudLibrary/pcl/pull/887)
+* Fixed a problem with multiple definition of `setInputFeatureCloud` and
+ `nearestNeighborSearch` symbols in `PPFRegistration`
+ [[#891]](https://github.com/PointCloudLibrary/pcl/pull/891)
+ [[#907]](https://github.com/PointCloudLibrary/pcl/pull/907)
+* Added an implementation of the algorithm "4-Points Congruent Sets for Robust
+ Surface Registration"
+ [[#976]](https://github.com/PointCloudLibrary/pcl/pull/976)
+* Added an implementation of the algorithm "Keypoint-based 4-Points Congruent
+ Sets – Automated marker-less registration of laser scans"
+ [[#979]](https://github.com/PointCloudLibrary/pcl/pull/979)
+* Fixed compilation of `pcl_registration` module with MSVC2010
+ [[#1014]](https://github.com/PointCloudLibrary/pcl/pull/1014)
+* Removed wrong error normalization in `SampleConsensusPrerejective`
+ [[#1037]](https://github.com/PointCloudLibrary/pcl/pull/1037)
+* Added a new `IncrementalRegistration` class that allows to register a stream
+ of clouds where each cloud is aligned to the previous cloud
+ [[#1202]](https://github.com/PointCloudLibrary/pcl/pull/1202)
+ [[#1451]](https://github.com/PointCloudLibrary/pcl/pull/1451)
+* Fixed a wrong typedef for `KdTreeReciprocalPtr`
+ [[#1204]](https://github.com/PointCloudLibrary/pcl/pull/1204)
+* Added support for externally computed covariance matrices in
+ `GeneralizedIterativeClosestPoint`
+ [[#1262]](https://github.com/PointCloudLibrary/pcl/pull/1262)
+* Fixed initialization of source and target covariances in
+ `GeneralizedIterativeClosestPoint6D`
+ [[#1304]](https://github.com/PointCloudLibrary/pcl/pull/1304)
+* Added a new `MetaRegistration` class that allows to register a stream of
+ clouds where each cloud is aligned to the conglomerate of all previous clouds
+ [[#1426]](https://github.com/PointCloudLibrary/pcl/pull/1426)
+* Fixed segmentation fault occuring in `CorrespondenceRejectorSurfaceNormal`
+ [[#1536]](https://github.com/PointCloudLibrary/pcl/pull/1536)
+* Use aligned allocator in vectors of MatchingCandidate
+ [[#1552]](https://github.com/PointCloudLibrary/pcl/pull/1552)
+
+### `libpcl_sample_consensus:`
+
+* Fixed behavior of `SACMODEL_PARALLEL_LINE` to match the name (instead of
+ searching for lines perpendicular to a given axis)
+ [[#969]](https://github.com/PointCloudLibrary/pcl/pull/969)
+* Added `getClassName()` function to all SAC models
+ [[#1071]](https://github.com/PointCloudLibrary/pcl/pull/1071)
+* Improved performance of `SampleConsensusModel::computeVariance()` by up to 10
+ times
+ [[#1285]](https://github.com/PointCloudLibrary/pcl/pull/1285)
+* Fixed assignment operators for `SacModelCone` and `SacModelCylinder`
+ [[#1299]](https://github.com/PointCloudLibrary/pcl/pull/1299)
+* Refactored SAC models to store expected model and sample sizes in a protected
+ member field; this deprecated `SAC_SAMPLE_SIZE` map
+ [[#1367]](https://github.com/PointCloudLibrary/pcl/pull/1367)
+ [[#1396]](https://github.com/PointCloudLibrary/pcl/pull/1396)
+
+### `libpcl_search:`
+
+* Fixed potential segfault in `OrganizedNeighbor::estimateProjectionMatrix()`
+ [[#1176]](https://github.com/PointCloudLibrary/pcl/pull/1176)
+
+### `libpcl_segmentation:`
+
+* Added implementation of `LCCP` segmentation algorithm
+ [[#718]](https://github.com/PointCloudLibrary/pcl/pull/718)
+ [[#1287]](https://github.com/PointCloudLibrary/pcl/pull/1287)
+ [[#1389]](https://github.com/PointCloudLibrary/pcl/pull/1389)
+* Made `SupervoxelClustering` fully deterministic and made some internal
+ refactoring
+ [[#912]](https://github.com/PointCloudLibrary/pcl/pull/912)
+* Moved specializations of `OctreePointCloudAdjacency::VoxelData` class from
+ header to implementation files
+ [[#919]](https://github.com/PointCloudLibrary/pcl/pull/919)
+* Deprecated `SupervoxelClustering::getColoredCloud()`
+ [[#941]](https://github.com/PointCloudLibrary/pcl/pull/941)
+* Fixed a regression in `ExtractPolygonalPrismData`; both explicitly and
+ implicitly closed polygons are supported again
+ [[#1044]](https://github.com/PointCloudLibrary/pcl/pull/1044)
+* Added an overload of `setConditionFunction()` in
+ `ConditionalEuclideanClustering` that takes `boost::function`
+ [[#1050]](https://github.com/PointCloudLibrary/pcl/pull/1050)
+* Updated `SupervoxelClustering` to use the depth dependent transform by
+ default only if the input cloud is organized; added a function to force use
+ of the transform, and removed corresponding parameter from the constructor
+ [[#1115]](https://github.com/PointCloudLibrary/pcl/pull/1115)
+* Substituted hard-coded label point type with template parameter in
+ `OrganizedConnectedComponentSegmentation`
+ [[#1264]](https://github.com/PointCloudLibrary/pcl/pull/1264)
+* Added an implementation of supervoxel graph partitioning algorithm described
+ in "Constrained Planar Cuts - Object Partitioning for Point Clouds"
+ [[#1278]](https://github.com/PointCloudLibrary/pcl/pull/1278)
+* Fixed crashes in `ApproximateProgressiveMorphologicalFilter` in the case of
+ non-default cell size
+ [[#1293]](https://github.com/PointCloudLibrary/pcl/pull/1293)
+* Fixed a bug in `RegionGrowing::validatePoint()`
+ [[#1327]](https://github.com/PointCloudLibrary/pcl/pull/1327)
+* Fixed return value of `SupervoxelClustering::getSeedResolution()`
+ [[#1339]](https://github.com/PointCloudLibrary/pcl/pull/1339)
+
+### `libpcl_stereo:`
+
+* Added a new `DisparityMapConverter` class for converting disparity maps into
+ point clouds
+ [[#1021]](https://github.com/PointCloudLibrary/pcl/pull/1021)
+* Added a new `DigitalElevationMapBuilder` class for building Digital Elevation
+ Maps from disparity maps
+ [[#1021]](https://github.com/PointCloudLibrary/pcl/pull/1021)
+
+### `libpcl_surface:`
+
+* Updated `TextureMapping` to not use hard-coded point types
+ [[#929]](https://github.com/PointCloudLibrary/pcl/pull/929)
+* Added a new function `getHullPointIndices` in concave and convex hull classes
+ to retrieve indices of points that form the computed hull
+ [[#1213]](https://github.com/PointCloudLibrary/pcl/pull/1213)
+* Added several functions and parameters to the `OrganizedFastMesh` class
+ [[#1262]](https://github.com/PointCloudLibrary/pcl/pull/1262)
+* Added missing `PCL_EXPORTS` attributes for OpenNURBS classes
+ [[#1315]](https://github.com/PointCloudLibrary/pcl/pull/1315)
+* Fixed memory leak in `MeshSmoothingLaplacianVTK`
+ [[#1424]](https://github.com/PointCloudLibrary/pcl/pull/1424)
+
+### `libpcl_tracking:`
+
+* Improved OMP 2.0 compatibility of `PyramidalKLTTracker`
+ [[#1214]](https://github.com/PointCloudLibrary/pcl/pull/1214)
+ [[#1223]](https://github.com/PointCloudLibrary/pcl/pull/1223)
+* Fixed occasional segfault in `KLDAdaptiveParticleFilterOMPTracker`
+ [[#1392]](https://github.com/PointCloudLibrary/pcl/pull/1392)
+
+### `libpcl_visualization:`
+
+* Added a new `PointCloudColorHandler` for "label" field
+ [[#849]](https://github.com/PointCloudLibrary/pcl/pull/849)
+* Fixed `setSize()` and `setPosition()` functions in `PCLVisualizer`
+ [[#923]](https://github.com/PointCloudLibrary/pcl/pull/923)
+* Fixed an issue with `PCLVisualizer` producing empty screenshots on some system
+ configurations
+ [[#956]](https://github.com/PointCloudLibrary/pcl/pull/956)
+* Added a new function `removeAllCoordinateSystems()` in `PCLVisualizer`
+ [[#965]](https://github.com/PointCloudLibrary/pcl/pull/965)
+* Made `PCLVisualizer::addPointCloudPrincipalCurvatures()` templated on point
+ and normal type
+ [[#965]](https://github.com/PointCloudLibrary/pcl/pull/965)
+* Fixed a minor bug in `PCLVisualizer::updatePolygonMesh()`
+ [[#977]](https://github.com/PointCloudLibrary/pcl/pull/977)
+* Fixed a minor bug in `ImageViewer::addMask()`
+ [[#990]](https://github.com/PointCloudLibrary/pcl/pull/990)
+* Fixed opacity handling in `ImageViewer`
+ [[#995]](https://github.com/PointCloudLibrary/pcl/pull/995)
+* Fixed a bug with `ImageViewer` not displaying anything with VTK 6
+ [[#1009]](https://github.com/PointCloudLibrary/pcl/pull/1009)
+* Updated `ImageViewer` to work around a bug in VTK 6.1
+ [[#1017]](https://github.com/PointCloudLibrary/pcl/pull/1017)
+* Fixed an Eigen-related compilation error in `PCLVisualizer::renderView()`
+ [[#1019]](https://github.com/PointCloudLibrary/pcl/pull/1019)
+* Fixed wrong axis flipping in `PCLVisualizer::renderView()`
+ [[#1026]](https://github.com/PointCloudLibrary/pcl/pull/1026)
+* Fixed a bug in `renderViewTesselatedSphere` when generated vertices were not
+ guaranteed to stay on the unit sphere
+ [[#1043]](https://github.com/PointCloudLibrary/pcl/pull/1043)
+* Fixed misaligned context items in `ImageViewer`
+ [[#1049]](https://github.com/PointCloudLibrary/pcl/pull/1049)
+* Fixed opacity handling for layered rectangles of context items in
+ `ImageViewer`
+ [[#1051]](https://github.com/PointCloudLibrary/pcl/pull/1051)
+* Fixed a regression in `RenderViewsTesselatedSphere::generateViews()` related
+ to handling of multiple VTK versions
+ [[#1056]](https://github.com/PointCloudLibrary/pcl/pull/1056)
+ [[#1067]](https://github.com/PointCloudLibrary/pcl/pull/1067)
+ [[#1072]](https://github.com/PointCloudLibrary/pcl/pull/1072)
+* Updated `PCLVisualizer` to use `PointCloudColorHandlerRGBAField` for
+ `PointXYZRGBA` clouds by default
+ [[#1064]](https://github.com/PointCloudLibrary/pcl/pull/1064)
+* Fixed a bug in `PointCloudColorHandlerLabelField` where red and blue channels
+ were swapped
+ [[#1076]](https://github.com/PointCloudLibrary/pcl/pull/1076)
+* Updated `PCLPlotter` to ignore NaN values in histogram computation
+ [[#1120]](https://github.com/PointCloudLibrary/pcl/pull/1120)
+ [[#1126]](https://github.com/PointCloudLibrary/pcl/pull/1126)
+* Fixed initial size of the `PCLVisualizer` window
+ [[#1125]](https://github.com/PointCloudLibrary/pcl/pull/1125)
+* Changed default representation of all shapes in `PCLVisualizer` to "surface"
+ [[#1132]](https://github.com/PointCloudLibrary/pcl/pull/1132)
+* Added a check for model coefficients size in functions that add shapes to
+ `PCLVisualizer`
+ [[#1142]](https://github.com/PointCloudLibrary/pcl/pull/1142)
+* Added an option to switch between static/optimal color assignment in
+ `PointCloudColorHandlerLabelField`
+ [[#1156]](https://github.com/PointCloudLibrary/pcl/pull/1156)
+* Added `PCLVisualizer::contains()` to check if a cloud, shape, or coordinate
+ axes with given id already exist
+ [[#1181]](https://github.com/PointCloudLibrary/pcl/pull/1181)
+* Improved shape visualization by enabling shading
+ [[#1211]](https://github.com/PointCloudLibrary/pcl/pull/1211)
+* Improved 'u' key functionality in `PCLVisualizer`
+ [[#1241]](https://github.com/PointCloudLibrary/pcl/pull/1241)
+ [[#1321]](https://github.com/PointCloudLibrary/pcl/pull/1321)
+ [[#1323]](https://github.com/PointCloudLibrary/pcl/pull/1323)
+* Fixed potential crashes in `PCLVisualizer` by always checking result of
+ `vtkSafeDownCast` calls
+ [[#1245]](https://github.com/PointCloudLibrary/pcl/pull/1245)
+* Updated `addPointCloud()` to use `PointCloudColorHandlerRGBField` when the
+ cloud has color field
+ [[#1295]](https://github.com/PointCloudLibrary/pcl/pull/1295)
+ [[#1325]](https://github.com/PointCloudLibrary/pcl/pull/1325)
+* Updated `PCLVisualizer` not to disable shading when changing shape's color
+ [[#1300]](https://github.com/PointCloudLibrary/pcl/pull/1300)
+* Fixed behavior of `PCLVisualizer::wasStopped()` with VTK6 on OSX
+ [[#1436]](https://github.com/PointCloudLibrary/pcl/pull/1436)
+* Improve pointcloud visualization with colormaps
+ [[#1581]](https://github.com/PointCloudLibrary/pcl/pull/1581)
+
+### `PCL Apps:`
+
+* Fixed compilation of `point_cloud_editor` with Qt5
+ [[#935]](https://github.com/PointCloudLibrary/pcl/pull/935)
+* Fixed compilation of `dominant_plane_segmentation` and `manual_registration`
+ with Boost 1.57
+ [[#1062]](https://github.com/PointCloudLibrary/pcl/pull/1062)
+ [[#1063]](https://github.com/PointCloudLibrary/pcl/pull/1063)
+
+### `PCL Examples:`
+
+* Updated supervoxel clustering example
+ [[#915]](https://github.com/PointCloudLibrary/pcl/pull/915)
+* Fixes for MS Visual Studio 2013
+ [[#1526]](https://github.com/PointCloudLibrary/pcl/pull/1526)
+
+### `PCL Tools:`
+
+* Added support for point label visualization in `pcl_viewer`
+ [[#849]](https://github.com/PointCloudLibrary/pcl/pull/849)
+* Added support for absolute positioning of visualized point clouds in
+ `pcl_viewer`
+ [[#1154]](https://github.com/PointCloudLibrary/pcl/pull/1154)
+* Fixed PLY file loading in `pcl_mesh_sampling` tool
+ [[#1155]](https://github.com/PointCloudLibrary/pcl/pull/1155)
+* Added loop distance (`-D`) and loop count (`-c`) parameters to the LUM tool
+ [[#1291]](https://github.com/PointCloudLibrary/pcl/pull/1291)
+* Fixed in-place filtering with `VoxelGrid` in `mesh_sampling` tool
+ [[#1366]](https://github.com/PointCloudLibrary/pcl/pull/1366)
+* Added a tool to convert OBJ files to PLY format
+ [[#1375]](https://github.com/PointCloudLibrary/pcl/pull/1375)
+* Added a universal mesh/cloud converted tool to convert between OBJ, PCD, PLY,
+ STL, and VTK files
+ [[#1442]](https://github.com/PointCloudLibrary/pcl/pull/1442)
+
## *= 1.7.2 (10.09.2014) =*
* Added support for VTK6
[[#811]](https://github.com/PointCloudLibrary/pcl/pull/811)
* Fixed memory corruption error in OUR-CVFH
[[#875]](https://github.com/PointCloudLibrary/pcl/pull/875)
+* Declare `const InterestPoint&` explicitly
+ [[#1541]](https://github.com/PointCloudLibrary/pcl/pull/1541)
### `libpcl_filters:`
[[#689]](https://github.com/PointCloudLibrary/pcl/pull/689)
* Reduced space usage in `MovingLeastSquares`
[[#785]](https://github.com/PointCloudLibrary/pcl/pull/785)
+* Adds MLS instantiation for input type PointXYZRGBNormal
+ [[#1545]](https://github.com/PointCloudLibrary/pcl/pull/1545)
### `libpcl_tracking:`
### ---[ PCL global CMake
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+if(POLICY CMP0017)
+ # Do not include files in CMAKE_MODULE_PATH from files
+ # in CMake module directory. Fix MXE build
+ cmake_policy(SET CMP0017 NEW)
+endif()
+
+if(POLICY CMP0020 AND (WIN32 AND NOT MINGW))
+ cmake_policy(SET CMP0020 NEW) # Automatically link Qt executables to qtmain target on Windows
+endif()
+
if(POLICY CMP0048)
cmake_policy(SET CMP0048 OLD) # do not use VERSION option in project() command
endif()
+if(POLICY CMP0054)
+ cmake_policy(SET CMP0054 OLD) # Silent warnings about quoted variables
+endif()
+
set(CMAKE_CONFIGURATION_TYPES "Debug;Release" CACHE STRING "possible configurations" FORCE)
# In case the user does not setup CMAKE_BUILD_TYPE, assume it's RelWithDebInfo
"Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel Maintainer."
FORCE)
-# ---[ Android check
-if (ANDROID)
- set (PCL_SHARED_LIBS OFF)
- message ("PCL shared libs on Android must be: ${PCL_SHARED_LIBS}")
+# Compiler identification
+# Define a variable CMAKE_COMPILER_IS_X where X is the compiler short name.
+# Note: CMake automatically defines one for GNUCXX, nothing to do in this case.
+if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+ set(CMAKE_COMPILER_IS_CLANG 1)
+elseif(__COMPILER_PATHSCALE)
+ set(CMAKE_COMPILER_IS_PATHSCALE 1)
+elseif(MSVC)
+ set(CMAKE_COMPILER_IS_MSVC 1)
endif()
include("${PCL_SOURCE_DIR}/cmake/pcl_verbosity.cmake")
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
endif(NOT ANDROID)
+ if("${CMAKE_SHARED_LINKER_FLAGS}" STREQUAL "" AND NOT CMAKE_SYSTEM_NAME STREQUAL "Darwin")
+ SET(CMAKE_SHARED_LINKER_FLAGS "-Wl,--as-needed")
+ endif()
+
if(WIN32)
if(PCL_SHARED_LIBS)
SET(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--export-all-symbols -Wl,--enable-auto-import")
if (MINGW)
add_definitions("-DBOOST_THREAD_USE_LIB")
+ SET(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--allow-multiple-definition")
endif()
else(PCL_SHARED_LIBS)
add_definitions("-DBOOST_LIB_DIAGNOSTIC -DBOOST_THREAD_USE_LIB")
endif()
endif()
-if(MSVC)
- SET(CMAKE_COMPILER_IS_MSVC 1)
- add_definitions ("-DBOOST_ALL_NO_LIB -D_SCL_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_WARNINGS -DNOMINMAX /bigobj")
+if(CMAKE_COMPILER_IS_MSVC)
+ add_definitions("-DBOOST_ALL_NO_LIB -D_SCL_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_WARNINGS -DNOMINMAX -DPCL_ONLY_CORE_POINT_TYPES /bigobj ${SSE_DEFINITIONS}")
if("${CMAKE_CXX_FLAGS}" STREQUAL " /DWIN32 /D_WINDOWS /W3 /GR /EHsc") # Check against default flags
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /bigobj /EHsc /fp:precise /wd4800 /wd4521 /wd4251 /wd4275 /wd4305 /wd4355 ${SSE_FLAGS}")
# Add extra code generation/link optimizations
if(CMAKE_MSVC_CODE_LINK_OPTIMIZATION)
SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /GL")
- SET(CMAKE_SHARED_LINKER_FLAGS_RELEASE "${CMAKE_SHARED_LINKER_FLAGS_RELEASE} /LTCG")
+ SET(CMAKE_SHARED_LINKER_FLAGS_RELEASE "${CMAKE_SHARED_LINKER_FLAGS_RELEASE} /LTCG /OPT:REF")
SET(CMAKE_EXE_LINKER_FLAGS_RELEASE "${CMAKE_EXE_LINKER_FLAGS_RELEASE} /LTCG")
endif(CMAKE_MSVC_CODE_LINK_OPTIMIZATION)
# /MANIFEST:NO") # please, don't disable manifest generation, otherwise crash at start for vs2008
endif()
endif()
-if (__COMPILER_PATHSCALE)
- SET(CMAKE_COMPILER_IS_PATHSCALE 1)
+if(CMAKE_COMPILER_IS_PATHSCALE)
if("${CMAKE_CXX_FLAGS}" STREQUAL "")
SET(CMAKE_CXX_FLAGS "-Wno-uninitialized -zerouv -pthread -mp")
endif()
+ if("${CMAKE_SHARED_LINKER_FLAGS}" STREQUAL "")
+ SET(CMAKE_SHARED_LINKER_FLAGS "-mp")
+ endif()
endif()
-if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
- SET(CMAKE_COMPILER_IS_CLANG 1)
+if(CMAKE_COMPILER_IS_CLANG)
if("${CMAKE_C_FLAGS}" STREQUAL "")
SET(CMAKE_C_FLAGS "-Qunused-arguments")
endif()
endif()
include("${PCL_SOURCE_DIR}/cmake/pcl_utils.cmake")
-set(PCL_VERSION 1.7.2 CACHE STRING "PCL version")
+set(PCL_VERSION 1.8.0 CACHE STRING "PCL version")
DISSECT_VERSION()
GET_OS_INFO()
SET_INSTALL_DIRS()
### ---[ Find universal dependencies
# the gcc-4.2.1 coming with MacOS X is not compatible with the OpenMP pragmas we use, so disabling OpenMP for it
-if((NOT APPLE) OR (NOT CMAKE_COMPILER_IS_GNUCXX) OR (GCC_VERSION VERSION_GREATER 4.2.1) OR (CMAKE_CXX_COMPILER_ID STREQUAL "Clang"))
+if((NOT APPLE) OR (NOT CMAKE_COMPILER_IS_GNUCXX) OR (GCC_VERSION VERSION_GREATER 4.2.1) OR (CMAKE_COMPILER_IS_CLANG))
find_package(OpenMP)
endif()
if(OPENMP_FOUND)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
message (STATUS "Found OpenMP")
- if(MSVC90 OR MSVC10)
+ if(MSVC)
if(MSVC90)
set(OPENMP_DLL VCOMP90)
elseif(MSVC10)
set(OPENMP_DLL VCOMP100)
+ elseif(MSVC11)
+ set(OPENMP_DLL VCOMP110)
+ elseif(MSVC12)
+ set(OPENMP_DLL VCOMP120)
+ elseif(MSVC14)
+ set(OPENMP_DLL VCOMP140)
endif(MSVC90)
- set(CMAKE_SHARED_LINKER_FLAGS_DEBUG "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} /DELAYLOAD:${OPENMP_DLL}D.dll")
- set(CMAKE_SHARED_LINKER_FLAGS_RELEASE "${CMAKE_SHARED_LINKER_FLAGS_RELEASE} /DELAYLOAD:${OPENMP_DLL}.dll")
- endif(MSVC90 OR MSVC10)
+ if(OPENMP_DLL)
+ set(CMAKE_SHARED_LINKER_FLAGS_DEBUG "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} /DELAYLOAD:${OPENMP_DLL}D.dll")
+ set(CMAKE_SHARED_LINKER_FLAGS_RELEASE "${CMAKE_SHARED_LINKER_FLAGS_RELEASE} /DELAYLOAD:${OPENMP_DLL}.dll")
+ else(OPENMP_DLL)
+ message(WARNING "Delay loading flag for OpenMP DLL is invalid.")
+ endif(OPENMP_DLL)
+ endif(MSVC)
else(OPENMP_FOUND)
message (STATUS "Not found OpenMP")
endif()
-# Boost (required)
-include("${PCL_SOURCE_DIR}/cmake/pcl_find_boost.cmake")
+
# Eigen (required)
find_package(Eigen REQUIRED)
include_directories(SYSTEM ${EIGEN_INCLUDE_DIRS})
endif(LIBUSB_1_FOUND)
endif(WITH_LIBUSB)
-# OpenNI
-option(WITH_OPENNI "OpenNI driver support" TRUE)
-if(WITH_OPENNI)
- find_package(OpenNI)
- if (OPENNI_FOUND)
- set(HAVE_OPENNI ON)
- include_directories(SYSTEM ${OPENNI_INCLUDE_DIRS})
- endif(OPENNI_FOUND)
-endif(WITH_OPENNI)
-
-# OpenNI 2
-option(WITH_OPENNI2 "OpenNI 2 driver support" TRUE)
-if(WITH_OPENNI2)
- find_package(OpenNI2)
- if (OPENNI2_FOUND)
- set(HAVE_OPENNI2 ON)
- include_directories(SYSTEM ${OPENNI2_INCLUDE_DIRS})
- endif(OPENNI2_FOUND)
-endif(WITH_OPENNI2)
-
-# Fotonic (FZ_API)
-option(WITH_FZAPI "Build Fotonic Camera support" TRUE)
-if(WITH_FZAPI)
- find_package(FZAPI)
- if (FZAPI_FOUND)
- set(HAVE_FZAPI ON)
- include_directories(SYSTEM "${FZAPI_INCLUDE_DIR}")
- endif(FZAPI_FOUND)
-endif(WITH_FZAPI)
-
-# Intel Perceptional Computing Interface (PXCAPI)
-option(WITH_PXCAPI "Build PXC Device support" TRUE)
-if(WITH_PXCAPI)
- find_package(PXCAPI)
- if (PXCAPI_FOUND)
- set(HAVE_PXCAPI ON)
- include_directories(SYSTEM ${PXCAPI_INCLUDE_DIRS})
- endif(PXCAPI_FOUND)
-endif(WITH_PXCAPI)
+# Dependencies for different grabbers
+PCL_ADD_GRABBER_DEPENDENCY("OpenNI" "OpenNI grabber support")
+PCL_ADD_GRABBER_DEPENDENCY("OpenNI2" "OpenNI2 grabber support")
+PCL_ADD_GRABBER_DEPENDENCY("FZAPI" "Fotonic camera support")
+PCL_ADD_GRABBER_DEPENDENCY("Ensenso" "IDS-Imaging Ensenso camera support")
+PCL_ADD_GRABBER_DEPENDENCY("davidSDK" "David Vision Systems SDK support")
+PCL_ADD_GRABBER_DEPENDENCY("DSSDK" "DepthSense SDK support")
+PCL_ADD_GRABBER_DEPENDENCY("RSSDK" "RealSense SDK support")
# metslib
if (PKG_CONFIG_FOUND)
set(QHULL_USE_STATIC ON)
endif(NOT PCL_SHARED_LIBS OR WIN32)
find_package(Qhull)
+ if(QHULL_FOUND)
+ include_directories(${QHULL_INCLUDE_DIRS})
+ endif(QHULL_FOUND)
endif(WITH_QHULL)
+# Cuda
+option(WITH_CUDA "Build NVIDIA-CUDA support" TRUE)
+if(WITH_CUDA)
+ include("${PCL_SOURCE_DIR}/cmake/pcl_find_cuda.cmake")
+endif(WITH_CUDA)
+
option(WITH_QT "Build QT Front-End" TRUE)
if(WITH_QT)
- # Find Qt4
- find_package(Qt4)
- if (QT4_FOUND)
- include("${QT_USE_FILE}")
- endif (QT4_FOUND)
-
- # Find QT5
- if(NOT QT4_FOUND)
- include(cmake/pcl_find_qt5.cmake)
- endif(NOT QT4_FOUND)
+ set(PCL_QT_VERSION 5 CACHE STRING "Which QT version to use")
+ if("${PCL_QT_VERSION}" STREQUAL "4")
+ find_package(Qt4)
+ if (QT4_FOUND)
+ include("${QT_USE_FILE}")
+ endif (QT4_FOUND)
+ elseif("${PCL_QT_VERSION}" STREQUAL "5")
+ include(cmake/pcl_find_qt5.cmake)
+ else()
+ message(SEND_ERROR "PCL_QT_VERSION must be 4 or 5")
+ endif()
endif(WITH_QT)
# Find VTK
if(WITH_VTK AND NOT ANDROID)
find_package(VTK)
if(VTK_FOUND)
- message(STATUS "VTK_MAJOR_VERSION ${VTK_MAJOR_VERSION}")
+ if(NOT DEFINED VTK_RENDERING_BACKEND)
+ # On old VTK versions this variable does not exist. In this case it is
+ # safe to assume OpenGL backend
+ set(VTK_RENDERING_BACKEND "OpenGL")
+ endif()
+ message(STATUS "VTK_MAJOR_VERSION ${VTK_MAJOR_VERSION}, rendering backend: ${VTK_RENDERING_BACKEND}")
if (PCL_SHARED_LIBS OR
(NOT (PCL_SHARED_LIBS) AND NOT (VTK_BUILD_SHARED_LIBS)))
set(VTK_FOUND TRUE)
option (VTK_USE_COCOA "Use Cocoa for VTK render windows" ON)
MARK_AS_ADVANCED (VTK_USE_COCOA)
endif (APPLE)
+ if(${VTK_RENDERING_BACKEND} STREQUAL "OpenGL")
+ set(VTK_RENDERING_BACKEND_OPENGL_VERSION "1")
+ elseif(${VTK_RENDERING_BACKEND} STREQUAL "OpenGL2")
+ set(VTK_RENDERING_BACKEND_OPENGL_VERSION "2")
+ endif()
set(HAVE_VTK ON)
else ()
set(VTK_FOUND OFF)
endif(WITH_PCAP)
# OpenGL and GLUT
-include("${PCL_SOURCE_DIR}/cmake/pcl_find_gl.cmake")
+option(WITH_OPENGL "Support for OpenGL" TRUE)
+if(WITH_OPENGL)
+ include("${PCL_SOURCE_DIR}/cmake/pcl_find_gl.cmake")
+endif(WITH_OPENGL)
+
+# Boost (required)
+include("${PCL_SOURCE_DIR}/cmake/pcl_find_boost.cmake")
### ---[ Create the config.h file
set(pcl_config_h_in "${CMAKE_CURRENT_SOURCE_DIR}/pcl_config.h.in")
### ---[ Finish up
PCL_WRITE_STATUS_REPORT()
PCL_RESET_MAPS()
+
endif(QHULL_FOUND)
endmacro(find_qhull)
-#remove this as soon as openni-dev is shipped with FindOpenni.cmake
+#remove this as soon as libopenni is shipped with FindOpenni.cmake
macro(find_openni)
- if(NOT OPENNI_ROOT AND ("@HAVE_OPENNI@" STREQUAL "ON"))
+ if(NOT OPENNI_ROOT AND ("@HAVE_OPENNI@" STREQUAL "TRUE"))
set(OPENNI_INCLUDE_DIRS_HINT "@OPENNI_INCLUDE_DIRS@")
get_filename_component(OPENNI_LIBRARY_HINT "@OPENNI_LIBRARY@" PATH)
- endif(NOT OPENNI_ROOT AND ("@HAVE_OPENNI@" STREQUAL "ON"))
+ endif()
if(PKG_CONFIG_FOUND)
- pkg_check_modules(PC_OPENNI openni-dev)
+ pkg_check_modules(PC_OPENNI libopenni)
endif(PKG_CONFIG_FOUND)
find_path(OPENNI_INCLUDE_DIRS XnStatus.h
HINTS ${PC_OPENNI_INCLUDEDIR} ${PC_OPENNI_INCLUDE_DIRS}
endif(OPENNI_FOUND)
endmacro(find_openni)
-#remove this as soon as openni2-dev is shipped with FindOpenni2.cmake
+#remove this as soon as libopenni2 is shipped with FindOpenni2.cmake
macro(find_openni2)
- if(NOT OPENNI2_ROOT AND ("ON" STREQUAL "ON"))
- get_filename_component(OPENNI2_LIBRARY_HINT "OPENNI_LIBRARY-NOTFOUND" PATH)
- endif(NOT OPENNI2_ROOT AND ("ON" STREQUAL "ON"))
+ if(PCL_FIND_QUIETLY)
+ set(OpenNI2_FIND_QUIETLY TRUE)
+ endif()
+
+ if(NOT OPENNI2_ROOT AND ("@HAVE_OPENNI2@" STREQUAL "TRUE"))
+ set(OPENNI2_INCLUDE_DIRS_HINT "@OPENNI2_INCLUDE_DIRS@")
+ get_filename_component(OPENNI2_LIBRARY_HINT "@OPENNI2_LIBRARY@" PATH)
+ endif()
set(OPENNI2_SUFFIX)
if(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8)
endif(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8)
if(PKG_CONFIG_FOUND)
- pkg_check_modules(PC_OPENNI2 openni2-dev)
+ if(PCL_FIND_QUIETLY)
+ pkg_check_modules(PC_OPENNI2 QUIET libopenni2)
+ else()
+ pkg_check_modules(PC_OPENNI2 libopenni2)
+ endif()
endif(PKG_CONFIG_FOUND)
find_path(OPENNI2_INCLUDE_DIRS OpenNI.h
- HINTS /usr/include/openni2 /usr/include/ni2
- PATHS "$ENV{OPENNI2_INCLUDE${OPENNI2_SUFFIX}}"
+ HINTS /usr/include/openni2 /usr/include/ni2 /usr/local/include/openni2 /usr/local/include/ni2
+ PATHS "$ENV{OPENNI2_INCLUDE${OPENNI2_SUFFIX}}" "${OPENNI2_INCLUDE_DIRS_HINT}"
PATH_SUFFIXES openni openni2 include Include)
find_library(OPENNI2_LIBRARY
NAMES OpenNI2 # No suffix needed on Win64
- HINTS /usr/lib
- PATHS "$ENV{OPENNI2_LIB${OPENNI2_SUFFIX}}"
+ HINTS /usr/lib /usr/local/lib/ni2
+ PATHS "$ENV{OPENNI2_LIB${OPENNI2_SUFFIX}}" "${OPENNI2_LIBRARY_HINT}"
PATH_SUFFIXES lib Lib Lib64)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(OpenNI2 DEFAULT_MSG OPENNI2_LIBRARY OPENNI2_INCLUDE_DIRS)
if(OPENNI2_FOUND)
- get_filename_component(OPENNI_LIBRARY_PATH ${OPENNI2_LIBRARY} PATH)
+ get_filename_component(OPENNI2_LIBRARY_PATH ${OPENNI2_LIBRARY} PATH)
set(OPENNI2_LIBRARY_DIRS ${OPENNI2_LIBRARY_PATH})
set(OPENNI2_LIBRARIES "${OPENNI2_LIBRARY}")
set(OPENNI2_REDIST_DIR $ENV{OPENNI2_REDIST${OPENNI2_SUFFIX}})
endif(OPENNI2_FOUND)
endmacro(find_openni2)
+#remove this as soon as the Ensenso SDK is shipped with FindEnsenso.cmake
+macro(find_ensenso)
+ if(PCL_FIND_QUIETLY)
+ set(ensenso_FIND_QUIETLY TRUE)
+ endif()
+
+ if(NOT ENSENSO_ROOT AND ("@HAVE_ENSENSO@" STREQUAL "TRUE"))
+ get_filename_component(ENSENSO_ABI_HINT @ENSENSO_INCLUDE_DIR@ PATH)
+ endif()
+
+ find_path(ENSENSO_INCLUDE_DIR nxLib.h
+ HINTS ${ENSENSO_ABI_HINT}
+ /opt/ensenso/development/c
+ "$ENV{PROGRAMFILES}/Ensenso/development/c" "$ENV{PROGRAMW6432}/Ensenso/development/c"
+ PATH_SUFFIXES include/)
+
+ find_library(ENSENSO_LIBRARY QUIET NAMES NxLib64 NxLib32 nxLib64 nxLib32
+ HINTS ${ENSENSO_ABI_HINT}
+ "$ENV{PROGRAMFILES}/Ensenso/development/c" "$ENV{PROGRAMW6432}/Ensenso/development/c"
+ PATH_SUFFIXES lib/)
+
+ include(FindPackageHandleStandardArgs)
+ find_package_handle_standard_args(ensenso DEFAULT_MSG
+ ENSENSO_LIBRARY ENSENSO_INCLUDE_DIR)
+
+ if(ENSENSO_FOUND)
+ get_filename_component(ENSENSO_LIBRARY_PATH ${ENSENSO_LIBRARY} PATH)
+ set(ENSENSO_INCLUDE_DIRS ${ENSENSO_INCLUDE_DIR})
+ set(ENSENSO_LIBRARY_DIRS ${ENSENSO_LIBRARY_PATH})
+ set(ENSENSO_LIBRARIES "${ENSENSO_LIBRARY}")
+ set(ENSENSO_REDIST_DIR $ENV{ENSENSO_REDIST${ENSENSO_SUFFIX}})
+ endif(ENSENSO_FOUND)
+endmacro(find_ensenso)
+
+#remove this as soon as the davidSDK is shipped with FinddavidSDK.cmake
+macro(find_davidSDK)
+ if(PCL_FIND_QUIETLY)
+ set(DAVIDSDK_FIND_QUIETLY TRUE)
+ endif()
+
+ if(NOT davidSDK_ROOT AND ("@HAVE_DAVIDSDK@" STREQUAL "TRUE"))
+ get_filename_component(DAVIDSDK_ABI_HINT @DAVIDSDK_INCLUDE_DIR@ PATH)
+ endif()
+
+ find_path(DAVIDSDK_INCLUDE_DIR david.h
+ HINTS ${DAVIDSDK_ABI_HINT}
+ /usr/local/include/davidSDK
+ "$ENV{PROGRAMFILES}/davidSDK" "$ENV{PROGRAMW6432}/davidSDK"
+ PATH_SUFFIXES include/)
+
+ find_library(DAVIDSDK_LIBRARY QUIET NAMES davidSDK
+ HINTS ${DAVIDSDK_ABI_HINT}
+ "$ENV{PROGRAMFILES}/davidSDK" "$ENV{PROGRAMW6432}/davidSDK"
+ PATH_SUFFIXES lib/)
+
+ include(FindPackageHandleStandardArgs)
+ find_package_handle_standard_args(DAVIDSDK DEFAULT_MSG
+ DAVIDSDK_LIBRARY DAVIDSDK_INCLUDE_DIR)
+
+ if(DAVIDSDK_FOUND)
+ get_filename_component(DAVIDSDK_LIBRARY_PATH ${DAVIDSDK_LIBRARY} PATH)
+ set(DAVIDSDK_INCLUDE_DIRS ${DAVIDSDK_INCLUDE_DIR})
+ set(DAVIDSDK_LIBRARY_DIRS ${DAVIDSDK_LIBRARY_PATH})
+ set(DAVIDSDK_LIBRARIES "${DAVIDSDK_LIBRARY}")
+ set(DAVIDSDK_REDIST_DIR $ENV{DAVIDSDK_REDIST${DAVIDSDK_SUFFIX}})
+ endif(DAVIDSDK_FOUND)
+endmacro(find_davidSDK)
+
+macro(find_dssdk)
+ if(PCL_FIND_QUIETLY)
+ set(DSSDK_FIND_QUIETLY TRUE)
+ endif()
+ if(NOT DSSDK_DIR AND ("@HAVE_DSSDK@" STREQUAL "TRUE"))
+ get_filename_component(DSSDK_DIR_HINT "@DSSDK_INCLUDE_DIRS@" PATH)
+ endif()
+ find_path(DSSDK_DIR include/DepthSenseVersion.hxx
+ HINTS ${DSSDK_DIR_HINT}
+ "$ENV{DEPTHSENSESDK32}"
+ "$ENV{DEPTHSENSESDK64}"
+ PATHS "$ENV{PROGRAMFILES}/SoftKinetic/DepthSenseSDK"
+ "$ENV{PROGRAMW6432}/SoftKinetic/DepthSenseSDK"
+ "C:/Program Files (x86)/SoftKinetic/DepthSenseSDK"
+ "C:/Program Files/SoftKinetic/DepthSenseSDK"
+ "/opt/softkinetic/DepthSenseSDK"
+ DOC "DepthSense SDK directory")
+ set(_DSSDK_INCLUDE_DIRS ${DSSDK_DIR}/include)
+ if(MSVC)
+ set(DSSDK_LIBRARIES_NAMES DepthSense)
+ else()
+ set(DSSDK_LIBRARIES_NAMES DepthSense DepthSensePlugins turbojpeg)
+ endif()
+ foreach(LIB ${DSSDK_LIBRARIES_NAMES})
+ find_library(DSSDK_LIBRARY_${LIB}
+ NAMES "${LIB}"
+ PATHS "${DSSDK_DIR}/lib/" NO_DEFAULT_PATH)
+ list(APPEND _DSSDK_LIBRARIES ${DSSDK_LIBRARY_${LIB}})
+ mark_as_advanced(DSSDK_LIBRARY_${LIB})
+ endforeach()
+ find_package_handle_standard_args(DSSDK DEFAULT_MSG _DSSDK_LIBRARIES _DSSDK_INCLUDE_DIRS)
+ if(DSSDK_FOUND)
+ set(DSSDK_LIBRARIES ${_DSSDK_LIBRARIES})
+ mark_as_advanced(DSSDK_LIBRARIES)
+ set(DSSDK_INCLUDE_DIRS ${_DSSDK_INCLUDE_DIRS})
+ mark_as_advanced(DSSDK_INCLUDE_DIRS)
+ endif()
+endmacro(find_dssdk)
+
+macro(find_rssdk)
+ if(PCL_FIND_QUIETLY)
+ set(RSSDK_FIND_QUIETLY TRUE)
+ endif()
+ if(NOT RSSDK_DIR AND ("@HAVE_RSSDK@" STREQUAL "TRUE"))
+ get_filename_component(RSSDK_DIR_HINT "@RSSDK_INCLUDE_DIRS@" PATH)
+ endif()
+ find_path(RSSDK_DIR include/pxcversion.h
+ HINTS ${RSSDK_DIR_HINT}
+ PATHS "$ENV{RSSDK_DIR}"
+ "$ENV{PROGRAMFILES}/Intel/RSSDK"
+ "$ENV{PROGRAMW6432}/Intel/RSSDK"
+ "C:/Program Files (x86)/Intel/RSSDK"
+ "C:/Program Files/Intel/RSSDK"
+ DOC "RealSense SDK directory")
+ set(_RSSDK_INCLUDE_DIRS ${RSSDK_DIR}/include)
+ set(RSSDK_RELEASE_NAME libpxc.lib)
+ set(RSSDK_DEBUG_NAME libpxc_d.lib)
+ find_library(RSSDK_LIBRARY
+ NAMES ${RSSDK_RELEASE_NAME}
+ PATHS "${RSSDK_DIR}/lib/" NO_DEFAULT_PATH
+ PATH_SUFFIXES x64 Win32)
+ find_library(RSSDK_LIBRARY_DEBUG
+ NAMES ${RSSDK_DEBUG_NAME} ${RSSDK_RELEASE_NAME}
+ PATHS "${RSSDK_DIR}/lib/" NO_DEFAULT_PATH
+ PATH_SUFFIXES x64 Win32)
+ if(NOT RSSDK_LIBRARY_DEBUG)
+ set(RSSDK_LIBRARY_DEBUG ${RSSDK_LIBRARY})
+ endif()
+ set(_RSSDK_LIBRARIES optimized ${RSSDK_LIBRARY} debug ${RSSDK_LIBRARY_DEBUG})
+ mark_as_advanced(RSSDK_LIBRARY RSSDK_LIBRARY_DEBUG)
+ find_package_handle_standard_args(RSSDK DEFAULT_MSG _RSSDK_LIBRARIES _RSSDK_INCLUDE_DIRS)
+ if(RSSDK_FOUND)
+ set(RSSDK_LIBRARIES ${_RSSDK_LIBRARIES})
+ mark_as_advanced(RSSDK_LIBRARIES)
+ set(RSSDK_INCLUDE_DIRS ${_RSSDK_INCLUDE_DIRS})
+ mark_as_advanced(RSSDK_INCLUDE_DIRS)
+ endif()
+endmacro(find_rssdk)
+
#remove this as soon as flann is shipped with FindFlann.cmake
macro(find_flann)
if(PCL_ALL_IN_ONE_INSTALLER)
macro(find_VTK)
if(PCL_ALL_IN_ONE_INSTALLER AND NOT ANDROID)
- set(VTK_DIR "${PCL_ROOT}/3rdParty/VTK/lib/vtk-5.8")
+ if(EXISTS "${PCL_ROOT}/3rdParty/VTK/lib/cmake")
+ set(VTK_DIR "${PCL_ROOT}/3rdParty/VTK/lib/cmake/vtk-@VTK_MAJOR_VERSION@.@VTK_MINOR_VERSION@")
+ else()
+ set(VTK_DIR "${PCL_ROOT}/3rdParty/VTK/lib/vtk-@VTK_MAJOR_VERSION@.@VTK_MINOR_VERSION@")
+ endif()
elseif(NOT VTK_DIR AND NOT ANDROID)
set(VTK_DIR "@VTK_DIR@")
endif(PCL_ALL_IN_ONE_INSTALLER AND NOT ANDROID)
ELSE (WIN32)
IF (APPLE)
-# These values for Apple could probably do with improvement.
- FIND_PATH( GLEW_INCLUDE_DIR glew.h
+
+ FIND_PATH( GLEW_INCLUDE_DIR GL/glew.h
+ /usr/local/include
/System/Library/Frameworks/GLEW.framework/Versions/A/Headers
${OPENGL_LIBRARY_DIR}
)
- SET(GLEW_GLEW_LIBRARY "-framework GLEW" CACHE STRING "GLEW library for OSX")
- SET(GLEW_cocoa_LIBRARY "-framework Cocoa" CACHE STRING "Cocoa framework for OSX")
+
+ FIND_LIBRARY( GLEW_GLEW_LIBRARY GLEW
+ /usr/local/lib
+ /usr/openwin/lib
+ /usr/X11R6/lib
+ )
+
+ if(NOT GLEW_GLEW_LIBRARY)
+ SET(GLEW_GLEW_LIBRARY "-framework GLEW" CACHE STRING "GLEW library for OSX")
+ SET(GLEW_cocoa_LIBRARY "-framework Cocoa" CACHE STRING "Cocoa framework for OSX")
+ endif()
ELSE (APPLE)
FIND_PATH( GLEW_INCLUDE_DIR GL/glew.h
find_openni()
elseif("${_lib}" STREQUAL "openni2")
find_openni2()
+ elseif("${_lib}" STREQUAL "ensenso")
+ find_ensenso()
+ elseif("${_lib}" STREQUAL "davidSDK")
+ find_davidSDK()
+ elseif("${_lib}" STREQUAL "dssdk")
+ find_dssdk()
+ elseif("${_lib}" STREQUAL "rssdk")
+ find_rssdk()
elseif("${_lib}" STREQUAL "vtk")
find_VTK()
elseif("${_lib}" STREQUAL "libusb-1.0")
string(TOUPPER "${_component}" COMPONENT)
string(TOUPPER "${_lib}" LIB)
+ string(REGEX REPLACE "[.-]" "_" LIB ${LIB})
if(${LIB}_FOUND)
list(APPEND PCL_${COMPONENT}_INCLUDE_DIRS ${${LIB}_INCLUDE_DIRS})
if(${LIB}_USE_FILE)
@PCLCONFIG_OPTIONAL_DEPENDENCIES@
-set(pcl_header_only_components geometry modeler in_hand_scanner point_cloud_editor cloud_composer optronic_viewer)
+set(pcl_header_only_components 2d cuda_common geometry gpu_tracking modeler in_hand_scanner point_cloud_editor cloud_composer optronic_viewer)
include(FindPackageHandleStandardArgs)
pcl_message(STATUS "looking for PCL_${COMPONENT}")
+ string(REGEX REPLACE "^cuda_(.*)$" "\\1" cuda_component "${component}")
+ string(REGEX REPLACE "^gpu_(.*)$" "\\1" gpu_component "${component}")
+
find_path(PCL_${COMPONENT}_INCLUDE_DIR
NAMES pcl/${component}
pcl/apps/${component}
+ pcl/cuda/${cuda_component} pcl/cuda/${component}
+ pcl/gpu/${gpu_component} pcl/gpu/${component}
HINTS ${PCL_INCLUDE_DIRS}
"${PCL_SOURCES_TREE}"
PATH_SUFFIXES
${component}/include
apps/${component}/include
+ cuda/${cuda_component}/include
+ gpu/${gpu_component}/include
DOC "path to ${component} headers"
NO_DEFAULT_PATH)
pcl_remove_duplicate_libraries(PCL_LIBRARIES PCL_DEDUP_LIBRARIES)
set(PCL_LIBRARIES ${PCL_DEDUP_LIBRARIES})
# Add 3rd party libraries, as user code might include our .HPP implementations
-list(APPEND PCL_LIBRARIES ${BOOST_LIBRARIES} ${QHULL_LIBRARIES} ${OPENNI_LIBRARIES} ${OPENNI2_LIBRARIES} ${FLANN_LIBRARIES} ${VTK_LIBRARIES})
+list(APPEND PCL_LIBRARIES ${BOOST_LIBRARIES} ${QHULL_LIBRARIES} ${OPENNI_LIBRARIES} ${OPENNI2_LIBRARIES} ${ENSENSO_LIBRARIES} ${davidSDK_LIBRARIES} ${DSSDK_LIBRARIES} ${RSSDK_LIBRARIES} ${FLANN_LIBRARIES} ${VTK_LIBRARIES})
find_package_handle_standard_args(PCL DEFAULT_MSG PCL_LIBRARIES PCL_INCLUDE_DIRS)
mark_as_advanced(PCL_LIBRARIES PCL_INCLUDE_DIRS PCL_LIBRARY_DIRS)
+
-pcl
-===
+# Point Cloud Library
+<img src="http://ns50.pointclouds.org/assets/images/contents/logos/pcl/pcl_horz_large_pos.png" align="center" height="100">
+
+Continuous integration
+----------------------
+[ ![Release] [release-image] ] [releases]
+[ ![License] [license-image] ] [license]
+
+[release-image]: https://img.shields.io/badge/release-1.8.0-green.svg?style=flat
+[releases]: https://github.com/PointCloudLibrary/pcl/releases
+
+[license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat
+[license]: https://github.com/PointCloudLibrary/pcl/blob/master/LICENSE.txt
+
+[](https://travis-ci.org/PointCloudLibrary/pcl)
+
+Description
+-----------
The Point Cloud Library (PCL) is a standalone, large scale, open project for 2D/3D image and point cloud processing.
PCL is released under the terms of the BSD license, and thus free for commercial and research use. We are financially supported by a consortium of commercial companies, with our own non-profit organization, Open Perception. We would also like to thank individual donors and contributors that have been helping the project.
+
+Compiling
+---------
+Please refer to the platform specific tutorials:
+ - [Linux](http://www.pointclouds.org/documentation/tutorials/compiling_pcl_posix.php)
+ - [Mac OS X](http://www.pointclouds.org/documentation/tutorials/compiling_pcl_macosx.php)
+ - [Microsoft Windows](http://www.pointclouds.org/documentation/tutorials/compiling_pcl_windows.php)
+
+Documentation
+-------------
+- [Tutorials](http://www.pointclouds.org/documentation/tutorials/)
+- [PCL trunk documentation](http://docs.pointclouds.org/trunk/) (generated 2 times a week)
+
+Contributing
+------------
+Please read [CONTRIBUTING.md](https://github.com/PointCloudLibrary/pcl/blob/master/CONTRIBUTING.md).
+
+Issues
+------
+For general questions on how to use the PCL, please use the [pcl-users](http://www.pcl-users.org/) mailing list (do not forget to subscribe before posting).
+To report issues, please read [CONTRIBUTING.md#bug-reports](https://github.com/PointCloudLibrary/pcl/blob/master/CONTRIBUTING.md#bug-reports).
--- /dev/null
+set(SUBSUBSYS_NAME 3d_rec_framework)
+set(SUBSUBSYS_DESC "3D recognition framework")
+set(SUBSUBSYS_DEPS common geometry io filters sample_consensus segmentation visualization kdtree features surface octree registration keypoints tracking search recognition ml)
+
+# Find VTK
+if(NOT VTK_FOUND)
+ set(DEFAULT AUTO_OFF)
+ set(REASON "VTK was not found.")
+else(NOT VTK_FOUND)
+ set(DEFAULT TRUE)
+ set(REASON)
+ include("${VTK_USE_FILE}")
+endif(NOT VTK_FOUND)
+
+# OpenNI found?
+if(NOT WITH_OPENNI)
+ set(DEFAULT AUTO_OFF)
+ set(REASON "OpenNI was not found or was disabled by the user.")
+elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF")
+ set(DEFAULT TRUE)
+ set(REASON)
+endif()
+
+# Default to not building for now
+if (${DEFAULT} STREQUAL "TRUE")
+ set(DEFAULT FALSE)
+endif()
+
+PCL_SUBSUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSUBSYS_DESC}" ${DEFAULT} "${REASON}")
+PCL_SUBSUBSYS_DEPEND(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" DEPS ${SUBSUBSYS_DEPS} EXT_DEPS vtk openni)
+
+if(build)
+
+ include_directories("${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}/include")
+
+ set(incs_fw "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/normal_estimator.h")
+ set(incs_fw_global "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/global/cvfh_estimator.h"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/global/vfh_estimator.h"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/global/esf_estimator.h"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/global/crh_estimator.h"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/global/global_estimator.h"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/global/ourcvfh_estimator.h")
+
+ set(incs_fw_local "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/local/local_estimator.h"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/local/fpfh_local_estimator.h"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/local/fpfh_local_estimator_omp.h"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/local/shot_local_estimator.h"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/local/colorshot_local_estimator.h"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/local/shot_local_estimator_omp.h")
+ set(incs_pc_source "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pc_source/source.h"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pc_source/mesh_source.h"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pc_source/registered_views_source.h")
+
+ set(incs_pipelines "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline/global_nn_classifier.h"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline/global_nn_recognizer_crh.h"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline/global_nn_recognizer_cvfh.h"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline/local_recognizer.h")
+
+ set(incc_tools_framework "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/tools/openni_frame_source.h")
+
+ set(incs_utils "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/utils/metrics.h"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/utils/persistence_utils.h"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/utils/vtk_model_sampling.h")
+
+ set(srcs src/pipeline/global_nn_classifier.cpp
+ src/pipeline/global_nn_recognizer_crh.cpp
+ src/pipeline/global_nn_recognizer_cvfh.cpp
+ src/pipeline/local_recognizer.cpp
+ src/tools/openni_frame_source.cpp)
+
+ set(impl_incs_pipeline "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline/impl/global_nn_classifier.hpp"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline/impl/global_nn_recognizer_crh.hpp"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline/impl/global_nn_recognizer_cvfh.hpp"
+ "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline/impl/local_recognizer.hpp")
+
+ # Install include files
+ PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper" ${incs_fw})
+ PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/global" ${incs_fw_global})
+ PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/local" ${incs_fw_local})
+ PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSYS_NAME}/${SUBSUBSYS_NAME}/tools" ${incc_tools_framework})
+ PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline" ${incs_pipelines})
+ PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pc_source" ${incs_pc_source})
+ PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSYS_NAME}/${SUBSUBSYS_NAME}/utils" ${incs_utils})
+
+ PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline/impl" ${impl_incs_pipeline})
+
+ set(LIB_NAME "pcl_${SUBSUBSYS_NAME}")
+ PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSUBSYS_NAME}" ${srcs} ${impl_incs_pipeline} ${incs_utils} ${incs_fw} ${incs_fw_global} ${incs_fw_local} ${incc_tools_framework} ${incs_pipelines} ${incs_pc_source})
+ target_link_libraries("${LIB_NAME}" pcl_apps pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_surface pcl_features pcl_sample_consensus pcl_search)
+
+ if(WITH_OPENNI)
+ target_link_libraries("${LIB_NAME}" ${OPENNI_LIBRARIES})
+ if(NOT WIN32)
+ find_package(libusb-1.0 REQUIRED)
+ target_link_libraries("${LIB_NAME}" ${LIBUSB_1_LIBRARIES})
+ endif()
+ endif()
+
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSUBSYS_NAME}" "${SUBSUBSYS_DESC}" "" "" "" "" "")
+
+ add_subdirectory(tools)
+
+endif(build)
--- /dev/null
+/*
+ * vfh_estimator.h
+ *
+ * Created on: Mar 22, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_CRH_ESTIMATOR_H_
+#define REC_FRAMEWORK_CRH_ESTIMATOR_H_
+
+#include <pcl/apps/3d_rec_framework/feature_wrapper/global/global_estimator.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
+#include <pcl/features/crh.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+ template<typename PointInT, typename FeatureT>
+ class CRHEstimation : public GlobalEstimator<PointInT, FeatureT>
+ {
+
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ using GlobalEstimator<PointInT, FeatureT>::normal_estimator_;
+ using GlobalEstimator<PointInT, FeatureT>::normals_;
+
+ typename boost::shared_ptr<GlobalEstimator<PointInT, FeatureT> > feature_estimator_;
+ typedef pcl::PointCloud<pcl::Histogram<90> > CRHPointCloud;
+ std::vector< CRHPointCloud::Ptr > crh_histograms_;
+
+ public:
+
+ CRHEstimation ()
+ {
+
+ }
+
+ void
+ setFeatureEstimator(typename boost::shared_ptr<GlobalEstimator<PointInT, FeatureT> > & feature_estimator) {
+ feature_estimator_ = feature_estimator;
+ }
+
+ void
+ estimate (PointInTPtr & in, PointInTPtr & processed,
+ std::vector<pcl::PointCloud<FeatureT>, Eigen::aligned_allocator<pcl::PointCloud<FeatureT> > > & signatures,
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > & centroids)
+ {
+
+ if (!feature_estimator_)
+ {
+ PCL_ERROR("CRHEstimation needs a global estimator... please provide one\n");
+ return;
+ }
+
+ feature_estimator_->estimate(in, processed, signatures, centroids);
+
+ if(!feature_estimator_->computedNormals()) {
+ normals_.reset(new pcl::PointCloud<pcl::Normal>);
+ normal_estimator_->estimate (in, processed, normals_);
+ } else {
+ feature_estimator_->getNormals(normals_);
+ }
+
+ crh_histograms_.resize(signatures.size());
+
+ typedef typename pcl::CRHEstimation<PointInT, pcl::Normal, pcl::Histogram<90> > CRHEstimation;
+ CRHEstimation crh;
+ crh.setInputCloud(processed);
+ crh.setInputNormals(normals_);
+
+ for (size_t idx = 0; idx < signatures.size (); idx++)
+ {
+ Eigen::Vector4f centroid4f(centroids[idx][0],centroids[idx][1],centroids[idx][2],0);
+ crh.setCentroid(centroid4f);
+ crh_histograms_[idx].reset(new CRHPointCloud());
+ crh.compute (*crh_histograms_[idx]);
+ }
+
+ }
+
+ void getCRHHistograms(std::vector< CRHPointCloud::Ptr > & crh_histograms) {
+ crh_histograms = crh_histograms_;
+ }
+
+ bool
+ computedNormals ()
+ {
+ return true;
+ }
+ };
+ }
+}
+
+#endif /* REC_FRAMEWORK_CVFH_ESTIMATOR_H_ */
--- /dev/null
+/*
+ * vfh_estimator.h
+ *
+ * Created on: Mar 22, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_CVFH_ESTIMATOR_H_
+#define REC_FRAMEWORK_CVFH_ESTIMATOR_H_
+
+#include <pcl/apps/3d_rec_framework/feature_wrapper/global/global_estimator.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
+#include <pcl/features/cvfh.h>
+#include <pcl/surface/mls.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+ template<typename PointInT, typename FeatureT>
+ class CVFHEstimation : public GlobalEstimator<PointInT, FeatureT>
+ {
+
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ using GlobalEstimator<PointInT, FeatureT>::normal_estimator_;
+ using GlobalEstimator<PointInT, FeatureT>::normals_;
+ float eps_angle_threshold_;
+ float curvature_threshold_;
+ float cluster_tolerance_factor_;
+ bool normalize_bins_;
+ bool adaptative_MLS_;
+
+ public:
+
+ CVFHEstimation ()
+ {
+ eps_angle_threshold_ = 0.13f;
+ curvature_threshold_ = 0.035f;
+ normalize_bins_ = true;
+ cluster_tolerance_factor_ = 3.f;
+ }
+
+ void setCVFHParams(float p1, float p2, float p3) {
+ eps_angle_threshold_ = p1;
+ curvature_threshold_ = p2;
+ cluster_tolerance_factor_ = p3;
+ }
+
+ void setAdaptativeMLS(bool b) {
+ adaptative_MLS_ = b;
+ }
+
+ void
+ estimate (PointInTPtr & in, PointInTPtr & processed,
+ typename pcl::PointCloud<FeatureT>::CloudVectorType & signatures,
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > & centroids)
+ {
+
+ if (!normal_estimator_)
+ {
+ PCL_ERROR("This feature needs normals... please provide a normal estimator\n");
+ return;
+ }
+
+ pcl::MovingLeastSquares<PointInT, PointInT> mls;
+ if(adaptative_MLS_) {
+ typename search::KdTree<PointInT>::Ptr tree;
+ Eigen::Vector4f centroid_cluster;
+ pcl::compute3DCentroid (*in, centroid_cluster);
+ float dist_to_sensor = centroid_cluster.norm();
+ float sigma = dist_to_sensor * 0.01f;
+ mls.setSearchMethod(tree);
+ mls.setSearchRadius (sigma);
+ mls.setUpsamplingMethod (mls.SAMPLE_LOCAL_PLANE);
+ mls.setUpsamplingRadius (0.002);
+ mls.setUpsamplingStepSize (0.001);
+ }
+
+ normals_.reset (new pcl::PointCloud<pcl::Normal>);
+ {
+ normal_estimator_->estimate (in, processed, normals_);
+ }
+
+ if(adaptative_MLS_) {
+ mls.setInputCloud(processed);
+
+ PointInTPtr filtered(new pcl::PointCloud<PointInT>);
+ mls.process(*filtered);
+
+ processed.reset(new pcl::PointCloud<PointInT>);
+ normals_.reset (new pcl::PointCloud<pcl::Normal>);
+ {
+ filtered->is_dense = false;
+ normal_estimator_->estimate (filtered, processed, normals_);
+ }
+ }
+
+ /*normals_.reset(new pcl::PointCloud<pcl::Normal>);
+ normal_estimator_->estimate (in, processed, normals_);*/
+
+ typedef typename pcl::CVFHEstimation<PointInT, pcl::Normal, FeatureT> CVFHEstimation;
+ pcl::PointCloud<FeatureT> cvfh_signatures;
+ typename pcl::search::KdTree<PointInT>::Ptr cvfh_tree (new pcl::search::KdTree<PointInT>);
+
+ CVFHEstimation cvfh;
+ cvfh.setSearchMethod (cvfh_tree);
+ cvfh.setInputCloud (processed);
+ cvfh.setInputNormals (normals_);
+
+ cvfh.setEPSAngleThreshold (eps_angle_threshold_);
+ cvfh.setCurvatureThreshold (curvature_threshold_);
+ cvfh.setNormalizeBins (normalize_bins_);
+
+ float radius = normal_estimator_->normal_radius_;
+ float cluster_tolerance_radius = normal_estimator_->grid_resolution_ * cluster_tolerance_factor_;
+
+ if (normal_estimator_->compute_mesh_resolution_)
+ {
+ radius = normal_estimator_->mesh_resolution_ * normal_estimator_->factor_normals_;
+ cluster_tolerance_radius = normal_estimator_->mesh_resolution_ * cluster_tolerance_factor_;
+
+ if (normal_estimator_->do_voxel_grid_)
+ {
+ radius *= normal_estimator_->factor_voxel_grid_;
+ cluster_tolerance_radius *= normal_estimator_->factor_voxel_grid_;
+ }
+ }
+
+ cvfh.setClusterTolerance (cluster_tolerance_radius);
+ cvfh.setRadiusNormals (radius);
+ cvfh.setMinPoints (100);
+
+ //std::cout << "Res:" << normal_estimator_->mesh_resolution_ << " Radius normals:" << radius << " Cluster tolerance:" << cluster_tolerance_radius << " " << eps_angle_threshold_ << " " << curvature_threshold_ << std::endl;
+
+ cvfh.compute (cvfh_signatures);
+
+ for (size_t i = 0; i < cvfh_signatures.points.size (); i++)
+ {
+ pcl::PointCloud<FeatureT> vfh_signature;
+ vfh_signature.points.resize (1);
+ vfh_signature.width = vfh_signature.height = 1;
+ for (int d = 0; d < 308; ++d)
+ vfh_signature.points[0].histogram[d] = cvfh_signatures.points[i].histogram[d];
+
+ signatures.push_back (vfh_signature);
+ }
+
+ cvfh.getCentroidClusters (centroids);
+ //std::cout << "centroids size:" << centroids.size () << std::endl;
+
+ }
+
+ bool
+ computedNormals ()
+ {
+ return true;
+ }
+
+ void setNormalizeBins(bool b) {
+ normalize_bins_ = b;
+ }
+ };
+ }
+}
+
+#endif /* REC_FRAMEWORK_CVFH_ESTIMATOR_H_ */
--- /dev/null
+/*
+ * vfh_estimator.h
+ *
+ * Created on: Mar 22, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_ESF_ESTIMATOR_H_
+#define REC_FRAMEWORK_ESF_ESTIMATOR_H_
+
+#include <pcl/apps/3d_rec_framework/feature_wrapper/global/global_estimator.h>
+#include <pcl/features/esf.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+ template<typename PointInT, typename FeatureT>
+ class ESFEstimation : public GlobalEstimator<PointInT, FeatureT>
+ {
+
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+
+ public:
+ void
+ estimate (PointInTPtr & in, PointInTPtr & processed,
+ typename pcl::PointCloud<FeatureT>::CloudVectorType & signatures,
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > & centroids)
+ {
+
+ typedef typename pcl::ESFEstimation<PointInT, FeatureT> ESFEstimation;
+ pcl::PointCloud<FeatureT> ESF_signature;
+
+ ESFEstimation esf;
+ esf.setInputCloud (in);
+ esf.compute (ESF_signature);
+
+ signatures.resize (1);
+ centroids.resize (1);
+
+ signatures[0] = ESF_signature;
+
+ Eigen::Vector4f centroid4f;
+ pcl::compute3DCentroid (*in, centroid4f);
+ centroids[0] = Eigen::Vector3f (centroid4f[0], centroid4f[1], centroid4f[2]);
+
+ pcl::copyPointCloud(*in, *processed);
+ //processed = in;
+ }
+
+ bool
+ computedNormals ()
+ {
+ return false;
+ }
+ };
+ }
+}
+
+#endif /* REC_FRAMEWORK_ESF_ESTIMATOR_H_ */
--- /dev/null
+/*
+ * estimator.h
+ *
+ * Created on: Mar 22, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_GLOBAL_ESTIMATOR_H_
+#define REC_FRAMEWORK_GLOBAL_ESTIMATOR_H_
+
+#include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+ template <typename PointInT, typename FeatureT>
+ class GlobalEstimator {
+ protected:
+ bool computed_normals_;
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ typedef typename pcl::PointCloud<FeatureT>::Ptr FeatureTPtr;
+
+ typename boost::shared_ptr<PreProcessorAndNormalEstimator<PointInT, pcl::Normal> > normal_estimator_;
+
+ pcl::PointCloud<pcl::Normal>::Ptr normals_;
+
+ public:
+ virtual void
+ estimate (PointInTPtr & in, PointInTPtr & processed, std::vector<pcl::PointCloud<FeatureT>, Eigen::aligned_allocator<
+ pcl::PointCloud<FeatureT> > > & signatures, std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > & centroids)=0;
+
+ virtual bool computedNormals() = 0;
+
+ void setNormalEstimator(boost::shared_ptr<PreProcessorAndNormalEstimator<PointInT, pcl::Normal> > & ne) {
+ normal_estimator_ = ne;
+ }
+
+ void getNormals(pcl::PointCloud<pcl::Normal>::Ptr & normals) {
+ normals = normals_;
+ }
+
+ };
+ }
+}
+
+
+#endif /* REC_FRAMEWORK_ESTIMATOR_H_ */
--- /dev/null
+/*
+ * vfh_estimator.h
+ *
+ * Created on: Mar 22, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_OURCVFH_ESTIMATOR_H_
+#define REC_FRAMEWORK_OURCVFH_ESTIMATOR_H_
+
+#include <pcl/apps/3d_rec_framework/feature_wrapper/global/global_estimator.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
+#include <pcl/features/our_cvfh.h>
+#include <pcl/surface/mls.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+ template<typename PointInT, typename FeatureT>
+ class OURCVFHEstimator : public GlobalEstimator<PointInT, FeatureT>
+ {
+
+ protected:
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ using GlobalEstimator<PointInT, FeatureT>::normal_estimator_;
+ using GlobalEstimator<PointInT, FeatureT>::normals_;
+ float eps_angle_threshold_;
+ float curvature_threshold_;
+ float cluster_tolerance_factor_;
+ bool normalize_bins_;
+ bool adaptative_MLS_;
+ float refine_factor_;
+
+ std::vector<bool> valid_roll_transforms_;
+ std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transforms_;
+ std::vector<pcl::PointIndices> cluster_indices_;
+
+ public:
+
+ OURCVFHEstimator ()
+ {
+ eps_angle_threshold_ = 0.13f;
+ curvature_threshold_ = 0.035f;
+ normalize_bins_ = true;
+ cluster_tolerance_factor_ = 3.f;
+ adaptative_MLS_ = false;
+ }
+
+ void
+ setCVFHParams (float p1, float p2, float p3)
+ {
+ eps_angle_threshold_ = p1;
+ curvature_threshold_ = p2;
+ cluster_tolerance_factor_ = p3;
+ }
+
+ void
+ setAdaptativeMLS (bool b)
+ {
+ adaptative_MLS_ = b;
+ }
+
+ void
+ setRefineClustersParam (float p4)
+ {
+ refine_factor_ = p4;
+ }
+
+ void
+ getValidTransformsVec (std::vector<bool> & valid)
+ {
+ valid = valid_roll_transforms_;
+ }
+
+ void
+ getTransformsVec (std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & trans)
+ {
+ trans = transforms_;
+ }
+
+ virtual void
+ estimate (PointInTPtr & in, PointInTPtr & processed, typename pcl::PointCloud<FeatureT>::CloudVectorType & signatures,
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > & centroids)
+ {
+
+ valid_roll_transforms_.clear ();
+ transforms_.clear ();
+
+ if (!normal_estimator_)
+ {
+ PCL_ERROR("This feature needs normals... please provide a normal estimator\n");
+ return;
+ }
+
+ pcl::MovingLeastSquares<PointInT, PointInT> mls;
+ if (adaptative_MLS_)
+ {
+ typename search::KdTree<PointInT>::Ptr tree;
+ Eigen::Vector4f centroid_cluster;
+ pcl::compute3DCentroid (*in, centroid_cluster);
+ float dist_to_sensor = centroid_cluster.norm ();
+ float sigma = dist_to_sensor * 0.01f;
+ mls.setSearchMethod (tree);
+ mls.setSearchRadius (sigma);
+ mls.setUpsamplingMethod (mls.SAMPLE_LOCAL_PLANE);
+ mls.setUpsamplingRadius (0.002);
+ mls.setUpsamplingStepSize (0.001);
+ }
+
+ normals_.reset (new pcl::PointCloud<pcl::Normal>);
+ {
+ normal_estimator_->estimate (in, processed, normals_);
+ }
+
+ if (adaptative_MLS_)
+ {
+ mls.setInputCloud (processed);
+
+ PointInTPtr filtered (new pcl::PointCloud<PointInT>);
+ mls.process (*filtered);
+
+ processed.reset (new pcl::PointCloud<PointInT>);
+ normals_.reset (new pcl::PointCloud<pcl::Normal>);
+ {
+ filtered->is_dense = false;
+ normal_estimator_->estimate (filtered, processed, normals_);
+ }
+ }
+
+ /*normals_.reset(new pcl::PointCloud<pcl::Normal>);
+ normal_estimator_->estimate (in, processed, normals_);*/
+
+ typedef typename pcl::OURCVFHEstimation<PointInT, pcl::Normal, pcl::VFHSignature308> OURCVFHEstimation;
+ pcl::PointCloud<pcl::VFHSignature308> cvfh_signatures;
+ typename pcl::search::KdTree<PointInT>::Ptr cvfh_tree (new pcl::search::KdTree<PointInT>);
+
+ OURCVFHEstimation cvfh;
+ cvfh.setSearchMethod (cvfh_tree);
+ cvfh.setInputCloud (processed);
+ cvfh.setInputNormals (normals_);
+ cvfh.setEPSAngleThreshold (eps_angle_threshold_);
+ cvfh.setCurvatureThreshold (curvature_threshold_);
+ cvfh.setNormalizeBins (normalize_bins_);
+ cvfh.setRefineClusters(refine_factor_);
+
+ float radius = normal_estimator_->normal_radius_;
+ float cluster_tolerance_radius = normal_estimator_->grid_resolution_ * cluster_tolerance_factor_;
+
+ if (normal_estimator_->compute_mesh_resolution_)
+ {
+ radius = normal_estimator_->mesh_resolution_ * normal_estimator_->factor_normals_;
+ cluster_tolerance_radius = normal_estimator_->mesh_resolution_ * cluster_tolerance_factor_;
+
+ if (normal_estimator_->do_voxel_grid_)
+ {
+ radius *= normal_estimator_->factor_voxel_grid_;
+ cluster_tolerance_radius *= normal_estimator_->factor_voxel_grid_;
+ }
+ }
+
+ cvfh.setClusterTolerance (cluster_tolerance_radius);
+ cvfh.setRadiusNormals (radius);
+ cvfh.setMinPoints (100);
+ cvfh.compute (cvfh_signatures);
+
+ //std::cout << "Res:" << normal_estimator_->mesh_resolution_ << " Radius normals:" << radius << " Cluster tolerance:" << cluster_tolerance_radius << " " << eps_angle_threshold_ << " " << curvature_threshold_ << std::endl;
+
+ for (size_t i = 0; i < cvfh_signatures.points.size (); i++)
+ {
+ pcl::PointCloud<FeatureT> vfh_signature;
+ vfh_signature.points.resize (1);
+ vfh_signature.width = vfh_signature.height = 1;
+ for (int d = 0; d < 308; ++d)
+ vfh_signature.points[0].histogram[d] = cvfh_signatures.points[i].histogram[d];
+
+ signatures.push_back (vfh_signature);
+ }
+
+ cvfh.getCentroidClusters (centroids);
+ cvfh.getTransforms(transforms_);
+ cvfh.getValidTransformsVec(valid_roll_transforms_);
+ cvfh.getClusterIndices(cluster_indices_);
+ }
+
+ bool
+ computedNormals ()
+ {
+ return true;
+ }
+
+ void
+ setNormalizeBins (bool b)
+ {
+ normalize_bins_ = b;
+ }
+ };
+ }
+}
+
+#endif /* REC_FRAMEWORK_OURCVFH_ESTIMATOR_H_ */
--- /dev/null
+/*
+ * vfh_estimator.h
+ *
+ * Created on: Mar 22, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_VFH_ESTIMATOR_H_
+#define REC_FRAMEWORK_VFH_ESTIMATOR_H_
+
+#include <pcl/apps/3d_rec_framework/feature_wrapper/global/global_estimator.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
+#include <pcl/features/vfh.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+ template<typename PointInT, typename FeatureT>
+ class VFHEstimation : public GlobalEstimator<PointInT, FeatureT>
+ {
+
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ using GlobalEstimator<PointInT, FeatureT>::normal_estimator_;
+ using GlobalEstimator<PointInT, FeatureT>::normals_;
+
+ public:
+ void
+ estimate (PointInTPtr & in, PointInTPtr & processed,
+ typename pcl::PointCloud<FeatureT>::CloudVectorType & signatures,
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > & centroids)
+ {
+
+ if (!normal_estimator_)
+ {
+ PCL_ERROR("This feature needs normals... please provide a normal estimator\n");
+ return;
+ }
+
+ normals_.reset(new pcl::PointCloud<pcl::Normal>);
+ normal_estimator_->estimate (in, processed, normals_);
+
+ typedef typename pcl::VFHEstimation<PointInT, pcl::Normal, FeatureT> VFHEstimation;
+ pcl::PointCloud<FeatureT> vfh_signature;
+
+ VFHEstimation vfh;
+ typename pcl::search::KdTree<PointInT>::Ptr vfh_tree (new pcl::search::KdTree<PointInT>);
+ vfh.setSearchMethod (vfh_tree);
+ vfh.setInputCloud (processed);
+ vfh.setInputNormals (normals_);
+ vfh.setNormalizeBins (true);
+ vfh.setNormalizeDistance (true);
+ vfh.compute (vfh_signature);
+
+ signatures.resize (1);
+ centroids.resize (1);
+
+ signatures[0] = vfh_signature;
+
+ Eigen::Vector4f centroid4f;
+ pcl::compute3DCentroid (*in, centroid4f);
+ centroids[0] = Eigen::Vector3f (centroid4f[0], centroid4f[1], centroid4f[2]);
+
+ }
+
+ bool
+ computedNormals ()
+ {
+ return true;
+ }
+ };
+ }
+}
+
+#endif /* REC_FRAMEWORK_VFH_ESTIMATOR_H_ */
--- /dev/null
+/*
+ * shot_local_estimator.h
+ *
+ * Created on: Mar 24, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_COLORSHOT_LOCAL_ESTIMATOR_H_
+#define REC_FRAMEWORK_COLORSHOT_LOCAL_ESTIMATOR_H_
+
+#include <pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
+#include <pcl/features/shot.h>
+#include <pcl/io/pcd_io.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+ template<typename PointInT, typename FeatureT>
+ class ColorSHOTLocalEstimation : public LocalEstimator<PointInT, FeatureT>
+ {
+
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ typedef typename pcl::PointCloud<FeatureT>::Ptr FeatureTPtr;
+
+ using LocalEstimator<PointInT, FeatureT>::support_radius_;
+ using LocalEstimator<PointInT, FeatureT>::normal_estimator_;
+ using LocalEstimator<PointInT, FeatureT>::keypoint_extractor_;
+
+ public:
+ bool
+ estimate (PointInTPtr & in, PointInTPtr & processed, PointInTPtr & keypoints, FeatureTPtr & signatures)
+ {
+
+ if (!normal_estimator_)
+ {
+ PCL_ERROR("SHOTLocalEstimation :: This feature needs normals... please provide a normal estimator\n");
+ return false;
+ }
+
+ if (keypoint_extractor_.size() == 0)
+ {
+ PCL_ERROR("SHOTLocalEstimation :: This feature needs a keypoint extractor... please provide one\n");
+ return false;
+ }
+
+ pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+ normals.reset (new pcl::PointCloud<pcl::Normal>);
+ {
+ pcl::ScopeTime t ("Compute normals");
+ normal_estimator_->estimate (in, processed, normals);
+ }
+
+ //compute keypoints
+ computeKeypoints(processed, keypoints, normals);
+
+ if (keypoints->points.size () == 0)
+ {
+ PCL_WARN("ColorSHOTLocalEstimation :: No keypoints were found\n");
+ return false;
+ }
+
+ //compute signatures
+ typedef typename pcl::SHOTColorEstimation<PointInT, pcl::Normal, pcl::SHOT1344> SHOTEstimator;
+ typename pcl::search::KdTree<PointInT>::Ptr tree (new pcl::search::KdTree<PointInT>);
+
+ pcl::PointCloud<pcl::SHOT1344>::Ptr shots (new pcl::PointCloud<pcl::SHOT1344>);
+ SHOTEstimator shot_estimate;
+ shot_estimate.setSearchMethod (tree);
+ shot_estimate.setInputNormals (normals);
+ shot_estimate.setInputCloud (keypoints);
+ shot_estimate.setSearchSurface(processed);
+ shot_estimate.setRadiusSearch (support_radius_);
+ shot_estimate.compute (*shots);
+ signatures->resize (shots->points.size ());
+ signatures->width = static_cast<int> (shots->points.size ());
+ signatures->height = 1;
+
+ int size_feat = sizeof(signatures->points[0].histogram) / sizeof(float);
+
+ for (size_t k = 0; k < shots->points.size (); k++)
+ for (int i = 0; i < size_feat; i++)
+ signatures->points[k].histogram[i] = shots->points[k].descriptor[i];
+
+ return true;
+
+ }
+
+ };
+ }
+}
+
+#endif /* REC_FRAMEWORK_COLORSHOT_LOCAL_ESTIMATOR_H_ */
--- /dev/null
+/*
+ * shot_local_estimator.h
+ *
+ * Created on: Mar 24, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_FPFH_LOCAL_ESTIMATOR_H_
+#define REC_FRAMEWORK_FPFH_LOCAL_ESTIMATOR_H_
+
+#include <pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
+#include <pcl/features/fpfh.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+ template<typename PointInT, typename FeatureT>
+ class FPFHLocalEstimation : public LocalEstimator<PointInT, FeatureT>
+ {
+
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ typedef typename pcl::PointCloud<FeatureT>::Ptr FeatureTPtr;
+ typedef pcl::PointCloud<pcl::PointXYZ> KeypointCloud;
+
+ using LocalEstimator<PointInT, FeatureT>::support_radius_;
+ using LocalEstimator<PointInT, FeatureT>::normal_estimator_;
+ using LocalEstimator<PointInT, FeatureT>::keypoint_extractor_;
+
+ public:
+ bool
+ estimate (PointInTPtr & in, PointInTPtr & processed, PointInTPtr & keypoints, FeatureTPtr & signatures)
+ {
+
+ if (!normal_estimator_)
+ {
+ PCL_ERROR("FPFHLocalEstimation :: This feature needs normals... please provide a normal estimator\n");
+ return false;
+ }
+
+ if (keypoint_extractor_.size() == 0)
+ {
+ PCL_ERROR("FPFHLocalEstimation :: This feature needs a keypoint extractor... please provide one\n");
+ return false;
+ }
+
+ //compute normals
+ pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+ normal_estimator_->estimate (in, processed, normals);
+
+ this->computeKeypoints(processed, keypoints, normals);
+ std::cout << " " << normals->points.size() << " " << processed->points.size() << std::endl;
+
+ if (keypoints->points.size () == 0)
+ {
+ PCL_WARN("FPFHLocalEstimation :: No keypoints were found\n");
+ return false;
+ }
+
+ assert (processed->points.size () == normals->points.size ());
+
+ //compute signatures
+ typedef typename pcl::FPFHEstimation<PointInT, pcl::Normal, pcl::FPFHSignature33> FPFHEstimator;
+ typename pcl::search::KdTree<PointInT>::Ptr tree (new pcl::search::KdTree<PointInT>);
+
+ pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs (new pcl::PointCloud<pcl::FPFHSignature33>);
+ FPFHEstimator fpfh_estimate;
+ fpfh_estimate.setSearchMethod (tree);
+ fpfh_estimate.setInputCloud (keypoints);
+ fpfh_estimate.setSearchSurface(processed);
+ fpfh_estimate.setInputNormals (normals);
+ fpfh_estimate.setRadiusSearch (support_radius_);
+ fpfh_estimate.compute (*fpfhs);
+
+ signatures->resize (fpfhs->points.size ());
+ signatures->width = static_cast<int> (fpfhs->points.size ());
+ signatures->height = 1;
+
+ int size_feat = 33;
+ for (size_t k = 0; k < fpfhs->points.size (); k++)
+ for (int i = 0; i < size_feat; i++)
+ signatures->points[k].histogram[i] = fpfhs->points[k].histogram[i];
+
+ return true;
+
+ }
+
+ };
+ }
+}
+
+#endif /* REC_FRAMEWORK_FPFH_LOCAL_ESTIMATOR_H_ */
--- /dev/null
+/*
+ * shot_local_estimator.h
+ *
+ * Created on: Mar 24, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_FPFH_LOCAL_ESTIMATOR_OMP_H_
+#define REC_FRAMEWORK_FPFH_LOCAL_ESTIMATOR_OMP_H_
+
+#include <pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
+#include <pcl/features/fpfh_omp.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+ template<typename PointInT, typename FeatureT>
+ class FPFHLocalEstimationOMP : public LocalEstimator<PointInT, FeatureT>
+ {
+
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ typedef typename pcl::PointCloud<FeatureT>::Ptr FeatureTPtr;
+ typedef pcl::PointCloud<pcl::PointXYZ> KeypointCloud;
+
+ using LocalEstimator<PointInT, FeatureT>::support_radius_;
+ using LocalEstimator<PointInT, FeatureT>::normal_estimator_;
+ using LocalEstimator<PointInT, FeatureT>::keypoint_extractor_;
+
+ public:
+ bool
+ estimate (PointInTPtr & in, PointInTPtr & processed, PointInTPtr & keypoints, FeatureTPtr & signatures)
+ {
+
+ if (!normal_estimator_)
+ {
+ PCL_ERROR("FPFHLocalEstimation :: This feature needs normals... please provide a normal estimator\n");
+ return false;
+ }
+
+ if (keypoint_extractor_.size() == 0)
+ {
+ PCL_ERROR("FPFHLocalEstimation :: This feature needs a keypoint extractor... please provide one\n");
+ return false;
+ }
+
+ //compute normals
+ pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+ normal_estimator_->estimate (in, processed, normals);
+
+ //compute keypoints
+ computeKeypoints(processed, keypoints, normals);
+ std::cout << " " << normals->points.size() << " " << processed->points.size() << std::endl;
+
+ if (keypoints->points.size () == 0)
+ {
+ PCL_WARN("FPFHLocalEstimation :: No keypoints were found\n");
+ return false;
+ }
+
+ assert (processed->points.size () == normals->points.size ());
+
+ //compute signatures
+ typedef typename pcl::FPFHEstimationOMP<PointInT, pcl::Normal, pcl::FPFHSignature33> FPFHEstimator;
+ typename pcl::search::KdTree<PointInT>::Ptr tree (new pcl::search::KdTree<PointInT>);
+
+ pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs (new pcl::PointCloud<pcl::FPFHSignature33>);
+ FPFHEstimator fpfh_estimate;
+ fpfh_estimate.setNumberOfThreads(8);
+ fpfh_estimate.setSearchMethod (tree);
+ fpfh_estimate.setInputCloud (keypoints);
+ fpfh_estimate.setSearchSurface(processed);
+ fpfh_estimate.setInputNormals (normals);
+ fpfh_estimate.setRadiusSearch (support_radius_);
+ fpfh_estimate.compute (*fpfhs);
+
+ signatures->resize (fpfhs->points.size ());
+ signatures->width = static_cast<int> (fpfhs->points.size ());
+ signatures->height = 1;
+
+ int size_feat = 33;
+ for (size_t k = 0; k < fpfhs->points.size (); k++)
+ for (int i = 0; i < size_feat; i++)
+ signatures->points[k].histogram[i] = fpfhs->points[k].histogram[i];
+
+ return true;
+
+ }
+
+ };
+ }
+}
+
+#endif /* REC_FRAMEWORK_FPFH_LOCAL_ESTIMATOR_H_ */
--- /dev/null
+/*
+ * estimator.h
+ *
+ * Created on: Mar 22, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_LOCAL_ESTIMATOR_H_
+#define REC_FRAMEWORK_LOCAL_ESTIMATOR_H_
+
+#include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
+#include <pcl/filters/uniform_sampling.h>
+#include <pcl/surface/mls.h>
+#include <pcl/keypoints/harris_3d.h>
+#include <pcl/keypoints/sift_keypoint.h>
+#include <pcl/keypoints/susan.h>
+
+namespace pcl
+{
+ template<>
+ struct SIFTKeypointFieldSelector<PointXYZ>
+ {
+ inline float
+ operator () (const PointXYZ & p) const
+ {
+ return p.z;
+ }
+ };
+}
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+
+ template<typename PointInT>
+ class KeypointExtractor
+ {
+ protected:
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointOutTPtr;
+ typename pcl::PointCloud<PointInT>::Ptr input_;
+ float radius_;
+
+ public:
+ void
+ setInputCloud (PointInTPtr & input)
+ {
+ input_ = input;
+ }
+
+ void
+ setSupportRadius (float f)
+ {
+ radius_ = f;
+ }
+
+ virtual void
+ compute (PointOutTPtr & keypoints) = 0;
+
+ virtual void
+ setNormals (const pcl::PointCloud<pcl::Normal>::Ptr & /*normals*/)
+ {
+
+ }
+
+ virtual bool
+ needNormals ()
+ {
+ return false;
+ }
+
+ };
+
+ template<typename PointInT>
+ class UniformSamplingExtractor : public KeypointExtractor<PointInT>
+ {
+ private:
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ bool filter_planar_;
+ using KeypointExtractor<PointInT>::input_;
+ using KeypointExtractor<PointInT>::radius_;
+ float sampling_density_;
+ boost::shared_ptr<std::vector<std::vector<int> > > neighborhood_indices_;
+ boost::shared_ptr<std::vector<std::vector<float> > > neighborhood_dist_;
+
+ void
+ filterPlanar (PointInTPtr & input, PointInTPtr & keypoints_cloud)
+ {
+ pcl::PointCloud<int> filtered_keypoints;
+ //create a search object
+ typename pcl::search::Search<PointInT>::Ptr tree;
+ if (input->isOrganized ())
+ tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
+ else
+ tree.reset (new pcl::search::KdTree<PointInT> (false));
+ tree->setInputCloud (input);
+
+ neighborhood_indices_.reset (new std::vector<std::vector<int> >);
+ neighborhood_indices_->resize (keypoints_cloud->points.size ());
+ neighborhood_dist_.reset (new std::vector<std::vector<float> >);
+ neighborhood_dist_->resize (keypoints_cloud->points.size ());
+
+ filtered_keypoints.points.resize (keypoints_cloud->points.size ());
+ int good = 0;
+
+ for (size_t i = 0; i < keypoints_cloud->points.size (); i++)
+ {
+
+ if (tree->radiusSearch (keypoints_cloud->points[i], radius_, (*neighborhood_indices_)[good], (*neighborhood_dist_)[good]))
+ {
+
+ EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
+ Eigen::Vector4f xyz_centroid;
+ EIGEN_ALIGN16 Eigen::Vector3f eigenValues;
+ EIGEN_ALIGN16 Eigen::Matrix3f eigenVectors;
+
+ //compute planarity of the region
+ computeMeanAndCovarianceMatrix (*input, (*neighborhood_indices_)[good], covariance_matrix, xyz_centroid);
+ pcl::eigen33 (covariance_matrix, eigenVectors, eigenValues);
+
+ float eigsum = eigenValues.sum ();
+ if (!pcl_isfinite(eigsum))
+ {
+ PCL_ERROR("Eigen sum is not finite\n");
+ }
+
+ if ((fabs (eigenValues[0] - eigenValues[1]) < 1.5e-4) || (eigsum != 0 && fabs (eigenValues[0] / eigsum) > 1.e-2))
+ {
+ //region is not planar, add to filtered keypoint
+ keypoints_cloud->points[good] = keypoints_cloud->points[i];
+ good++;
+ }
+ }
+ }
+
+ neighborhood_indices_->resize (good);
+ neighborhood_dist_->resize (good);
+ keypoints_cloud->points.resize (good);
+
+ neighborhood_indices_->clear ();
+ neighborhood_dist_->clear ();
+
+ }
+
+ public:
+
+ void
+ setFilterPlanar (bool b)
+ {
+ filter_planar_ = b;
+ }
+
+ void
+ setSamplingDensity (float f)
+ {
+ sampling_density_ = f;
+ }
+
+ void
+ compute (PointInTPtr & keypoints)
+ {
+ keypoints.reset (new pcl::PointCloud<PointInT>);
+
+ pcl::UniformSampling<PointInT> keypoint_extractor;
+ keypoint_extractor.setRadiusSearch (sampling_density_);
+ keypoint_extractor.setInputCloud (input_);
+
+ keypoint_extractor.filter (*keypoints);
+
+ if (filter_planar_)
+ filterPlanar (input_, keypoints);
+ }
+ };
+
+ template<typename PointInT>
+ class SIFTKeypointExtractor : public KeypointExtractor<PointInT>
+ {
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ using KeypointExtractor<PointInT>::input_;
+ using KeypointExtractor<PointInT>::radius_;
+
+ public:
+ void
+ compute (PointInTPtr & keypoints)
+ {
+ keypoints.reset (new pcl::PointCloud<PointInT>);
+
+ typename pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_keypoints (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::SIFTKeypoint<PointInT, pcl::PointXYZI> sift3D;
+ sift3D.setScales (0.003f, 3, 2);
+ sift3D.setMinimumContrast (0.1f);
+ sift3D.setInputCloud (input_);
+ sift3D.setSearchSurface (input_);
+ sift3D.compute (*intensity_keypoints);
+ pcl::copyPointCloud (*intensity_keypoints, *keypoints);
+ }
+ };
+
+ template<typename PointInT>
+ class SIFTSurfaceKeypointExtractor : public KeypointExtractor<PointInT>
+ {
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ pcl::PointCloud<pcl::Normal>::Ptr normals_;
+ using KeypointExtractor<PointInT>::input_;
+ using KeypointExtractor<PointInT>::radius_;
+
+ bool
+ needNormals ()
+ {
+ return true;
+ }
+
+ void
+ setNormals (const pcl::PointCloud<pcl::Normal>::Ptr & normals)
+ {
+ normals_ = normals;
+ }
+
+ public:
+ void
+ compute (PointInTPtr & keypoints)
+ {
+ if (normals_ == 0 || (normals_->points.size () != input_->points.size ()))
+ PCL_WARN("SIFTSurfaceKeypointExtractor -- Normals are not valid\n");
+
+ keypoints.reset (new pcl::PointCloud<PointInT>);
+
+ typename pcl::PointCloud<pcl::PointNormal>::Ptr input_cloud (new pcl::PointCloud<pcl::PointNormal>);
+ input_cloud->width = input_->width;
+ input_cloud->height = input_->height;
+ input_cloud->points.resize (input_->width * input_->height);
+ for (size_t i = 0; i < input_->points.size (); i++)
+ {
+ input_cloud->points[i].getVector3fMap () = input_->points[i].getVector3fMap ();
+ input_cloud->points[i].getNormalVector3fMap () = normals_->points[i].getNormalVector3fMap ();
+ }
+
+ typename pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_keypoints (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::SIFTKeypoint<pcl::PointNormal, pcl::PointXYZI> sift3D;
+ sift3D.setScales (0.003f, 3, 2);
+ sift3D.setMinimumContrast (0.0);
+ sift3D.setInputCloud (input_cloud);
+ sift3D.setSearchSurface (input_cloud);
+ sift3D.compute (*intensity_keypoints);
+ pcl::copyPointCloud (*intensity_keypoints, *keypoints);
+ }
+ };
+
+ template<typename PointInT, typename NormalT = pcl::Normal>
+ class HarrisKeypointExtractor : public KeypointExtractor<PointInT>
+ {
+
+ pcl::PointCloud<pcl::Normal>::Ptr normals_;
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ using KeypointExtractor<PointInT>::input_;
+ using KeypointExtractor<PointInT>::radius_;
+ typename pcl::HarrisKeypoint3D<PointInT, pcl::PointXYZI>::ResponseMethod m_;
+ float non_max_radius_;
+ float threshold_;
+
+ public:
+
+ HarrisKeypointExtractor ()
+ {
+ m_ = pcl::HarrisKeypoint3D<PointInT, pcl::PointXYZI>::HARRIS;
+ non_max_radius_ = 0.01f;
+ threshold_ = 0.f;
+ }
+
+ bool
+ needNormals ()
+ {
+ return true;
+ }
+
+ void
+ setNormals (const pcl::PointCloud<pcl::Normal>::Ptr & normals)
+ {
+ normals_ = normals;
+ }
+
+ void
+ setThreshold(float t) {
+ threshold_ = t;
+ }
+
+ void
+ setResponseMethod (typename pcl::HarrisKeypoint3D<PointInT, pcl::PointXYZI>::ResponseMethod m)
+ {
+ m_ = m;
+ }
+
+ void
+ setNonMaximaRadius(float r) {
+ non_max_radius_ = r;
+ }
+
+ void
+ compute (PointInTPtr & keypoints)
+ {
+ keypoints.reset (new pcl::PointCloud<PointInT>);
+
+ if (normals_ == 0 || (normals_->points.size () != input_->points.size ()))
+ PCL_WARN("HarrisKeypointExtractor -- Normals are not valid\n");
+
+ typename pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_keypoints (new pcl::PointCloud<pcl::PointXYZI>);
+
+ pcl::HarrisKeypoint3D<PointInT, pcl::PointXYZI> harris;
+ harris.setNonMaxSupression (true);
+ harris.setRefine (false);
+ harris.setThreshold (threshold_);
+ harris.setInputCloud (input_);
+ harris.setNormals (normals_);
+ harris.setRadius (non_max_radius_);
+ harris.setRadiusSearch (non_max_radius_);
+ harris.setMethod (m_);
+ harris.compute (*intensity_keypoints);
+
+ pcl::copyPointCloud (*intensity_keypoints, *keypoints);
+ }
+ };
+
+ template<typename PointInT, typename NormalT = pcl::Normal>
+ class SUSANKeypointExtractor : public KeypointExtractor<PointInT>
+ {
+
+ pcl::PointCloud<pcl::Normal>::Ptr normals_;
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ using KeypointExtractor<PointInT>::input_;
+ using KeypointExtractor<PointInT>::radius_;
+
+ public:
+
+ SUSANKeypointExtractor ()
+ {
+
+ }
+
+ bool
+ needNormals ()
+ {
+ return true;
+ }
+
+ void
+ setNormals (const pcl::PointCloud<pcl::Normal>::Ptr & normals)
+ {
+ normals_ = normals;
+ }
+
+ void
+ compute (PointInTPtr & keypoints)
+ {
+ keypoints.reset (new pcl::PointCloud<PointInT>);
+
+ if (normals_ == 0 || (normals_->points.size () != input_->points.size ()))
+ PCL_WARN("SUSANKeypointExtractor -- Normals are not valid\n");
+
+ typename pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_keypoints (new pcl::PointCloud<pcl::PointXYZI>);
+
+ pcl::SUSANKeypoint<PointInT, pcl::PointXYZI> susan;
+ susan.setNonMaxSupression (true);
+ susan.setInputCloud (input_);
+ susan.setNormals (normals_);
+ susan.setRadius (0.01f);
+ susan.setRadiusSearch (0.01f);
+ susan.compute (*intensity_keypoints);
+
+ pcl::copyPointCloud (*intensity_keypoints, *keypoints);
+ }
+ };
+
+ template<typename PointInT, typename FeatureT>
+ class LocalEstimator
+ {
+ protected:
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ typedef typename pcl::PointCloud<FeatureT>::Ptr FeatureTPtr;
+
+ typename boost::shared_ptr<PreProcessorAndNormalEstimator<PointInT, pcl::Normal> > normal_estimator_;
+ //typename boost::shared_ptr<UniformSampling<PointInT> > keypoint_extractor_;
+ std::vector<typename boost::shared_ptr<KeypointExtractor<PointInT> > > keypoint_extractor_; //this should be a vector
+ float support_radius_;
+ //bool filter_planar_;
+
+ bool adaptative_MLS_;
+
+ boost::shared_ptr<std::vector<std::vector<int> > > neighborhood_indices_;
+ boost::shared_ptr<std::vector<std::vector<float> > > neighborhood_dist_;
+
+ //std::vector< std::vector<int> > neighborhood_indices_;
+ //std::vector< std::vector<float> > neighborhood_dist_;
+
+ void
+ computeKeypoints (PointInTPtr & cloud, PointInTPtr & keypoints, pcl::PointCloud<pcl::Normal>::Ptr & normals)
+ {
+ keypoints.reset (new pcl::PointCloud<PointInT>);
+ for (size_t i = 0; i < keypoint_extractor_.size (); i++)
+ {
+ keypoint_extractor_[i]->setInputCloud (cloud);
+ if (keypoint_extractor_[i]->needNormals ())
+ keypoint_extractor_[i]->setNormals (normals);
+
+ keypoint_extractor_[i]->setSupportRadius (support_radius_);
+
+ PointInTPtr detected_keypoints;
+ keypoint_extractor_[i]->compute (detected_keypoints);
+ *keypoints += *detected_keypoints;
+ }
+ }
+
+ public:
+
+ LocalEstimator ()
+ {
+ adaptative_MLS_ = false;
+ keypoint_extractor_.clear ();
+ }
+
+ void
+ setAdaptativeMLS (bool b)
+ {
+ adaptative_MLS_ = b;
+ }
+
+ virtual bool
+ estimate (PointInTPtr & in, PointInTPtr & processed, PointInTPtr & keypoints, FeatureTPtr & signatures)=0;
+
+ void
+ setNormalEstimator (boost::shared_ptr<PreProcessorAndNormalEstimator<PointInT, pcl::Normal> > & ne)
+ {
+ normal_estimator_ = ne;
+ }
+
+ /**
+ * \brief Right now only uniformSampling keypoint extractor is allowed
+ */
+ void
+ addKeypointExtractor (boost::shared_ptr<KeypointExtractor<PointInT> > & ke)
+ {
+ keypoint_extractor_.push_back (ke);
+ }
+
+ void
+ setKeypointExtractors (std::vector<typename boost::shared_ptr<KeypointExtractor<PointInT> > > & ke)
+ {
+ keypoint_extractor_ = ke;
+ }
+
+ void
+ setSupportRadius (float r)
+ {
+ support_radius_ = r;
+ }
+
+ /*void
+ setFilterPlanar (bool b)
+ {
+ filter_planar_ = b;
+ }
+
+ void
+ filterPlanar (PointInTPtr & input, KeypointCloud & keypoints_cloud)
+ {
+ pcl::PointCloud<int> filtered_keypoints;
+ //create a search object
+ typename pcl::search::Search<PointInT>::Ptr tree;
+ if (input->isOrganized ())
+ tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
+ else
+ tree.reset (new pcl::search::KdTree<PointInT> (false));
+ tree->setInputCloud (input);
+
+ //std::vector<int> nn_indices;
+ //std::vector<float> nn_distances;
+
+ neighborhood_indices_.reset (new std::vector<std::vector<int> >);
+ neighborhood_indices_->resize (keypoints_cloud.points.size ());
+ neighborhood_dist_.reset (new std::vector<std::vector<float> >);
+ neighborhood_dist_->resize (keypoints_cloud.points.size ());
+
+ filtered_keypoints.points.resize (keypoints_cloud.points.size ());
+ int good = 0;
+
+ //#pragma omp parallel for num_threads(8)
+ for (size_t i = 0; i < keypoints_cloud.points.size (); i++)
+ {
+
+ if (tree->radiusSearch (keypoints_cloud[i], support_radius_, (*neighborhood_indices_)[good], (*neighborhood_dist_)[good]))
+ {
+
+ EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
+ Eigen::Vector4f xyz_centroid;
+ EIGEN_ALIGN16 Eigen::Vector3f eigenValues;
+ EIGEN_ALIGN16 Eigen::Matrix3f eigenVectors;
+
+ //compute planarity of the region
+ computeMeanAndCovarianceMatrix (*input, (*neighborhood_indices_)[good], covariance_matrix, xyz_centroid);
+ pcl::eigen33 (covariance_matrix, eigenVectors, eigenValues);
+
+ float eigsum = eigenValues.sum ();
+ if (!pcl_isfinite(eigsum))
+ {
+ PCL_ERROR("Eigen sum is not finite\n");
+ }
+
+ if ((fabs (eigenValues[0] - eigenValues[1]) < 1.5e-4) || (eigsum != 0 && fabs (eigenValues[0] / eigsum) > 1.e-2))
+ {
+ //region is not planar, add to filtered keypoint
+ keypoints_cloud.points[good] = keypoints_cloud.points[i];
+ good++;
+ }
+ }
+ }
+
+ neighborhood_indices_->resize (good);
+ neighborhood_dist_->resize (good);
+ keypoints_cloud.points.resize (good);
+ }*/
+
+ };
+ }
+}
+
+#endif /* REC_FRAMEWORK_LOCAL_ESTIMATOR_H_ */
--- /dev/null
+/*
+ * shot_local_estimator.h
+ *
+ * Created on: Mar 24, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_SHOT_LOCAL_ESTIMATOR_H_
+#define REC_FRAMEWORK_SHOT_LOCAL_ESTIMATOR_H_
+
+#include <pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
+#include <pcl/features/shot.h>
+#include <pcl/io/pcd_io.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+ template<typename PointInT, typename FeatureT>
+ class SHOTLocalEstimation : public LocalEstimator<PointInT, FeatureT>
+ {
+
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ typedef typename pcl::PointCloud<FeatureT>::Ptr FeatureTPtr;
+
+ using LocalEstimator<PointInT, FeatureT>::support_radius_;
+ using LocalEstimator<PointInT, FeatureT>::normal_estimator_;
+ using LocalEstimator<PointInT, FeatureT>::keypoint_extractor_;
+ using LocalEstimator<PointInT, FeatureT>::adaptative_MLS_;
+
+ public:
+ bool
+ estimate (PointInTPtr & in, PointInTPtr & processed, PointInTPtr & keypoints, FeatureTPtr & signatures)
+ {
+
+ if (!normal_estimator_)
+ {
+ PCL_ERROR("SHOTLocalEstimation :: This feature needs normals... please provide a normal estimator\n");
+ return false;
+ }
+
+ if (keypoint_extractor_.size() == 0)
+ {
+ PCL_ERROR("SHOTLocalEstimation :: This feature needs a keypoint extractor... please provide one\n");
+ return false;
+ }
+
+ pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+ pcl::MovingLeastSquares<PointInT, PointInT> mls;
+ if(adaptative_MLS_) {
+ typename search::KdTree<PointInT>::Ptr tree;
+ Eigen::Vector4f centroid_cluster;
+ pcl::compute3DCentroid (*in, centroid_cluster);
+ float dist_to_sensor = centroid_cluster.norm();
+ float sigma = dist_to_sensor * 0.01f;
+ mls.setSearchMethod(tree);
+ mls.setSearchRadius (sigma);
+ mls.setUpsamplingMethod (mls.SAMPLE_LOCAL_PLANE);
+ mls.setUpsamplingRadius (0.002);
+ mls.setUpsamplingStepSize (0.001);
+ }
+
+ normals.reset (new pcl::PointCloud<pcl::Normal>);
+ {
+ pcl::ScopeTime t ("Compute normals");
+ normal_estimator_->estimate (in, processed, normals);
+ }
+
+ if(adaptative_MLS_) {
+ mls.setInputCloud(processed);
+
+ PointInTPtr filtered(new pcl::PointCloud<PointInT>);
+ mls.process(*filtered);
+
+ processed.reset(new pcl::PointCloud<PointInT>);
+ normals.reset (new pcl::PointCloud<pcl::Normal>);
+ {
+ pcl::ScopeTime t ("Compute normals after MLS");
+ filtered->is_dense = false;
+ normal_estimator_->estimate (filtered, processed, normals);
+ }
+ }
+
+ //compute normals
+ //pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+ //normal_estimator_->estimate (in, processed, normals);
+
+ //compute keypoints
+ this->computeKeypoints(processed, keypoints, normals);
+ std::cout << " " << normals->points.size() << " " << processed->points.size() << std::endl;
+
+ //compute keypoints
+ /*keypoint_extractor_->setInputCloud (processed);
+ if(keypoint_extractor_->needNormals())
+ keypoint_extractor_->setNormals(normals);
+
+ std::cout << " " << normals->points.size() << " " << processed->points.size() << std::endl;
+
+ keypoint_extractor_->setSupportRadius(support_radius_);
+ keypoint_extractor_->compute (keypoints);*/
+
+ if (keypoints->points.size () == 0)
+ {
+ PCL_WARN("SHOTLocalEstimation :: No keypoints were found\n");
+ return false;
+ }
+
+ std::cout << keypoints->points.size() << " " << normals->points.size() << " " << processed->points.size() << std::endl;
+ //compute signatures
+ typedef typename pcl::SHOTEstimation<PointInT, pcl::Normal, pcl::SHOT352> SHOTEstimator;
+ typename pcl::search::KdTree<PointInT>::Ptr tree (new pcl::search::KdTree<PointInT>);
+
+ pcl::PointCloud<pcl::SHOT352>::Ptr shots (new pcl::PointCloud<pcl::SHOT352>);
+
+ SHOTEstimator shot_estimate;
+ shot_estimate.setSearchMethod (tree);
+ shot_estimate.setInputCloud (keypoints);
+ shot_estimate.setSearchSurface(processed);
+ shot_estimate.setInputNormals (normals);
+ shot_estimate.setRadiusSearch (support_radius_);
+ shot_estimate.compute (*shots);
+ signatures->resize (shots->points.size ());
+ signatures->width = static_cast<int> (shots->points.size ());
+ signatures->height = 1;
+
+ int size_feat = sizeof(signatures->points[0].histogram) / sizeof(float);
+
+ for (size_t k = 0; k < shots->points.size (); k++)
+ for (int i = 0; i < size_feat; i++)
+ signatures->points[k].histogram[i] = shots->points[k].descriptor[i];
+
+ return true;
+
+ }
+
+ private:
+
+
+ };
+ }
+}
+
+#endif /* REC_FRAMEWORK_SHOT_LOCAL_ESTIMATOR_H_ */
--- /dev/null
+/*
+ * shot_local_estimator.h
+ *
+ * Created on: Mar 24, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_SHOT_LOCAL_ESTIMATOR_OMP_H_
+#define REC_FRAMEWORK_SHOT_LOCAL_ESTIMATOR_OMP_H_
+
+#include <pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
+#include <pcl/features/shot_omp.h>
+#include <pcl/io/pcd_io.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+ template<typename PointInT, typename FeatureT>
+ class SHOTLocalEstimationOMP : public LocalEstimator<PointInT, FeatureT>
+ {
+
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ typedef typename pcl::PointCloud<FeatureT>::Ptr FeatureTPtr;
+ typedef pcl::PointCloud<pcl::PointXYZ> KeypointCloud;
+
+ using LocalEstimator<PointInT, FeatureT>::support_radius_;
+ using LocalEstimator<PointInT, FeatureT>::normal_estimator_;
+ using LocalEstimator<PointInT, FeatureT>::keypoint_extractor_;
+ using LocalEstimator<PointInT, FeatureT>::neighborhood_indices_;
+ using LocalEstimator<PointInT, FeatureT>::neighborhood_dist_;
+ using LocalEstimator<PointInT, FeatureT>::adaptative_MLS_;
+
+ public:
+ bool
+ estimate (PointInTPtr & in, PointInTPtr & processed, PointInTPtr & keypoints, FeatureTPtr & signatures)
+ {
+ if (!normal_estimator_)
+ {
+ PCL_ERROR("SHOTLocalEstimationOMP :: This feature needs normals... please provide a normal estimator\n");
+ return false;
+ }
+
+ if (keypoint_extractor_.size() == 0)
+ {
+ PCL_ERROR("SHOTLocalEstimationOMP :: This feature needs a keypoint extractor... please provide one\n");
+ return false;
+ }
+
+ pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+ pcl::MovingLeastSquares<PointInT, PointInT> mls;
+ if (adaptative_MLS_)
+ {
+ typename search::KdTree<PointInT>::Ptr tree;
+ Eigen::Vector4f centroid_cluster;
+ pcl::compute3DCentroid (*in, centroid_cluster);
+ float dist_to_sensor = centroid_cluster.norm ();
+ float sigma = dist_to_sensor * 0.01f;
+ mls.setSearchMethod (tree);
+ mls.setSearchRadius (sigma);
+ mls.setUpsamplingMethod (mls.SAMPLE_LOCAL_PLANE);
+ mls.setUpsamplingRadius (0.002);
+ mls.setUpsamplingStepSize (0.001);
+ }
+
+ normals.reset (new pcl::PointCloud<pcl::Normal>);
+ {
+ pcl::ScopeTime t ("Compute normals");
+ normal_estimator_->estimate (in, processed, normals);
+ }
+
+ if (adaptative_MLS_)
+ {
+ mls.setInputCloud (processed);
+
+ PointInTPtr filtered (new pcl::PointCloud<PointInT>);
+ mls.process (*filtered);
+
+ processed.reset (new pcl::PointCloud<PointInT>);
+ normals.reset (new pcl::PointCloud<pcl::Normal>);
+ {
+ pcl::ScopeTime t ("Compute normals after MLS");
+ filtered->is_dense = false;
+ normal_estimator_->estimate (filtered, processed, normals);
+ }
+ }
+
+ this->computeKeypoints(processed, keypoints, normals);
+ std::cout << " " << normals->points.size() << " " << processed->points.size() << std::endl;
+
+ if (keypoints->points.size () == 0)
+ {
+ PCL_WARN("SHOTLocalEstimationOMP :: No keypoints were found\n");
+ return false;
+ }
+
+ //compute signatures
+ typedef typename pcl::SHOTEstimationOMP<PointInT, pcl::Normal, pcl::SHOT352> SHOTEstimator;
+ typename pcl::search::KdTree<PointInT>::Ptr tree (new pcl::search::KdTree<PointInT>);
+ tree->setInputCloud (processed);
+
+ pcl::PointCloud<pcl::SHOT352>::Ptr shots (new pcl::PointCloud<pcl::SHOT352>);
+ SHOTEstimator shot_estimate;
+ shot_estimate.setNumberOfThreads (8);
+ shot_estimate.setSearchMethod (tree);
+ shot_estimate.setInputCloud (keypoints);
+ shot_estimate.setSearchSurface(processed);
+ shot_estimate.setInputNormals (normals);
+ shot_estimate.setRadiusSearch (support_radius_);
+
+ {
+ pcl::ScopeTime t ("Compute SHOT");
+ shot_estimate.compute (*shots);
+ }
+
+ signatures->resize (shots->points.size ());
+ signatures->width = static_cast<int> (shots->points.size ());
+ signatures->height = 1;
+
+ int size_feat = sizeof(signatures->points[0].histogram) / sizeof(float);
+
+ int good = 0;
+ for (size_t k = 0; k < shots->points.size (); k++)
+ {
+
+ int NaNs = 0;
+ for (int i = 0; i < size_feat; i++)
+ {
+ if (!pcl_isfinite(shots->points[k].descriptor[i]))
+ NaNs++;
+ }
+
+ if (NaNs == 0)
+ {
+ for (int i = 0; i < size_feat; i++)
+ {
+ signatures->points[good].histogram[i] = shots->points[k].descriptor[i];
+ }
+
+ good++;
+ }
+ }
+
+ signatures->resize (good);
+ signatures->width = good;
+
+ return true;
+
+ }
+
+ };
+ }
+}
+
+#endif /* REC_FRAMEWORK_SHOT_LOCAL_ESTIMATOR_OMP_H_ */
--- /dev/null
+/*
+ * normal_estimator.h
+ *
+ * Created on: Mar 22, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_NORMAL_ESTIMATOR_H_
+#define REC_FRAMEWORK_NORMAL_ESTIMATOR_H_
+
+#include <pcl/filters/radius_outlier_removal.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/features/integral_image_normal.h>
+#include <pcl/common/time.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+ template<typename PointInT, typename PointOutT>
+ class PreProcessorAndNormalEstimator
+ {
+
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+
+ float
+ computeMeshResolution (PointInTPtr & input)
+ {
+ typedef typename pcl::KdTree<PointInT>::Ptr KdTreeInPtr;
+ KdTreeInPtr tree = boost::make_shared<pcl::KdTreeFLANN<PointInT> > (false);
+ tree->setInputCloud (input);
+
+ std::vector<int> nn_indices (9);
+ std::vector<float> nn_distances (9);
+ std::vector<int> src_indices;
+
+ float sum_distances = 0.0;
+ std::vector<float> avg_distances (input->points.size ());
+ // Iterate through the source data set
+ for (size_t i = 0; i < input->points.size (); ++i)
+ {
+ tree->nearestKSearch (input->points[i], 9, nn_indices, nn_distances);
+
+ float avg_dist_neighbours = 0.0;
+ for (size_t j = 1; j < nn_indices.size (); j++)
+ avg_dist_neighbours += sqrtf (nn_distances[j]);
+
+ avg_dist_neighbours /= static_cast<float> (nn_indices.size ());
+
+ avg_distances[i] = avg_dist_neighbours;
+ sum_distances += avg_dist_neighbours;
+ }
+
+ std::sort (avg_distances.begin (), avg_distances.end ());
+ float avg = avg_distances[static_cast<int> (avg_distances.size ()) / 2 + 1];
+ return avg;
+ }
+
+ public:
+
+ bool compute_mesh_resolution_;
+ bool do_voxel_grid_;
+ bool remove_outliers_;
+
+ //this values are used when CMR=false
+ float grid_resolution_;
+ float normal_radius_;
+
+ //this are used when CMR=true
+ float factor_normals_;
+ float factor_voxel_grid_;
+ float mesh_resolution_;
+
+ PreProcessorAndNormalEstimator ()
+ {
+ remove_outliers_ = do_voxel_grid_ = compute_mesh_resolution_ = false;
+ }
+
+ void
+ setFactorsForCMR (float f1, float f2)
+ {
+ factor_voxel_grid_ = f1;
+ factor_normals_ = f2;
+ }
+
+ void
+ setValuesForCMRFalse (float f1, float f2)
+ {
+ grid_resolution_ = f1;
+ normal_radius_ = f2;
+ }
+
+ void
+ setDoVoxelGrid (bool b)
+ {
+ do_voxel_grid_ = b;
+ }
+
+ void
+ setRemoveOutliers (bool b)
+ {
+ remove_outliers_ = b;
+ }
+
+ void
+ setCMR (bool b)
+ {
+ compute_mesh_resolution_ = b;
+ }
+
+ void
+ estimate (PointInTPtr & in, PointInTPtr & out, pcl::PointCloud<pcl::Normal>::Ptr & normals)
+ {
+ if (compute_mesh_resolution_)
+ {
+ mesh_resolution_ = computeMeshResolution (in);
+ std::cout << "compute mesh resolution:" << mesh_resolution_ << std::endl;
+ }
+
+ if (do_voxel_grid_)
+ {
+ pcl::ScopeTime t ("Voxel grid...");
+ float voxel_grid_size = grid_resolution_;
+ if (compute_mesh_resolution_)
+ {
+ voxel_grid_size = mesh_resolution_ * factor_voxel_grid_;
+ }
+
+ pcl::VoxelGrid<PointInT> grid_;
+ grid_.setInputCloud (in);
+ grid_.setLeafSize (voxel_grid_size, voxel_grid_size, voxel_grid_size);
+ grid_.setDownsampleAllData (true);
+ grid_.filter (*out);
+ }
+ else
+ {
+ out = in;
+ }
+
+ if (out->points.size () == 0)
+ {
+ PCL_WARN("NORMAL estimator: Cloud has no points after voxel grid, wont be able to compute normals!\n");
+ return;
+ }
+
+ if (remove_outliers_)
+ {
+ pcl::ScopeTime t ("remove_outliers_...");
+ PointInTPtr out2 (new pcl::PointCloud<PointInT> ());
+ float radius = normal_radius_;
+ if (compute_mesh_resolution_)
+ {
+ radius = mesh_resolution_ * factor_normals_;
+ if (do_voxel_grid_)
+ radius *= factor_voxel_grid_;
+ }
+
+ //in synthetic views the render grazes some parts of the objects
+ //thus creating a very sparse set of points that causes the normals to be very noisy
+ //remove these points
+ pcl::RadiusOutlierRemoval<PointInT> sor;
+ sor.setInputCloud (out);
+ sor.setRadiusSearch (radius);
+ sor.setMinNeighborsInRadius (16);
+ sor.filter (*out2);
+ out = out2;
+
+ }
+
+ if (out->points.size () == 0)
+ {
+ PCL_WARN("NORMAL estimator: Cloud has no points after removing outliers...!\n");
+ return;
+ }
+
+ float radius = normal_radius_;
+ if (compute_mesh_resolution_)
+ {
+ radius = mesh_resolution_ * factor_normals_;
+ if (do_voxel_grid_)
+ radius *= factor_voxel_grid_;
+ }
+
+ if (out->isOrganized ())
+ {
+ typedef typename pcl::IntegralImageNormalEstimation<PointInT, pcl::Normal> NormalEstimator_;
+ NormalEstimator_ n3d;
+ n3d.setNormalEstimationMethod (n3d.COVARIANCE_MATRIX);
+ //n3d.setNormalEstimationMethod (n3d.AVERAGE_3D_GRADIENT);
+ n3d.setInputCloud (out);
+ n3d.setRadiusSearch (radius);
+ n3d.setKSearch (0);
+ //n3d.setMaxDepthChangeFactor(0.02f);
+ //n3d.setNormalSmoothingSize(15.0f);
+ {
+ pcl::ScopeTime t ("compute normals...");
+ n3d.compute (*normals);
+ }
+ }
+ else
+ {
+
+ //check nans before computing normals
+ {
+ pcl::ScopeTime t ("check nans...");
+ int j = 0;
+ for (size_t i = 0; i < out->points.size (); ++i)
+ {
+ if (!pcl_isfinite (out->points[i].x) || !pcl_isfinite (out->points[i].y) || !pcl_isfinite (out->points[i].z))
+ continue;
+
+ out->points[j] = out->points[i];
+ j++;
+ }
+
+ if (j != static_cast<int> (out->points.size ()))
+ {
+ PCL_ERROR("Contain nans...");
+ }
+
+ out->points.resize (j);
+ out->width = j;
+ out->height = 1;
+ }
+
+ typedef typename pcl::NormalEstimation<PointInT, pcl::Normal> NormalEstimator_;
+ NormalEstimator_ n3d;
+ typename pcl::search::KdTree<PointInT>::Ptr normals_tree (new pcl::search::KdTree<PointInT>);
+ normals_tree->setInputCloud (out);
+ n3d.setRadiusSearch (radius);
+ n3d.setSearchMethod (normals_tree);
+ n3d.setInputCloud (out);
+ {
+ pcl::ScopeTime t ("compute normals not organized...");
+ n3d.compute (*normals);
+ }
+ }
+
+ //check nans...
+ if (!out->isOrganized ())
+ {
+ pcl::ScopeTime t ("check nans...");
+ int j = 0;
+ for (size_t i = 0; i < normals->points.size (); ++i)
+ {
+ if (!pcl_isfinite (normals->points[i].normal_x) || !pcl_isfinite (normals->points[i].normal_y)
+ || !pcl_isfinite (normals->points[i].normal_z))
+ continue;
+
+ normals->points[j] = normals->points[i];
+ out->points[j] = out->points[i];
+ j++;
+ }
+
+ normals->points.resize (j);
+ normals->width = j;
+ normals->height = 1;
+
+ out->points.resize (j);
+ out->width = j;
+ out->height = 1;
+ }
+ else
+ {
+ //is is organized, we set the xyz points to NaN
+ pcl::ScopeTime t ("check nans organized...");
+ bool NaNs = false;
+ for (size_t i = 0; i < normals->points.size (); ++i)
+ {
+ if (pcl_isfinite (normals->points[i].normal_x) && pcl_isfinite (normals->points[i].normal_y)
+ && pcl_isfinite (normals->points[i].normal_z))
+ continue;
+
+ NaNs = true;
+
+ out->points[i].x = out->points[i].y = out->points[i].z = std::numeric_limits<float>::quiet_NaN ();
+ }
+
+ if (NaNs)
+ {
+ PCL_WARN("normals contain NaNs\n");
+ out->is_dense = false;
+ }
+ }
+
+ /*for (size_t i = 0; i < out->points.size (); i++)
+ {
+ int r, g, b;
+ r = static_cast<int> (out->points[i].r);
+ g = static_cast<int> (out->points[i].g);
+ b = static_cast<int> (out->points[i].b);
+ std::cout << "in normal estimator:" << r << " " << g << " " << b << std::endl;
+ }*/
+ }
+ };
+ }
+}
+
+#endif /* REC_FRAMEWORK_NORMAL_ESTIMATOR_H_ */
--- /dev/null
+/*
+ * ply_source.h
+ *
+ * Created on: Mar 9, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_MESH_SOURCE_H_
+#define REC_FRAMEWORK_MESH_SOURCE_H_
+
+#include <pcl/apps/3d_rec_framework/pc_source/source.h>
+#include <pcl/apps/render_views_tesselated_sphere.h>
+#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/apps/3d_rec_framework/utils/vtk_model_sampling.h>
+#include <boost/function.hpp>
+#include <vtkTransformPolyDataFilter.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+
+ /**
+ * \brief Data source class based on mesh models
+ * \author Aitor Aldoma
+ */
+
+ template<typename PointInT>
+ class MeshSource : public Source<PointInT>
+ {
+ typedef Source<PointInT> SourceT;
+ typedef Model<PointInT> ModelT;
+
+ using SourceT::path_;
+ using SourceT::models_;
+ using SourceT::createTrainingDir;
+ using SourceT::getModelsInDirectory;
+ using SourceT::model_scale_;
+
+ int tes_level_;
+ int resolution_;
+ float radius_sphere_;
+ float view_angle_;
+ bool gen_organized_;
+ boost::function<bool
+ (const Eigen::Vector3f &)> campos_constraints_func_;
+
+ public:
+
+ using SourceT::setFilterDuplicateViews;
+
+ MeshSource () :
+ SourceT ()
+ {
+ gen_organized_ = false;
+ }
+
+ void
+ setTesselationLevel (int lev)
+ {
+ tes_level_ = lev;
+ }
+
+ void
+ setCamPosConstraints (boost::function<bool
+ (const Eigen::Vector3f &)> & bb)
+ {
+ campos_constraints_func_ = bb;
+ }
+
+ void
+ setResolution (int res)
+ {
+ resolution_ = res;
+ }
+
+ void
+ setRadiusSphere (float r)
+ {
+ radius_sphere_ = r;
+ }
+
+ void
+ setViewAngle (float a)
+ {
+ view_angle_ = a;
+ }
+
+ void
+ loadOrGenerate (std::string & dir, std::string & model_path, ModelT & model)
+ {
+ std::stringstream pathmodel;
+ pathmodel << dir << "/" << model.class_ << "/" << model.id_;
+ bf::path trained_dir = pathmodel.str ();
+
+ model.views_.reset (new std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>);
+ model.poses_.reset (new std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >);
+ model.self_occlusions_.reset (new std::vector<float>);
+ model.assembled_.reset (new pcl::PointCloud<pcl::PointXYZ>);
+ uniform_sampling (model_path, 100000, *model.assembled_, model_scale_);
+
+ if (bf::exists (trained_dir))
+ {
+ //load views, poses and self-occlusions
+ std::vector < std::string > view_filenames;
+ int number_of_views = 0;
+ bf::directory_iterator end_itr;
+ for (bf::directory_iterator itr (trained_dir); itr != end_itr; ++itr)
+ {
+ //check if its a directory, then get models in it
+ if (!(bf::is_directory (*itr)))
+ {
+ //check that it is a ply file and then add, otherwise ignore..
+ std::vector < std::string > strs;
+ std::vector < std::string > strs_;
+
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string file = (itr->path ().filename ()).string();
+#else
+ std::string file = (itr->path ()).filename ();
+#endif
+
+ boost::split (strs, file, boost::is_any_of ("."));
+ boost::split (strs_, file, boost::is_any_of ("_"));
+
+ std::string extension = strs[strs.size () - 1];
+
+ if (extension == "pcd" && strs_[0] == "view")
+ {
+#if BOOST_FILESYSTEM_VERSION == 3
+ view_filenames.push_back ((itr->path ().filename ()).string());
+#else
+ view_filenames.push_back ((itr->path ()).filename ());
+#endif
+
+ number_of_views++;
+ }
+ }
+ }
+
+ for (size_t i = 0; i < view_filenames.size (); i++)
+ {
+ std::stringstream view_file;
+ view_file << pathmodel.str () << "/" << view_filenames[i];
+ typename pcl::PointCloud<PointInT>::Ptr cloud (new pcl::PointCloud<PointInT> ());
+ pcl::io::loadPCDFile (view_file.str (), *cloud);
+
+ model.views_->push_back (cloud);
+
+ std::string file_replaced1 (view_filenames[i]);
+ boost::replace_all (file_replaced1, "view", "pose");
+ boost::replace_all (file_replaced1, ".pcd", ".txt");
+
+ std::string file_replaced2 (view_filenames[i]);
+ boost::replace_all (file_replaced2, "view", "entropy");
+ boost::replace_all (file_replaced2, ".pcd", ".txt");
+
+ //read pose as well
+ std::stringstream pose_file;
+ pose_file << pathmodel.str () << "/" << file_replaced1;
+
+ Eigen::Matrix4f pose;
+ PersistenceUtils::readMatrixFromFile (pose_file.str (), pose);
+
+ model.poses_->push_back (pose);
+
+ //read entropy as well
+ std::stringstream entropy_file;
+ entropy_file << pathmodel.str () << "/" << file_replaced2;
+ float entropy = 0;
+ PersistenceUtils::readFloatFromFile (entropy_file.str (), entropy);
+ model.self_occlusions_->push_back (entropy);
+
+ }
+
+ }
+ else
+ {
+ //load PLY model and scale it
+ vtkSmartPointer<vtkPLYReader> reader = vtkSmartPointer<vtkPLYReader>::New ();
+ reader->SetFileName (model_path.c_str ());
+
+ vtkSmartPointer<vtkTransform> trans = vtkSmartPointer<vtkTransform>::New ();
+ trans->Scale (model_scale_, model_scale_, model_scale_);
+ trans->Modified ();
+ trans->Update ();
+
+ vtkSmartPointer<vtkTransformPolyDataFilter> filter_scale = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
+ filter_scale->SetTransform (trans);
+ filter_scale->SetInputConnection (reader->GetOutputPort ());
+ filter_scale->Update ();
+
+ vtkSmartPointer<vtkPolyData> mapper = filter_scale->GetOutput ();
+
+ //generate views
+ pcl::apps::RenderViewsTesselatedSphere render_views;
+ render_views.setResolution (resolution_);
+ render_views.setUseVertices (false);
+ render_views.setRadiusSphere (radius_sphere_);
+ render_views.setComputeEntropies (true);
+ render_views.setTesselationLevel (tes_level_);
+ render_views.setViewAngle (view_angle_);
+ render_views.addModelFromPolyData (mapper);
+ render_views.setGenOrganized (gen_organized_);
+ render_views.setCamPosConstraints (campos_constraints_func_);
+ render_views.generateViews ();
+
+ std::vector<typename PointCloud<PointInT>::Ptr> views_xyz_orig;
+ std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > poses;
+ std::vector<float> entropies;
+
+ render_views.getViews (views_xyz_orig);
+ render_views.getPoses (poses);
+ render_views.getEntropies (entropies);
+
+ model.views_.reset (new std::vector<typename PointCloud<PointInT>::Ptr> ());
+ model.poses_.reset (new std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > ());
+ model.self_occlusions_.reset (new std::vector<float> ());
+
+ for (size_t i = 0; i < views_xyz_orig.size (); i++)
+ {
+ model.views_->push_back (views_xyz_orig[i]);
+ model.poses_->push_back (poses[i]);
+ model.self_occlusions_->push_back (entropies[i]);
+ }
+
+ std::stringstream direc;
+ direc << dir << "/" << model.class_ << "/" << model.id_;
+ this->createClassAndModelDirectories (dir, model.class_, model.id_);
+
+ for (size_t i = 0; i < model.views_->size (); i++)
+ {
+ //save generated model for future use
+ std::stringstream path_view;
+ path_view << direc.str () << "/view_" << i << ".pcd";
+ pcl::io::savePCDFileBinary (path_view.str (), *(model.views_->at (i)));
+
+ std::stringstream path_pose;
+ path_pose << direc.str () << "/pose_" << i << ".txt";
+
+ pcl::rec_3d_framework::PersistenceUtils::writeMatrixToFile (path_pose.str (), model.poses_->at (i));
+
+ std::stringstream path_entropy;
+ path_entropy << direc.str () << "/entropy_" << i << ".txt";
+ pcl::rec_3d_framework::PersistenceUtils::writeFloatToFile (path_entropy.str (), model.self_occlusions_->at (i));
+ }
+
+ loadOrGenerate (dir, model_path, model);
+
+ }
+ }
+
+ /**
+ * \brief Creates the model representation of the training set, generating views if needed
+ */
+ void
+ generate (std::string & training_dir)
+ {
+
+ //create training dir fs if not existent
+ createTrainingDir (training_dir);
+
+ //get models in directory
+ std::vector < std::string > files;
+ std::string start = "";
+ std::string ext = std::string ("ply");
+ bf::path dir = path_;
+ getModelsInDirectory (dir, start, files, ext);
+
+ models_.reset (new std::vector<ModelT>);
+
+ for (size_t i = 0; i < files.size (); i++)
+ {
+ ModelT m;
+ this->getIdAndClassFromFilename (files[i], m.id_, m.class_);
+
+ //check which of them have been trained using training_dir and the model_id_
+ //load views, poses and self-occlusions for those that exist
+ //generate otherwise
+ std::cout << files[i] << std::endl;
+ std::stringstream model_path;
+ model_path << path_ << "/" << files[i];
+ std::string path_model = model_path.str ();
+ loadOrGenerate (training_dir, path_model, m);
+
+ models_->push_back (m);
+ }
+ }
+ };
+ }
+}
+
+#endif /* REC_FRAMEWORK_MESH_SOURCE_H_ */
--- /dev/null
+/*
+ * ply_source.h
+ *
+ * Created on: Mar 9, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_MESH_SOURCE_H_
+#define REC_FRAMEWORK_MESH_SOURCE_H_
+
+#include <pcl/apps/3d_rec_framework/pc_source/source.h>
+#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+
+ /**
+ * \brief Data source class based on partial views from sensor.
+ * In this case, the training data is obtained directly from a depth sensor.
+ * The filesystem should contain pcd files (representing a view of an object in
+ * camera coordinates) and each view needs to be associated with a txt file
+ * containing a 4x4 matrix representing the transformation from camera coordinates
+ * to a global object coordinates frame.
+ * \author Aitor Aldoma
+ */
+
+ template<typename PointInT>
+ class RegisteredViewsSource : public Source<PointInT>
+ {
+ typedef Source<PointInT> SourceT;
+ typedef Model<PointInT> ModelT;
+
+ using SourceT::path_;
+ using SourceT::models_;
+ using SourceT::createTrainingDir;
+ using SourceT::getModelsInDirectory;
+ using SourceT::model_scale_;
+
+ std::string view_prefix_;
+ int pose_files_order_; //0 is row, 1 is column
+
+ public:
+ RegisteredViewsSource ()
+ {
+ view_prefix_ = std::string ("view");
+ pose_files_order_ = 0;
+ }
+
+ void
+ setPrefix (std::string & pre)
+ {
+ view_prefix_ = pre;
+ }
+
+ void
+ setPoseRowOrder (int o)
+ {
+ pose_files_order_ = o;
+ }
+
+ void
+ getViewsFilenames (bf::path & path_with_views, std::vector<std::string> & view_filenames)
+ {
+ int number_of_views = 0;
+ bf::directory_iterator end_itr;
+ for (bf::directory_iterator itr (path_with_views); itr != end_itr; ++itr)
+ {
+ if (!(bf::is_directory (*itr)))
+ {
+ std::vector < std::string > strs;
+ std::vector < std::string > strs_;
+
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string file = (itr->path ().filename ()).string();
+#else
+ std::string file = (itr->path ()).filename ();
+#endif
+
+ boost::split (strs, file, boost::is_any_of ("."));
+ boost::split (strs_, file, boost::is_any_of ("_"));
+
+ std::string extension = strs[strs.size () - 1];
+
+ if (extension == "pcd" && (strs_[0].compare (view_prefix_) == 0))
+ {
+#if BOOST_FILESYSTEM_VERSION == 3
+ view_filenames.push_back ((itr->path ().filename ()).string());
+#else
+ view_filenames.push_back ((itr->path ()).filename ());
+#endif
+
+ number_of_views++;
+ }
+ }
+ }
+ }
+
+ void
+ assembleModelFromViewsAndPoses(ModelT & model, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & poses) {
+ for(size_t i=0; i < model.views_->size(); i++) {
+ Eigen::Matrix4f inv = poses[i];
+ typename pcl::PointCloud<PointInT>::Ptr global_cloud(new pcl::PointCloud<PointInT>);
+ pcl::transformPointCloud(*(model.views_->at(i)),*global_cloud, inv);
+ *(model.assembled_) += *global_cloud;
+ }
+ }
+
+ void
+ loadOrGenerate (std::string & dir, std::string & model_path, ModelT & model)
+ {
+ std::stringstream pathmodel;
+ pathmodel << dir << "/" << model.class_ << "/" << model.id_;
+ bf::path trained_dir = pathmodel.str ();
+
+ model.views_.reset (new std::vector<typename pcl::PointCloud<PointInT>::Ptr>);
+ model.poses_.reset (new std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >);
+ model.self_occlusions_.reset (new std::vector<float>);
+
+ if (bf::exists (trained_dir))
+ {
+ //load views and poses
+ std::vector < std::string > view_filenames;
+ int number_of_views = 0;
+ bf::directory_iterator end_itr;
+ for (bf::directory_iterator itr (trained_dir); itr != end_itr; ++itr)
+ {
+ //check if its a directory, then get models in it
+ if (!(bf::is_directory (*itr)))
+ {
+ //check that it is a ply file and then add, otherwise ignore..
+ std::vector < std::string > strs;
+ std::vector < std::string > strs_;
+
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string file = (itr->path ().filename ()).string();
+#else
+ std::string file = (itr->path ()).filename ();
+#endif
+
+ boost::split (strs, file, boost::is_any_of ("."));
+ boost::split (strs_, file, boost::is_any_of ("_"));
+
+ std::string extension = strs[strs.size () - 1];
+
+ if (extension == "pcd" && strs_[0] == "view")
+ {
+#if BOOST_FILESYSTEM_VERSION == 3
+ view_filenames.push_back ((itr->path ().filename ()).string());
+#else
+ view_filenames.push_back ((itr->path ()).filename ());
+#endif
+
+ number_of_views++;
+ }
+ }
+ }
+
+ std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > poses_to_assemble_;
+
+ for (size_t i = 0; i < view_filenames.size (); i++)
+ {
+ std::stringstream view_file;
+ view_file << pathmodel.str () << "/" << view_filenames[i];
+ typename pcl::PointCloud<PointInT>::Ptr cloud (new pcl::PointCloud<PointInT> ());
+ pcl::io::loadPCDFile (view_file.str (), *cloud);
+
+ model.views_->push_back (cloud);
+
+ std::string file_replaced1 (view_filenames[i]);
+ boost::replace_all (file_replaced1, "view", "pose");
+ boost::replace_all (file_replaced1, ".pcd", ".txt");
+
+ //read pose as well
+ std::stringstream pose_file;
+ pose_file << pathmodel.str () << "/" << file_replaced1;
+ Eigen::Matrix4f pose;
+ PersistenceUtils::readMatrixFromFile (pose_file.str (), pose);
+
+ if(pose_files_order_ != 0) {
+ //std::cout << "Transpose..." << std::endl;
+
+ Eigen::Matrix4f pose_trans = pose.transpose();
+ poses_to_assemble_.push_back(pose_trans);
+ //pose = pose_trans;
+ //std::cout << pose << std::endl;
+ }
+
+ //std::cout << "pose being push backed to model" << std::endl;
+ std::cout << pose << std::endl;
+
+ //the recognizer assumes transformation from M to CC
+ Eigen::Matrix4f inv = pose.inverse();
+ model.poses_->push_back (inv);
+
+ model.self_occlusions_->push_back (-1.f);
+
+ }
+
+ model.assembled_.reset (new pcl::PointCloud<PointInT>);
+ assembleModelFromViewsAndPoses(model, poses_to_assemble_);
+
+ /*pcl::visualization::PCLVisualizer vis ("results");
+ pcl::visualization::PointCloudColorHandlerCustom<PointInT> random_handler (model.assembled_, 255, 0, 0);
+ vis.addPointCloud<PointInT> (model.assembled_, random_handler, "points");
+
+ Eigen::Matrix4f view_transformation = model.poses_->at(0).inverse();
+ typename pcl::PointCloud<PointInT>::Ptr view_trans(new pcl::PointCloud<PointInT>);
+ pcl::transformPointCloud(*(model.views_->at(0)), *view_trans, view_transformation);
+
+ pcl::visualization::PointCloudColorHandlerCustom<PointInT> random_handler2 (view_trans, 0, 255, 0);
+ vis.addPointCloud<PointInT> (view_trans, random_handler2, "view");
+
+ vis.addCoordinateSystem(0.1);
+ vis.spin ();*/
+
+ }
+ else
+ {
+
+ //we just need to copy the views to the training directory
+ std::stringstream direc;
+ direc << dir << "/" << model.class_ << "/" << model.id_;
+ createClassAndModelDirectories (dir, model.class_, model.id_);
+
+ std::vector < std::string > view_filenames;
+ bf::path model_dir = model_path;
+
+ getViewsFilenames (model_dir, view_filenames);
+ std::cout << view_filenames.size () << std::endl;
+
+ for (size_t i = 0; i < view_filenames.size (); i++)
+ {
+ std::stringstream view_file;
+ view_file << model_path << "/" << view_filenames[i];
+ typename pcl::PointCloud<PointInT>::Ptr cloud (new pcl::PointCloud<PointInT> ());
+ pcl::io::loadPCDFile (view_file.str (), *cloud);
+
+ std::cout << view_file.str () << std::endl;
+
+ std::stringstream path_view;
+ path_view << direc.str () << "/view_" << i << ".pcd";
+ pcl::io::savePCDFileBinary (path_view.str (), *cloud);
+
+ std::string file_replaced1 (view_file.str ());
+ boost::replace_all (file_replaced1, view_prefix_, "pose");
+ boost::replace_all (file_replaced1, ".pcd", ".txt");
+
+ Eigen::Matrix4f pose;
+ PersistenceUtils::readMatrixFromFile (file_replaced1, pose);
+
+ std::cout << pose << std::endl;
+
+ if(pose_files_order_ == 0) {
+ std::cout << "Transpose..." << std::endl;
+ Eigen::Matrix4f pose_trans = pose.transpose();
+ pose = pose_trans;
+ std::cout << pose << std::endl;
+ }
+
+ std::stringstream path_pose;
+ path_pose << direc.str () << "/pose_" << i << ".txt";
+ pcl::rec_3d_framework::PersistenceUtils::writeMatrixToFile (path_pose.str (), pose);
+ }
+
+ loadOrGenerate (dir, model_path, model);
+
+ }
+ }
+
+ bool
+ isleafDirectory (bf::path & path)
+ {
+ bf::directory_iterator end_itr;
+ bool no_dirs_inside = true;
+ for (bf::directory_iterator itr (path); itr != end_itr; ++itr)
+ {
+ if (bf::is_directory (*itr))
+ {
+ no_dirs_inside = false;
+ }
+ }
+
+ return no_dirs_inside;
+ }
+
+ void
+ getModelsInDirectory (bf::path & dir, std::string & rel_path_so_far, std::vector<std::string> & relative_paths)
+ {
+ bf::directory_iterator end_itr;
+ for (bf::directory_iterator itr (dir); itr != end_itr; ++itr)
+ {
+ //check if its a directory, then get models in it
+ if (bf::is_directory (*itr))
+ {
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string so_far = rel_path_so_far + (itr->path ().filename ()).string() + "/";
+#else
+ std::string so_far = rel_path_so_far + (itr->path ()).filename () + "/";
+#endif
+
+ bf::path curr_path = itr->path ();
+
+ if (isleafDirectory (curr_path))
+ {
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string path = rel_path_so_far + (itr->path ().filename ()).string();
+#else
+ std::string path = rel_path_so_far + (itr->path ()).filename ();
+#endif
+ relative_paths.push_back (path);
+
+ }
+ else
+ {
+ getModelsInDirectory (curr_path, so_far, relative_paths);
+ }
+ }
+ }
+ }
+
+ /**
+ * \brief Creates the model representation of the training set, generating views if needed
+ */
+ void
+ generate (std::string & training_dir)
+ {
+
+ //create training dir fs if not existent
+ createTrainingDir (training_dir);
+
+ //get models in directory
+ std::vector < std::string > files;
+ std::string start = "";
+ bf::path dir = path_;
+ getModelsInDirectory (dir, start, files);
+
+ models_.reset (new std::vector<ModelT>);
+
+ for (size_t i = 0; i < files.size (); i++)
+ {
+ ModelT m;
+
+ std::vector < std::string > strs;
+ boost::split (strs, files[i], boost::is_any_of ("/\\"));
+ std::string name = strs[strs.size () - 1];
+
+ if (strs.size () == 1)
+ {
+ m.id_ = strs[0];
+ }
+ else
+ {
+ std::stringstream ss;
+ for (int i = 0; i < (static_cast<int> (strs.size ()) - 1); i++)
+ {
+ ss << strs[i];
+ if (i != (static_cast<int> (strs.size ()) - 1))
+ ss << "/";
+ }
+
+ m.class_ = ss.str ();
+ m.id_ = strs[strs.size () - 1];
+ }
+
+ std::cout << m.class_ << " . " << m.id_ << std::endl;
+ //check which of them have been trained using training_dir and the model_id_
+ //load views, poses and self-occlusions for those that exist
+ //generate otherwise
+
+ std::stringstream model_path;
+ model_path << path_ << "/" << files[i];
+ std::string path_model = model_path.str ();
+ loadOrGenerate (training_dir, path_model, m);
+
+ models_->push_back (m);
+
+ //std::cout << files[i] << std::endl;
+ }
+ }
+ };
+ }
+}
+
+#endif /* REC_FRAMEWORK_MESH_SOURCE_H_ */
--- /dev/null
+/*
+ * source.h
+ *
+ * Created on: Mar 9, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_VIEWS_SOURCE_H_
+#define REC_FRAMEWORK_VIEWS_SOURCE_H_
+
+#include <boost/filesystem.hpp>
+#include <boost/algorithm/string.hpp>
+#include <pcl/io/pcd_io.h>
+#include <pcl/apps/3d_rec_framework/utils/persistence_utils.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/common/transforms.h>
+
+namespace bf = boost::filesystem;
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+
+ /**
+ * \brief Model representation
+ * \author Aitor Aldoma
+ */
+
+ template<typename PointT>
+ class Model
+ {
+ typedef typename pcl::PointCloud<PointT>::Ptr PointTPtr;
+ typedef typename pcl::PointCloud<PointT>::ConstPtr PointTPtrConst;
+
+ public:
+ boost::shared_ptr<std::vector<PointTPtr> > views_;
+ boost::shared_ptr<std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > > poses_;
+ boost::shared_ptr<std::vector<float> > self_occlusions_;
+ std::string id_;
+ std::string class_;
+ PointTPtr assembled_;
+ typename std::map<float, PointTPtrConst> voxelized_assembled_;
+
+ bool
+ operator== (const Model &other) const
+ {
+ return (id_ == other.id_) && (class_ == other.class_);
+ }
+
+ PointTPtrConst
+ getAssembled (float resolution)
+ {
+ if(resolution <= 0)
+ return assembled_;
+
+ typename std::map<float, PointTPtrConst>::iterator it = voxelized_assembled_.find (resolution);
+ if (it == voxelized_assembled_.end ())
+ {
+ PointTPtr voxelized (new pcl::PointCloud<PointT>);
+ pcl::VoxelGrid<PointT> grid_;
+ grid_.setInputCloud (assembled_);
+ grid_.setLeafSize (resolution, resolution, resolution);
+ grid_.setDownsampleAllData(true);
+ grid_.filter (*voxelized);
+
+ PointTPtrConst voxelized_const (new pcl::PointCloud<PointT> (*voxelized));
+ voxelized_assembled_[resolution] = voxelized_const;
+ return voxelized_const;
+ }
+
+ return it->second;
+ }
+ };
+
+ /**
+ * \brief Abstract data source class, manages filesystem, incremental training, etc.
+ * \author Aitor Aldoma
+ */
+
+ template<typename PointInT>
+ class Source
+ {
+
+ protected:
+ typedef Model<PointInT> ModelT;
+ std::string path_;
+ boost::shared_ptr<std::vector<ModelT> > models_;
+ float model_scale_;
+ bool filter_duplicate_views_;
+ bool load_views_;
+
+ void
+ getIdAndClassFromFilename (std::string & filename, std::string & id, std::string & classname)
+ {
+
+ std::vector < std::string > strs;
+ boost::split (strs, filename, boost::is_any_of ("/\\"));
+ std::string name = strs[strs.size () - 1];
+
+ std::stringstream ss;
+ for (int i = 0; i < (static_cast<int> (strs.size ()) - 1); i++)
+ {
+ ss << strs[i];
+ if (i != (static_cast<int> (strs.size ()) - 1))
+ ss << "/";
+ }
+
+ classname = ss.str ();
+ id = name.substr (0, name.length () - 4);
+ }
+
+ void
+ createTrainingDir (std::string & training_dir)
+ {
+ bf::path trained_dir = training_dir;
+ if (!bf::exists (trained_dir))
+ bf::create_directory (trained_dir);
+ }
+
+ void
+ createClassAndModelDirectories (std::string & training_dir, std::string & class_str, std::string & id_str)
+ {
+ std::vector < std::string > strs;
+ boost::split (strs, class_str, boost::is_any_of ("/\\"));
+
+ std::stringstream ss;
+ ss << training_dir << "/";
+ for (size_t i = 0; i < strs.size (); i++)
+ {
+ ss << strs[i] << "/";
+ bf::path trained_dir = ss.str ();
+ if (!bf::exists (trained_dir))
+ bf::create_directory (trained_dir);
+ }
+
+ ss << id_str;
+ bf::path trained_dir = ss.str ();
+ if (!bf::exists (trained_dir))
+ bf::create_directory (trained_dir);
+ }
+
+ public:
+
+ Source() {
+ load_views_ = true;
+ }
+
+ float
+ getScale ()
+ {
+ return model_scale_;
+ }
+
+ void
+ setModelScale (float s)
+ {
+ model_scale_ = s;
+ }
+
+ void setFilterDuplicateViews(bool f) {
+ filter_duplicate_views_ = f;
+ std::cout << "setting filter duplicate views to " << f << std::endl;
+ }
+
+ void
+ getModelsInDirectory (bf::path & dir, std::string & rel_path_so_far, std::vector<std::string> & relative_paths, std::string & ext)
+ {
+ bf::directory_iterator end_itr;
+ for (bf::directory_iterator itr (dir); itr != end_itr; ++itr)
+ {
+ //check if its a directory, then get models in it
+ if (bf::is_directory (*itr))
+ {
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string so_far = rel_path_so_far + (itr->path ().filename ()).string() + "/";
+#else
+ std::string so_far = rel_path_so_far + (itr->path ()).filename () + "/";
+#endif
+
+ bf::path curr_path = itr->path ();
+ getModelsInDirectory (curr_path, so_far, relative_paths, ext);
+ }
+ else
+ {
+ //check that it is a ply file and then add, otherwise ignore..
+ std::vector < std::string > strs;
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string file = (itr->path ().filename ()).string();
+#else
+ std::string file = (itr->path ()).filename ();
+#endif
+
+ boost::split (strs, file, boost::is_any_of ("."));
+ std::string extension = strs[strs.size () - 1];
+
+ if (extension.compare (ext) == 0)
+ {
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string path = rel_path_so_far + (itr->path ().filename ()).string();
+#else
+ std::string path = rel_path_so_far + (itr->path ()).filename ();
+#endif
+
+ relative_paths.push_back (path);
+ }
+ }
+ }
+ }
+
+ void
+ voxelizeAllModels (float resolution)
+ {
+ for (size_t i = 0; i < models_->size (); i++)
+ {
+ models_->at (i).getAssembled (resolution);
+ }
+ }
+
+ /**
+ * \brief Generate model representation
+ */
+ virtual void
+ generate (std::string & training_dir)=0;
+
+ /**
+ * \brief Get the generated model
+ */
+ boost::shared_ptr<std::vector<ModelT> >
+ getModels ()
+ {
+ return models_;
+ }
+
+ boost::shared_ptr<std::vector<ModelT> >
+ getModels (std::string & model_id)
+ {
+
+ typename std::vector<ModelT>::iterator it = models_->begin ();
+ while (it != models_->end ())
+ {
+ if (model_id.compare ((*it).id_) != 0)
+ {
+ it = models_->erase (it);
+ }
+ else
+ {
+ it++;
+ }
+ }
+
+ return models_;
+ }
+
+ bool
+ modelAlreadyTrained (ModelT m, std::string & base_dir, std::string & descr_name)
+ {
+ std::stringstream dir;
+ dir << base_dir << "/" << m.class_ << "/" << m.id_ << "/" << descr_name;
+ bf::path desc_dir = dir.str ();
+ std::cout << dir.str () << std::endl;
+ if (bf::exists (desc_dir))
+ {
+ return true;
+ }
+
+ return false;
+ }
+
+ std::string
+ getModelDescriptorDir (ModelT m, std::string & base_dir, std::string & descr_name)
+ {
+ std::stringstream dir;
+ dir << base_dir << "/" << m.class_ << "/" << m.id_ << "/" << descr_name;
+ return dir.str ();
+ }
+
+ void
+ removeDescDirectory (ModelT m, std::string & base_dir, std::string & descr_name)
+ {
+ std::string dir = getModelDescriptorDir (m, base_dir, descr_name);
+
+ bf::path desc_dir = dir;
+ if (bf::exists (desc_dir))
+ bf::remove_all (desc_dir);
+ }
+
+ void
+ setPath (std::string & path)
+ {
+ path_ = path;
+ }
+
+ void setLoadViews(bool load) {
+ load_views_ = load;
+ }
+ };
+ }
+}
+
+#endif /* REC_FRAMEWORK_VIEWS_SOURCE_H_ */
--- /dev/null
+/*
+ * global.h
+ *
+ * Created on: Mar 9, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_GLOBAL_PIPELINE_H_
+#define REC_FRAMEWORK_GLOBAL_PIPELINE_H_
+
+#include <flann/flann.h>
+#include <pcl/common/common.h>
+#include <pcl/apps/3d_rec_framework/pc_source/source.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/global/global_estimator.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+
+ template<typename PointInT>
+ class PCL_EXPORTS GlobalClassifier {
+ public:
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+
+ virtual void
+ setNN (int nn) = 0;
+
+ virtual void
+ getCategory (std::vector<std::string> & categories) = 0;
+
+ virtual void
+ getConfidence (std::vector<float> & conf) = 0;
+
+ virtual void
+ classify () = 0;
+
+ virtual void
+ setIndices (std::vector<int> & indices) = 0;
+
+ virtual void
+ setInputCloud (const PointInTPtr & cloud) = 0;
+ };
+
+ /**
+ * \brief Nearest neighbor search based classification of PCL point type features.
+ * FLANN is used to identify a neighborhood, based on which different scoring schemes
+ * can be employed to obtain likelihood values for a specified list of classes.
+ * Available features: ESF, VFH, CVFH
+ * See apps/3d_rec_framework/tools/apps/global_classification.cpp for usage
+ * \author Aitor Aldoma
+ */
+
+ template<template<class > class Distance, typename PointInT, typename FeatureT>
+ class PCL_EXPORTS GlobalNNPipeline : public pcl::rec_3d_framework::GlobalClassifier<PointInT>
+ {
+
+ protected:
+
+ struct index_score
+ {
+ int idx_models_;
+ int idx_input_;
+ double score_;
+ };
+
+ struct sortIndexScores
+ {
+ bool
+ operator() (const index_score& d1, const index_score& d2)
+ {
+ return d1.score_ < d2.score_;
+ }
+ } sortIndexScoresOp;
+
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ typedef Distance<float> DistT;
+ typedef Model<PointInT> ModelT;
+
+ /** \brief Directory where the trained structure will be saved */
+ std::string training_dir_;
+
+ /** \brief Point cloud to be classified */
+ PointInTPtr input_;
+
+ /** \brief Model data source */
+ typename boost::shared_ptr<pcl::rec_3d_framework::Source<PointInT> > source_;
+
+ /** \brief Computes a feature */
+ typename boost::shared_ptr<GlobalEstimator<PointInT, FeatureT> > estimator_;
+
+ /** \brief Descriptor name */
+ std::string descr_name_;
+
+ typedef std::pair<ModelT, std::vector<float> > flann_model;
+ flann::Matrix<float> flann_data_;
+ flann::Index<DistT> * flann_index_;
+ std::vector<flann_model> flann_models_;
+
+ std::vector<int> indices_;
+
+ //load features from disk and create flann structure
+ void
+ loadFeaturesAndCreateFLANN ();
+
+ inline void
+ convertToFLANN (const std::vector<flann_model> &models, flann::Matrix<float> &data)
+ {
+ data.rows = models.size ();
+ data.cols = models[0].second.size (); // number of histogram bins
+
+ flann::Matrix<float> flann_data (new float[models.size () * models[0].second.size ()], models.size (), models[0].second.size ());
+
+ for (size_t i = 0; i < data.rows; ++i)
+ for (size_t j = 0; j < data.cols; ++j)
+ {
+ flann_data.ptr ()[i * data.cols + j] = models[i].second[j];
+ }
+
+ data = flann_data;
+ }
+
+ void
+ nearestKSearch (flann::Index<DistT> * index, const flann_model &model, int k, flann::Matrix<int> &indices, flann::Matrix<float> &distances);
+
+ int NN_;
+ std::vector<std::string> categories_;
+ std::vector<float> confidences_;
+
+ std::string first_nn_category_;
+
+ public:
+
+ GlobalNNPipeline ()
+ {
+ NN_ = 1;
+ }
+
+ ~GlobalNNPipeline ()
+ {
+ }
+
+ void
+ setNN (int nn)
+ {
+ NN_ = nn;
+ }
+
+ void
+ getCategory (std::vector<std::string> & categories)
+ {
+ categories = categories_;
+ }
+
+ void
+ getConfidence (std::vector<float> & conf)
+ {
+ conf = confidences_;
+ }
+
+ /**
+ * \brief Initializes the FLANN structure from the provided source
+ */
+
+ void
+ initialize (bool force_retrain = false);
+
+ /**
+ * \brief Performs classification
+ */
+
+ void
+ classify ();
+
+ /**
+ * \brief Sets the model data source_
+ */
+ void
+ setDataSource (typename boost::shared_ptr<Source<PointInT> > & source)
+ {
+ source_ = source;
+ }
+
+ /**
+ * \brief Sets the model data source_
+ */
+
+ void
+ setFeatureEstimator (typename boost::shared_ptr<GlobalEstimator<PointInT, FeatureT> > & feat)
+ {
+ estimator_ = feat;
+ }
+
+ void
+ setIndices (std::vector<int> & indices)
+ {
+ indices_ = indices;
+ }
+
+ /**
+ * \brief Sets the input cloud to be classified
+ */
+ void
+ setInputCloud (const PointInTPtr & cloud)
+ {
+ input_ = cloud;
+ }
+
+ void
+ setDescriptorName (std::string & name)
+ {
+ descr_name_ = name;
+ }
+
+ void
+ setTrainingDir (std::string & dir)
+ {
+ training_dir_ = dir;
+ }
+ };
+ }
+}
+#endif /* REC_FRAMEWORK_GLOBAL_PIPELINE_H_ */
--- /dev/null
+/*
+ * global.h
+ *
+ * Created on: Mar 9, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_GLOBAL_RECOGNIZER_CRH_H_
+#define REC_FRAMEWORK_GLOBAL_RECOGNIZER_CRH_H_
+
+#include <flann/flann.h>
+#include <pcl/common/common.h>
+#include <pcl/apps/3d_rec_framework/pc_source/source.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/global/global_estimator.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/global/crh_estimator.h>
+#include <pcl/recognition/hv/hypotheses_verification.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+
+ /**
+ * \brief Nearest neighbor search based classification of PCL point type features.
+ * FLANN is used to identify a neighborhood, based on which different scoring schemes
+ * can be employed to obtain likelihood values for a specified list of classes.
+ * Available features: ESF, VFH, CVFH
+ * \author Aitor Aldoma
+ */
+
+ template<template<class > class Distance, typename PointInT, typename FeatureT>
+ class PCL_EXPORTS GlobalNNCRHRecognizer
+ {
+
+ protected:
+
+ struct index_score
+ {
+ int idx_models_;
+ int idx_input_;
+ double score_;
+ };
+
+ struct sortIndexScores
+ {
+ bool
+ operator() (const index_score& d1, const index_score& d2)
+ {
+ return d1.score_ < d2.score_;
+ }
+ } sortIndexScoresOp;
+
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ typedef typename pcl::PointCloud<PointInT>::ConstPtr ConstPointInTPtr;
+
+ typedef Distance<float> DistT;
+ typedef Model<PointInT> ModelT;
+ typedef pcl::PointCloud<pcl::Histogram<90> > CRHPointCloud;
+
+ /** \brief Directory where the trained structure will be saved */
+ std::string training_dir_;
+
+ /** \brief Point cloud to be classified */
+ PointInTPtr input_;
+
+ /** \brief Model data source */
+ typename boost::shared_ptr<pcl::rec_3d_framework::Source<PointInT> > source_;
+
+ /** \brief Computes a feature */
+ typename boost::shared_ptr<CRHEstimation<PointInT, FeatureT> > crh_estimator_;
+
+ /** \brief Hypotheses verification algorithm */
+ typename boost::shared_ptr<HypothesisVerification<PointInT, PointInT> > hv_algorithm_;
+
+ /** \brief Descriptor name */
+ std::string descr_name_;
+
+ int ICP_iterations_;
+
+ bool noisify_;
+ float noise_;
+
+ class flann_model
+ {
+ public:
+ ModelT model;
+ int view_id;
+ int descriptor_id;
+ std::vector<float> descr;
+ };
+
+ flann::Matrix<float> flann_data_;
+ flann::Index<DistT> * flann_index_;
+ std::vector<flann_model> flann_models_;
+
+
+ bool use_cache_;
+ std::map<std::pair<std::string, int>, Eigen::Matrix4f, std::less<std::pair<std::string, int> >, Eigen::aligned_allocator<std::pair<std::pair<
+ std::string, int>, Eigen::Matrix4f> > > poses_cache_;
+ std::map<std::pair<std::string, int>, Eigen::Vector3f > centroids_cache_;
+
+ std::vector<int> indices_;
+
+ //load features from disk and create flann structure
+ void
+ loadFeaturesAndCreateFLANN ();
+
+ inline void
+ convertToFLANN (const std::vector<flann_model> &models, flann::Matrix<float> &data)
+ {
+ data.rows = models.size ();
+ data.cols = models[0].descr.size (); // number of histogram bins
+
+ flann::Matrix<float> flann_data (new float[models.size () * models[0].descr.size ()], models.size (), models[0].descr.size ());
+
+ for (size_t i = 0; i < data.rows; ++i)
+ for (size_t j = 0; j < data.cols; ++j)
+ {
+ flann_data.ptr ()[i * data.cols + j] = models[i].descr[j];
+ }
+
+ data = flann_data;
+ }
+
+ void
+ nearestKSearch (flann::Index<DistT> * index, const flann_model &model, int k, flann::Matrix<int> &indices, flann::Matrix<float> &distances);
+
+ void
+ getPose (ModelT & model, int view_id, Eigen::Matrix4f & pose_matrix);
+
+ void
+ getCRH (ModelT & model, int view_id, int d_id, CRHPointCloud::Ptr & hist);
+
+ void
+ getCentroid (ModelT & model, int view_id, int d_id, Eigen::Vector3f & centroid);
+
+ void
+ getView (ModelT & model, int view_id, PointInTPtr & view);
+
+ int NN_;
+
+ boost::shared_ptr<std::vector<ModelT> > models_;
+ boost::shared_ptr<std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > > transforms_;
+
+ public:
+
+ GlobalNNCRHRecognizer ()
+ {
+ ICP_iterations_ = 0;
+ noisify_ = false;
+ do_CRH_ = true;
+ }
+
+ ~GlobalNNCRHRecognizer ()
+ {
+ }
+
+ void setNoise(float n) {
+ noisify_ = true;
+ noise_ = n;
+ }
+
+ void setDOCRH(bool b) {
+ do_CRH_ = b;
+ }
+
+ void
+ setNN (int nn)
+ {
+ NN_ = nn;
+ }
+
+ void
+ setICPIterations (int it)
+ {
+ ICP_iterations_ = it;
+ }
+
+ /**
+ * \brief Initializes the FLANN structure from the provided source
+ */
+
+ void
+ initialize (bool force_retrain = false);
+
+ /**
+ * \brief Sets the model data source_
+ */
+ void
+ setDataSource (typename boost::shared_ptr<Source<PointInT> > & source)
+ {
+ source_ = source;
+ }
+
+ /**
+ * \brief Sets the model data source_
+ */
+
+ void
+ setFeatureEstimator (typename boost::shared_ptr<CRHEstimation<PointInT, FeatureT> > & feat)
+ {
+ crh_estimator_ = feat;
+ }
+
+ /**
+ * \brief Sets the HV algorithm
+ */
+ void
+ setHVAlgorithm (typename boost::shared_ptr<HypothesisVerification<PointInT, PointInT> > & alg)
+ {
+ hv_algorithm_ = alg;
+ }
+
+ void
+ setIndices (std::vector<int> & indices)
+ {
+ indices_ = indices;
+ }
+
+ /**
+ * \brief Sets the input cloud to be classified
+ */
+ void
+ setInputCloud (const PointInTPtr & cloud)
+ {
+ input_ = cloud;
+ }
+
+ void
+ setDescriptorName (std::string & name)
+ {
+ descr_name_ = name;
+ }
+
+ void
+ setTrainingDir (std::string & dir)
+ {
+ training_dir_ = dir;
+ }
+
+ /**
+ * \brief Performs recognition on the input cloud
+ */
+
+ void
+ recognize ();
+
+ boost::shared_ptr<std::vector<ModelT> >
+ getModels ()
+ {
+ return models_;
+ }
+
+ boost::shared_ptr<std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > >
+ getTransforms ()
+ {
+ return transforms_;
+ }
+
+ void
+ setUseCache (bool u)
+ {
+ use_cache_ = u;
+ }
+
+ bool do_CRH_;
+
+ };
+ }
+}
+#endif /* REC_FRAMEWORK_GLOBAL_PIPELINE_H_ */
--- /dev/null
+/*
+ * global.h
+ *
+ * Created on: Mar 9, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_GLOBAL_RECOGNIZER_CVFH_H_
+#define REC_FRAMEWORK_GLOBAL_RECOGNIZER_CVFH_H_
+
+#include <flann/flann.h>
+#include <pcl/common/common.h>
+#include <pcl/apps/3d_rec_framework/pc_source/source.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/global/global_estimator.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/global/ourcvfh_estimator.h>
+#include <pcl/recognition/hv/hypotheses_verification.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+
+ /**
+ * \brief Nearest neighbor search based classification of PCL point type features.
+ * Available features: CVFH
+ * \author Aitor Aldoma
+ */
+
+ template<template<class > class Distance, typename PointInT, typename FeatureT = pcl::VFHSignature308>
+ class PCL_EXPORTS GlobalNNCVFHRecognizer
+ {
+
+ protected:
+
+ struct index_score
+ {
+ int idx_models_;
+ int idx_input_;
+ double score_;
+ };
+
+ struct sortIndexScores
+ {
+ bool
+ operator() (const index_score& d1, const index_score& d2)
+ {
+ return d1.score_ < d2.score_;
+ }
+ } sortIndexScoresOp;
+
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ typedef typename pcl::PointCloud<PointInT>::ConstPtr ConstPointInTPtr;
+
+ typedef Distance<float> DistT;
+ typedef Model<PointInT> ModelT;
+
+ /** \brief Directory where the trained structure will be saved */
+ std::string training_dir_;
+
+ /** \brief Point cloud to be classified */
+ PointInTPtr input_;
+
+ /** \brief Model data source */
+ typename boost::shared_ptr<pcl::rec_3d_framework::Source<PointInT> > source_;
+
+ /** \brief Computes a feature */
+ typename boost::shared_ptr<OURCVFHEstimator<PointInT, FeatureT> > micvfh_estimator_;
+
+ /** \brief Hypotheses verification algorithm */
+ typename boost::shared_ptr<HypothesisVerification<PointInT, PointInT> > hv_algorithm_;
+
+ /** \brief Descriptor name */
+ std::string descr_name_;
+
+ int ICP_iterations_;
+
+ bool noisify_;
+ float noise_;
+
+ class flann_model
+ {
+ public:
+ ModelT model;
+ int view_id;
+ int descriptor_id;
+ std::vector<float> descr;
+
+ bool
+ operator< (const flann_model &other) const
+ {
+ if ((this->model.id_.compare (other.model.id_) < 0))
+ {
+ return true;
+ }
+ else
+ {
+
+ if (this->model.id_.compare (other.model.id_) == 0)
+ {
+ //check view id
+ if ((this->view_id < other.view_id))
+ {
+ return true;
+ }
+ else
+ {
+ if (this->view_id == other.view_id)
+ {
+ if (this->descriptor_id < other.descriptor_id)
+ {
+ return true;
+ }
+ }
+ }
+ }
+ }
+
+ return false;
+ }
+
+ bool
+ operator== (const flann_model &other) const
+ {
+ return (model.id_ == other.model.id_) && (view_id == other.view_id) && (descriptor_id == other.descriptor_id);
+ }
+
+ };
+
+ flann::Matrix<float> flann_data_;
+ flann::Index<DistT> * flann_index_;
+ std::vector<flann_model> flann_models_;
+
+ std::vector<flann::Matrix<float> > single_categories_data_;
+ std::vector<flann::Index<DistT> *> single_categories_index_;
+ std::vector<boost::shared_ptr<std::vector<int> > > single_categories_pointers_to_models_;
+ std::map<std::string, int> category_to_vectors_indices_;
+ std::vector<std::string> categories_to_be_searched_;
+ bool use_single_categories_;
+
+ bool use_cache_;
+ std::map<std::pair<std::string, int>, Eigen::Matrix4f, std::less<std::pair<std::string, int> >, Eigen::aligned_allocator<std::pair<std::pair<
+ std::string, int>, Eigen::Matrix4f> > > poses_cache_;
+ std::map<std::pair<std::string, int>, Eigen::Vector3f> centroids_cache_;
+
+ std::vector<int> indices_;
+
+ bool compute_scale_;
+
+ //load features from disk and create flann structure
+ void
+ loadFeaturesAndCreateFLANN ();
+
+ inline void
+ convertToFLANN (const std::vector<flann_model> &models, flann::Matrix<float> &data)
+ {
+ data.rows = models.size ();
+ data.cols = models[0].descr.size (); // number of histogram bins
+
+ flann::Matrix<float> flann_data (new float[models.size () * models[0].descr.size ()], models.size (), models[0].descr.size ());
+
+ for (size_t i = 0; i < data.rows; ++i)
+ for (size_t j = 0; j < data.cols; ++j)
+ {
+ flann_data.ptr ()[i * data.cols + j] = models[i].descr[j];
+ }
+
+ data = flann_data;
+ }
+
+ inline void
+ convertToFLANN (const std::vector<flann_model> &models, boost::shared_ptr<std::vector<int> > & indices, flann::Matrix<float> &data)
+ {
+ data.rows = indices->size ();
+ data.cols = models[0].descr.size (); // number of histogram bins
+
+ flann::Matrix<float> flann_data(new float[indices->size () * models[0].descr.size ()],indices->size(),models[0].descr.size ());
+
+ for (size_t i = 0; i < data.rows; ++i)
+ for (size_t j = 0; j < data.cols; ++j)
+ {
+ flann_data.ptr()[i * data.cols + j] = models[indices->at(i)].descr[j];
+ }
+
+ data = flann_data;
+ }
+
+ void
+ nearestKSearch (flann::Index<DistT> * index, const flann_model &model, int k, flann::Matrix<int> &indices, flann::Matrix<float> &distances);
+
+ void
+ getPose (ModelT & model, int view_id, Eigen::Matrix4f & pose_matrix);
+
+ bool
+ getRollPose (ModelT & model, int view_id, int d_id, Eigen::Matrix4f & pose_matrix);
+
+ void
+ getCentroid (ModelT & model, int view_id, int d_id, Eigen::Vector3f & centroid);
+
+ void
+ getView (ModelT & model, int view_id, PointInTPtr & view);
+
+ int NN_;
+
+ boost::shared_ptr<std::vector<ModelT> > models_;
+ boost::shared_ptr<std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > > transforms_;
+
+ std::vector<float> descriptor_distances_;
+
+ public:
+
+ GlobalNNCVFHRecognizer ()
+ {
+ ICP_iterations_ = 0;
+ noisify_ = false;
+ compute_scale_ = false;
+ use_single_categories_ = false;
+ }
+
+ ~GlobalNNCVFHRecognizer ()
+ {
+ }
+
+ void
+ getDescriptorDistances (std::vector<float> & ds)
+ {
+ ds = descriptor_distances_;
+ }
+
+ void
+ setComputeScale (bool d)
+ {
+ compute_scale_ = d;
+ }
+
+ void
+ setCategoriesToUseForRecognition (std::vector<std::string> & cats_to_use)
+ {
+ categories_to_be_searched_.clear ();
+ categories_to_be_searched_ = cats_to_use;
+ }
+
+ void setUseSingleCategories(bool b) {
+ use_single_categories_ = b;
+ }
+
+ void
+ setNoise (float n)
+ {
+ noisify_ = true;
+ noise_ = n;
+ }
+
+ void
+ setNN (int nn)
+ {
+ NN_ = nn;
+ }
+
+ void
+ setICPIterations (int it)
+ {
+ ICP_iterations_ = it;
+ }
+
+ /**
+ * \brief Initializes the FLANN structure from the provided source
+ */
+
+ void
+ initialize (bool force_retrain = false);
+
+ /**
+ * \brief Sets the model data source_
+ */
+ void
+ setDataSource (typename boost::shared_ptr<Source<PointInT> > & source)
+ {
+ source_ = source;
+ }
+
+ /**
+ * \brief Sets the model data source_
+ */
+
+ void
+ setFeatureEstimator (typename boost::shared_ptr<OURCVFHEstimator<PointInT, FeatureT> > & feat)
+ {
+ micvfh_estimator_ = feat;
+ }
+
+ /**
+ * \brief Sets the HV algorithm
+ */
+ void
+ setHVAlgorithm (typename boost::shared_ptr<HypothesisVerification<PointInT, PointInT> > & alg)
+ {
+ hv_algorithm_ = alg;
+ }
+
+ void
+ setIndices (std::vector<int> & indices)
+ {
+ indices_ = indices;
+ }
+
+ /**
+ * \brief Sets the input cloud to be classified
+ */
+ void
+ setInputCloud (const PointInTPtr & cloud)
+ {
+ input_ = cloud;
+ }
+
+ void
+ setDescriptorName (std::string & name)
+ {
+ descr_name_ = name;
+ }
+
+ void
+ setTrainingDir (std::string & dir)
+ {
+ training_dir_ = dir;
+ }
+
+ /**
+ * \brief Performs recognition on the input cloud
+ */
+
+ void
+ recognize ();
+
+ boost::shared_ptr<std::vector<ModelT> >
+ getModels ()
+ {
+ return models_;
+ }
+
+ boost::shared_ptr<std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > >
+ getTransforms ()
+ {
+ return transforms_;
+ }
+
+ void
+ setUseCache (bool u)
+ {
+ use_cache_ = u;
+ }
+
+ };
+ }
+}
+#endif /* REC_FRAMEWORK_GLOBAL_PIPELINE_H_ */
--- /dev/null
+/*
+ * global_nn_classifier.cpp
+ *
+ * Created on: Mar 9, 2012
+ * Author: aitor
+ */
+
+#include <pcl/apps/3d_rec_framework/pipeline/global_nn_classifier.h>
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNPipeline<Distance, PointInT, FeatureT>::loadFeaturesAndCreateFLANN ()
+ {
+ boost::shared_ptr < std::vector<ModelT> > models = source_->getModels ();
+ for (size_t i = 0; i < models->size (); i++)
+ {
+ std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_);
+ bf::path inside = path;
+ bf::directory_iterator end_itr;
+
+ for (bf::directory_iterator itr_in (inside); itr_in != end_itr; ++itr_in)
+ {
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string file_name = (itr_in->path ().filename ()).string();
+#else
+ std::string file_name = (itr_in->path ()).filename ();
+#endif
+
+ std::vector < std::string > strs;
+ boost::split (strs, file_name, boost::is_any_of ("_"));
+
+ if (strs[0] == "descriptor")
+ {
+ std::string full_file_name = itr_in->path ().string ();
+ std::vector < std::string > strs;
+ boost::split (strs, full_file_name, boost::is_any_of ("/"));
+
+ typename pcl::PointCloud<FeatureT>::Ptr signature (new pcl::PointCloud<FeatureT>);
+ pcl::io::loadPCDFile (full_file_name, *signature);
+
+ flann_model descr_model;
+ descr_model.first = models->at (i);
+ int size_feat = sizeof(signature->points[0].histogram) / sizeof(float);
+ descr_model.second.resize (size_feat);
+ memcpy (&descr_model.second[0], &signature->points[0].histogram[0], size_feat * sizeof(float));
+
+ flann_models_.push_back (descr_model);
+ }
+ }
+ }
+
+ convertToFLANN (flann_models_, flann_data_);
+ flann_index_ = new flann::Index<DistT> (flann_data_, flann::LinearIndexParams ());
+ flann_index_->buildIndex ();
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNPipeline<Distance, PointInT, FeatureT>::nearestKSearch (flann::Index<DistT> * index, const flann_model &model,
+ int k, flann::Matrix<int> &indices,
+ flann::Matrix<float> &distances)
+ {
+ flann::Matrix<float> p = flann::Matrix<float> (new float[model.second.size ()], 1, model.second.size ());
+ memcpy (&p.ptr ()[0], &model.second[0], p.cols * p.rows * sizeof(float));
+
+ indices = flann::Matrix<int> (new int[k], 1, k);
+ distances = flann::Matrix<float> (new float[k], 1, k);
+ index->knnSearch (p, indices, distances, k, flann::SearchParams (512));
+ delete[] p.ptr ();
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNPipeline<Distance, PointInT, FeatureT>::classify ()
+ {
+
+ categories_.clear ();
+ confidences_.clear ();
+
+ first_nn_category_ = std::string ("");
+
+ PointInTPtr processed (new pcl::PointCloud<PointInT>);
+ PointInTPtr in (new pcl::PointCloud<PointInT>);
+
+ typename pcl::PointCloud<FeatureT>::CloudVectorType signatures;
+ std::vector < Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > centroids;
+
+ if (indices_.size ())
+ {
+ pcl::copyPointCloud (*input_, indices_, *in);
+ }
+ else
+ {
+ in = input_;
+ }
+
+ estimator_->estimate (in, processed, signatures, centroids);
+ std::vector<index_score> indices_scores;
+
+ if (signatures.size () > 0)
+ {
+ for (size_t idx = 0; idx < signatures.size (); idx++)
+ {
+ float* hist = signatures[idx].points[0].histogram;
+ int size_feat = sizeof(signatures[idx].points[0].histogram) / sizeof(float);
+ std::vector<float> std_hist (hist, hist + size_feat);
+ ModelT empty;
+
+ flann_model histogram (empty, std_hist);
+ flann::Matrix<int> indices;
+ flann::Matrix<float> distances;
+ nearestKSearch (flann_index_, histogram, NN_, indices, distances);
+
+ //gather NN-search results
+ double score = 0;
+ for (int i = 0; i < NN_; ++i)
+ {
+ score = distances[0][i];
+ index_score is;
+ is.idx_models_ = indices[0][i];
+ is.idx_input_ = static_cast<int> (idx);
+ is.score_ = score;
+ indices_scores.push_back (is);
+ }
+ }
+
+ std::sort (indices_scores.begin (), indices_scores.end (), sortIndexScoresOp);
+ first_nn_category_ = flann_models_[indices_scores[0].idx_models_].first.class_;
+
+ std::map<std::string, int> category_map;
+ std::map<std::string, int>::iterator it;
+ int num_n = std::min (NN_, static_cast<int> (indices_scores.size ()));
+
+ for (int i = 0; i < num_n; ++i)
+ {
+ std::string cat = flann_models_[indices_scores[i].idx_models_].first.class_;
+ it = category_map.find (cat);
+ if (it == category_map.end ())
+ {
+ category_map[cat] = 1;
+ }
+ else
+ {
+ it->second++;
+ }
+ }
+
+ for (it = category_map.begin (); it != category_map.end (); it++)
+ {
+ float prob = static_cast<float> (it->second) / static_cast<float> (num_n);
+ categories_.push_back (it->first);
+ confidences_.push_back (prob);
+ }
+
+ }
+ else
+ {
+ first_nn_category_ = std::string ("error");
+ categories_.push_back (first_nn_category_);
+ }
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNPipeline<Distance, PointInT, FeatureT>::initialize (bool force_retrain)
+ {
+
+ //use the source to know what has to be trained and what not, checking if the descr_name directory exists
+ //unless force_retrain is true, then train everything
+ boost::shared_ptr < std::vector<ModelT> > models = source_->getModels ();
+ std::cout << "Models size:" << models->size () << std::endl;
+
+ if (force_retrain)
+ {
+ for (size_t i = 0; i < models->size (); i++)
+ {
+ source_->removeDescDirectory (models->at (i), training_dir_, descr_name_);
+ }
+ }
+
+ for (size_t i = 0; i < models->size (); i++)
+ {
+ if (!source_->modelAlreadyTrained (models->at (i), training_dir_, descr_name_))
+ {
+ for (size_t v = 0; v < models->at (i).views_->size (); v++)
+ {
+ PointInTPtr processed (new pcl::PointCloud<PointInT>);
+ //pro view, compute signatures
+ typename pcl::PointCloud<FeatureT>::CloudVectorType signatures;
+ std::vector < Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > centroids;
+ estimator_->estimate (models->at (i).views_->at (v), processed, signatures, centroids);
+
+ //source_->makeModelPersistent (models->at (i), training_dir_, descr_name_, static_cast<int> (v));
+ std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_);
+
+ bf::path desc_dir = path;
+ if (!bf::exists (desc_dir))
+ bf::create_directory (desc_dir);
+
+ std::stringstream path_view;
+ path_view << path << "/view_" << v << ".pcd";
+ pcl::io::savePCDFileBinary (path_view.str (), *processed);
+
+ std::stringstream path_pose;
+ path_pose << path << "/pose_" << v << ".txt";
+ PersistenceUtils::writeMatrixToFile (path_pose.str (), models->at (i).poses_->at (v));
+
+ std::stringstream path_entropy;
+ path_entropy << path << "/entropy_" << v << ".txt";
+ PersistenceUtils::writeFloatToFile (path_entropy.str (), models->at (i).self_occlusions_->at (v));
+
+ //save signatures and centroids to disk
+ for (size_t j = 0; j < signatures.size (); j++)
+ {
+ std::stringstream path_centroid;
+ path_centroid << path << "/centroid_" << v << "_" << j << ".txt";
+ Eigen::Vector3f centroid (centroids[j][0], centroids[j][1], centroids[j][2]);
+ PersistenceUtils::writeCentroidToFile (path_centroid.str (), centroid);
+
+ std::stringstream path_descriptor;
+ path_descriptor << path << "/descriptor_" << v << "_" << j << ".pcd";
+ pcl::io::savePCDFileBinary (path_descriptor.str (), signatures[j]);
+ }
+ }
+
+ }
+ else
+ {
+ //else skip model
+ std::cout << "The model has already been trained..." << std::endl;
+ }
+ }
+
+ //load features from disk
+ //initialize FLANN structure
+ loadFeaturesAndCreateFLANN ();
+ }
--- /dev/null
+/*
+ * global_nn_classifier.cpp
+ *
+ * Created on: Mar 9, 2012
+ * Author: aitor
+ */
+
+#include <pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_crh.h>
+#include <pcl/recognition/crh_alignment.h>
+#include <pcl/registration/icp.h>
+#include <boost/random.hpp>
+#include <boost/random/normal_distribution.hpp>
+#include <pcl/common/time.h>
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNCRHRecognizer<Distance, PointInT, FeatureT>::getPose (ModelT & model, int view_id, Eigen::Matrix4f & pose_matrix)
+ {
+
+ if (use_cache_)
+ {
+ typedef std::pair<std::string, int> mv_pair;
+ mv_pair pair_model_view = std::make_pair (model.id_, view_id);
+
+ std::map<mv_pair, Eigen::Matrix4f, std::less<mv_pair>, Eigen::aligned_allocator<std::pair<mv_pair, Eigen::Matrix4f> > >::iterator it =
+ poses_cache_.find (pair_model_view);
+
+ if (it != poses_cache_.end ())
+ {
+ pose_matrix = it->second;
+ return;
+ }
+
+ }
+
+ std::stringstream dir;
+ std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_);
+ dir << path << "/pose_" << view_id << ".txt";
+
+ PersistenceUtils::readMatrixFromFile (dir.str (), pose_matrix);
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNCRHRecognizer<Distance, PointInT, FeatureT>::getCRH (ModelT & model, int view_id, int d_id,
+ CRHPointCloud::Ptr & hist)
+ {
+
+ hist.reset (new CRHPointCloud);
+ std::stringstream dir;
+ std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_);
+ dir << path << "/crh_" << view_id << "_" << d_id << ".pcd";
+
+ pcl::io::loadPCDFile (dir.str (), *hist);
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNCRHRecognizer<Distance, PointInT, FeatureT>::getCentroid (ModelT & model, int view_id, int d_id,
+ Eigen::Vector3f & centroid)
+ {
+ std::stringstream dir;
+ std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_);
+ dir << path << "/centroid_" << view_id << "_" << d_id << ".txt";
+
+ PersistenceUtils::getCentroidFromFile (dir.str (), centroid);
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNCRHRecognizer<Distance, PointInT, FeatureT>::getView (ModelT & model, int view_id, PointInTPtr & view)
+ {
+ view.reset (new pcl::PointCloud<PointInT>);
+ std::stringstream dir;
+ std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_);
+ dir << path << "/view_" << view_id << ".pcd";
+ pcl::io::loadPCDFile (dir.str (), *view);
+
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNCRHRecognizer<Distance, PointInT, FeatureT>::loadFeaturesAndCreateFLANN ()
+ {
+ boost::shared_ptr < std::vector<ModelT> > models = source_->getModels ();
+ for (size_t i = 0; i < models->size (); i++)
+ {
+ std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_);
+ bf::path inside = path;
+ bf::directory_iterator end_itr;
+
+ for (bf::directory_iterator itr_in (inside); itr_in != end_itr; ++itr_in)
+ {
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string file_name = (itr_in->path ().filename ()).string();
+#else
+ std::string file_name = (itr_in->path ()).filename ();
+#endif
+
+ std::vector < std::string > strs;
+ boost::split (strs, file_name, boost::is_any_of ("_"));
+
+ if (strs[0] == "descriptor")
+ {
+
+ int view_id = atoi (strs[1].c_str ());
+ std::vector < std::string > strs1;
+ boost::split (strs1, strs[2], boost::is_any_of ("."));
+ int descriptor_id = atoi (strs1[0].c_str ());
+
+ std::string full_file_name = itr_in->path ().string ();
+ typename pcl::PointCloud<FeatureT>::Ptr signature (new pcl::PointCloud<FeatureT>);
+ pcl::io::loadPCDFile (full_file_name, *signature);
+
+ flann_model descr_model;
+ descr_model.model = models->at (i);
+ descr_model.view_id = view_id;
+ descr_model.descriptor_id = descriptor_id;
+
+ int size_feat = sizeof(signature->points[0].histogram) / sizeof(float);
+ descr_model.descr.resize (size_feat);
+ memcpy (&descr_model.descr[0], &signature->points[0].histogram[0], size_feat * sizeof(float));
+
+ flann_models_.push_back (descr_model);
+
+ if (use_cache_)
+ {
+
+ std::stringstream dir_pose;
+ dir_pose << path << "/pose_" << descr_model.view_id << ".txt";
+
+ Eigen::Matrix4f pose_matrix;
+ PersistenceUtils::readMatrixFromFile (dir_pose.str (), pose_matrix);
+
+ std::pair<std::string, int> pair_model_view = std::make_pair (models->at (i).id_, descr_model.view_id);
+ poses_cache_[pair_model_view] = pose_matrix;
+ }
+ }
+ }
+ }
+
+ convertToFLANN (flann_models_, flann_data_);
+ flann_index_ = new flann::Index<DistT> (flann_data_, flann::LinearIndexParams ());
+ flann_index_->buildIndex ();
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNCRHRecognizer<Distance, PointInT, FeatureT>::nearestKSearch (flann::Index<DistT> * index, const flann_model &model,
+ int k, flann::Matrix<int> &indices,
+ flann::Matrix<float> &distances)
+ {
+ flann::Matrix<float> p = flann::Matrix<float> (new float[model.descr.size ()], 1, model.descr.size ());
+ memcpy (&p.ptr ()[0], &model.descr[0], p.cols * p.rows * sizeof(float));
+
+ indices = flann::Matrix<int> (new int[k], 1, k);
+ distances = flann::Matrix<float> (new float[k], 1, k);
+ index->knnSearch (p, indices, distances, k, flann::SearchParams (512));
+ delete[] p.ptr ();
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNCRHRecognizer<Distance, PointInT, FeatureT>::recognize ()
+ {
+
+ models_.reset (new std::vector<ModelT>);
+ transforms_.reset (new std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >);
+
+ PointInTPtr processed (new pcl::PointCloud<PointInT>);
+ PointInTPtr in (new pcl::PointCloud<PointInT>);
+
+ std::vector<pcl::PointCloud<FeatureT>, Eigen::aligned_allocator<pcl::PointCloud<FeatureT> > > signatures;
+ std::vector < Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > centroids;
+
+ if (indices_.size ())
+ pcl::copyPointCloud (*input_, indices_, *in);
+ else
+ in = input_;
+
+ {
+ pcl::ScopeTime t ("Estimate feature");
+ crh_estimator_->estimate (in, processed, signatures, centroids);
+ }
+
+ std::vector<CRHPointCloud::Ptr> crh_histograms;
+ crh_estimator_->getCRHHistograms (crh_histograms);
+
+ std::vector<index_score> indices_scores;
+ if (signatures.size () > 0)
+ {
+
+ {
+ pcl::ScopeTime t_matching ("Matching and roll...");
+ for (size_t idx = 0; idx < signatures.size (); idx++)
+ {
+
+ float* hist = signatures[idx].points[0].histogram;
+ int size_feat = sizeof(signatures[idx].points[0].histogram) / sizeof(float);
+ std::vector<float> std_hist (hist, hist + size_feat);
+ ModelT empty;
+
+ flann_model histogram;
+ histogram.descr = std_hist;
+
+ flann::Matrix<int> indices;
+ flann::Matrix<float> distances;
+ nearestKSearch (flann_index_, histogram, NN_, indices, distances);
+
+ //gather NN-search results
+ double score = 0;
+ for (int i = 0; i < NN_; ++i)
+ {
+ score = distances[0][i];
+ index_score is;
+ is.idx_models_ = indices[0][i];
+ is.idx_input_ = static_cast<int> (idx);
+ is.score_ = score;
+ indices_scores.push_back (is);
+ }
+ }
+
+ std::sort (indices_scores.begin (), indices_scores.end (), sortIndexScoresOp);
+
+ int num_n = std::min (NN_, static_cast<int> (indices_scores.size ()));
+
+ /*
+ * Filter some hypothesis regarding to their distance to the first neighbour
+ */
+
+ /*std::vector<index_score> indices_scores_filtered;
+ indices_scores_filtered.resize (num_n);
+ indices_scores_filtered[0] = indices_scores[0];
+
+ float best_score = indices_scores[0].score_;
+ int kept = 1;
+ for (int i = 1; i < num_n; ++i)
+ {
+ std::cout << best_score << indices_scores[i].score_ << (best_score / indices_scores[i].score_) << std::endl;
+ if ((best_score / indices_scores[i].score_) > 0.75)
+ {
+ indices_scores_filtered[i] = indices_scores[i];
+ kept++;
+ }
+
+ //best_score = indices_scores[i].score_;
+ }
+
+ indices_scores_filtered.resize (kept);
+ std::cout << indices_scores_filtered.size () << " § " << num_n << std::endl;
+
+ indices_scores = indices_scores_filtered;
+ num_n = indices_scores.size ();*/
+
+ if (do_CRH_)
+ {
+ /*
+ * Once we have the models, we need to find a 6DOF pose using the roll histogram
+ * pass to pcl_recognition::CRHAlignment both views, centroids and CRH
+ */
+
+ pcl::CRHAlignment<PointInT, 90> crha;
+
+ for (int i = 0; i < num_n; ++i)
+ {
+ ModelT m = flann_models_[indices_scores[i].idx_models_].model;
+ int view_id = flann_models_[indices_scores[i].idx_models_].view_id;
+ int desc_id = flann_models_[indices_scores[i].idx_models_].descriptor_id;
+
+ std::cout << m.id_ << " " << view_id << " " << desc_id << std::endl;
+
+ //get crhs
+ CRHPointCloud::Ptr input_crh = crh_histograms[indices_scores[i].idx_input_];
+ CRHPointCloud::Ptr view_crh;
+ getCRH (m, view_id, desc_id, view_crh);
+
+ //get centroids
+ Eigen::Vector3f input_centroid = centroids[indices_scores[i].idx_input_];
+ Eigen::Vector3f view_centroid;
+ getCentroid (m, view_id, desc_id, view_centroid);
+
+ //crha.setModelAndInputView (view, processed);
+ crha.setInputAndTargetCentroids (view_centroid, input_centroid);
+ crha.align (*view_crh, *input_crh);
+
+ Eigen::Matrix4f model_view_pose;
+ getPose (m, view_id, model_view_pose);
+
+ std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > roll_transforms;
+ crha.getTransforms (roll_transforms);
+
+ //create object hypothesis
+ for (size_t k = 0; k < roll_transforms.size (); k++)
+ {
+ Eigen::Matrix4f final_roll_trans (roll_transforms[k] * model_view_pose);
+ models_->push_back (m);
+ transforms_->push_back (final_roll_trans);
+ }
+ }
+ }
+ else
+ {
+ for (int i = 0; i < num_n; ++i)
+ {
+ ModelT m = flann_models_[indices_scores[i].idx_models_].model;
+ models_->push_back (m);
+ }
+ }
+ }
+
+ std::cout << "Number of object hypotheses:" << models_->size () << std::endl;
+
+ /**
+ * POSE REFINEMENT
+ **/
+
+ if (ICP_iterations_ > 0)
+ {
+ pcl::ScopeTime t ("Pose refinement");
+
+ //Prepare scene and model clouds for the pose refinement step
+ float VOXEL_SIZE_ICP_ = 0.005f;
+ PointInTPtr cloud_voxelized_icp (new pcl::PointCloud<PointInT> ());
+ pcl::VoxelGrid<PointInT> voxel_grid_icp;
+ voxel_grid_icp.setInputCloud (processed);
+ voxel_grid_icp.setLeafSize (VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_);
+ voxel_grid_icp.filter (*cloud_voxelized_icp);
+ source_->voxelizeAllModels (VOXEL_SIZE_ICP_);
+
+#pragma omp parallel for num_threads(omp_get_num_procs())
+ for (int i = 0; i < static_cast<int> (models_->size ()); i++)
+ {
+
+ ConstPointInTPtr model_cloud = models_->at (i).getAssembled (VOXEL_SIZE_ICP_);
+ PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);
+ pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
+
+ pcl::IterativeClosestPoint<PointInT, PointInT> reg;
+ reg.setInputSource (model_aligned); //model
+ reg.setInputTarget (cloud_voxelized_icp); //scene
+ reg.setMaximumIterations (ICP_iterations_);
+ reg.setMaxCorrespondenceDistance (VOXEL_SIZE_ICP_ * 3.f);
+ reg.setTransformationEpsilon (1e-5);
+
+ typename pcl::PointCloud<PointInT>::Ptr output_ (new pcl::PointCloud<PointInT> ());
+ reg.align (*output_);
+
+ Eigen::Matrix4f icp_trans = reg.getFinalTransformation ();
+ transforms_->at (i) = icp_trans * transforms_->at (i);
+ }
+ }
+
+ /**
+ * HYPOTHESES VERIFICATION
+ **/
+
+ if (hv_algorithm_)
+ {
+
+ pcl::ScopeTime t ("HYPOTHESES VERIFICATION");
+
+ std::vector<typename pcl::PointCloud<PointInT>::ConstPtr> aligned_models;
+ aligned_models.resize (models_->size ());
+
+ for (size_t i = 0; i < models_->size (); i++)
+ {
+ ConstPointInTPtr model_cloud = models_->at (i).getAssembled (0.005f);
+ PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);
+ pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
+ aligned_models[i] = model_aligned;
+ }
+
+ std::vector<bool> mask_hv;
+ hv_algorithm_->setSceneCloud (input_);
+ hv_algorithm_->addModels (aligned_models, true);
+ hv_algorithm_->verify ();
+ hv_algorithm_->getMask (mask_hv);
+
+ boost::shared_ptr < std::vector<ModelT> > models_temp;
+ boost::shared_ptr < std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > > transforms_temp;
+
+ models_temp.reset (new std::vector<ModelT>);
+ transforms_temp.reset (new std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >);
+
+ for (size_t i = 0; i < models_->size (); i++)
+ {
+ if (!mask_hv[i])
+ continue;
+
+ models_temp->push_back (models_->at (i));
+ transforms_temp->push_back (transforms_->at (i));
+ }
+
+ models_ = models_temp;
+ transforms_ = transforms_temp;
+ }
+
+ }
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNCRHRecognizer<Distance, PointInT, FeatureT>::initialize (bool force_retrain)
+ {
+
+ //use the source to know what has to be trained and what not, checking if the descr_name directory exists
+ //unless force_retrain is true, then train everything
+ boost::shared_ptr < std::vector<ModelT> > models = source_->getModels ();
+ std::cout << "Models size:" << models->size () << std::endl;
+
+ if (force_retrain)
+ {
+ for (size_t i = 0; i < models->size (); i++)
+ {
+ source_->removeDescDirectory (models->at (i), training_dir_, descr_name_);
+ }
+ }
+
+ for (size_t i = 0; i < models->size (); i++)
+ {
+ if (!source_->modelAlreadyTrained (models->at (i), training_dir_, descr_name_))
+ {
+ for (size_t v = 0; v < models->at (i).views_->size (); v++)
+ {
+ PointInTPtr processed (new pcl::PointCloud<PointInT>);
+ PointInTPtr view = models->at (i).views_->at (v);
+
+ if (noisify_)
+ {
+ double noise_std = noise_;
+ boost::posix_time::ptime time = boost::posix_time::microsec_clock::local_time();
+ boost::posix_time::time_duration duration( time.time_of_day() );
+ boost::mt19937 rng;
+ rng.seed (static_cast<unsigned int> (duration.total_milliseconds()));
+ boost::normal_distribution<> nd (0.0, noise_std);
+ boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
+ // Noisify each point in the dataset
+ for (size_t cp = 0; cp < view->points.size (); ++cp)
+ view->points[cp].z += static_cast<float> (var_nor ());
+
+ }
+
+ //pro view, compute signatures and CRH
+ std::vector<pcl::PointCloud<FeatureT>, Eigen::aligned_allocator<pcl::PointCloud<FeatureT> > > signatures;
+ std::vector < Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > centroids;
+ crh_estimator_->estimate (view, processed, signatures, centroids);
+
+ std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_);
+
+ bf::path desc_dir = path;
+ if (!bf::exists (desc_dir))
+ bf::create_directory (desc_dir);
+
+ std::stringstream path_view;
+ path_view << path << "/view_" << v << ".pcd";
+ pcl::io::savePCDFileBinary (path_view.str (), *processed);
+
+ std::stringstream path_pose;
+ path_pose << path << "/pose_" << v << ".txt";
+ PersistenceUtils::writeMatrixToFile (path_pose.str (), models->at (i).poses_->at (v));
+
+ std::stringstream path_entropy;
+ path_entropy << path << "/entropy_" << v << ".txt";
+ PersistenceUtils::writeFloatToFile (path_entropy.str (), models->at (i).self_occlusions_->at (v));
+
+ std::vector<CRHPointCloud::Ptr> crh_histograms;
+ crh_estimator_->getCRHHistograms (crh_histograms);
+
+ //save signatures and centroids to disk
+ for (size_t j = 0; j < signatures.size (); j++)
+ {
+ std::stringstream path_centroid;
+ path_centroid << path << "/centroid_" << v << "_" << j << ".txt";
+ Eigen::Vector3f centroid (centroids[j][0], centroids[j][1], centroids[j][2]);
+ PersistenceUtils::writeCentroidToFile (path_centroid.str (), centroid);
+
+ std::stringstream path_descriptor;
+ path_descriptor << path << "/descriptor_" << v << "_" << j << ".pcd";
+ pcl::io::savePCDFileBinary (path_descriptor.str (), signatures[j]);
+
+ std::stringstream path_roll;
+ path_roll << path << "/crh_" << v << "_" << j << ".pcd";
+ pcl::io::savePCDFileBinary (path_roll.str (), *crh_histograms[j]);
+ }
+ }
+
+ }
+ else
+ {
+ //else skip model
+ std::cout << "The model has already been trained..." << std::endl;
+ }
+ }
+
+ //load features from disk
+ //initialize FLANN structure
+ loadFeaturesAndCreateFLANN ();
+ }
--- /dev/null
+/*
+ * global_nn_classifier.cpp
+ *
+ * Created on: Mar 9, 2012
+ * Author: aitor
+ */
+
+#include <pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_cvfh.h>
+#include <pcl/registration/icp.h>
+#include <boost/random.hpp>
+#include <boost/random/normal_distribution.hpp>
+#include <pcl/common/time.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Distance, PointInT, FeatureT>::getPose (ModelT & model, int view_id, Eigen::Matrix4f & pose_matrix)
+ {
+
+ if (use_cache_)
+ {
+ typedef std::pair<std::string, int> mv_pair;
+ mv_pair pair_model_view = std::make_pair (model.id_, view_id);
+
+ std::map<mv_pair, Eigen::Matrix4f, std::less<mv_pair>, Eigen::aligned_allocator<std::pair<mv_pair, Eigen::Matrix4f> > >::iterator it =
+ poses_cache_.find (pair_model_view);
+
+ if (it != poses_cache_.end ())
+ {
+ pose_matrix = it->second;
+ return;
+ }
+
+ }
+
+ std::stringstream dir;
+ std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_);
+ dir << path << "/pose_" << view_id << ".txt";
+
+ PersistenceUtils::readMatrixFromFile (dir.str (), pose_matrix);
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ bool
+ pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Distance, PointInT, FeatureT>::getRollPose (ModelT & model, int view_id, int d_id,
+ Eigen::Matrix4f & pose_matrix)
+ {
+
+ std::stringstream dir;
+ std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_);
+
+ dir << path << "/roll_trans_" << view_id << "_" << d_id << ".txt";
+
+ bf::path file_path = dir.str ();
+ if (bf::exists (file_path))
+ {
+ PersistenceUtils::readMatrixFromFile (dir.str (), pose_matrix);
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Distance, PointInT, FeatureT>::getCentroid (ModelT & model, int view_id, int d_id,
+ Eigen::Vector3f & centroid)
+ {
+ std::stringstream dir;
+ std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_);
+ dir << path << "/centroid_" << view_id << "_" << d_id << ".txt";
+
+ PersistenceUtils::getCentroidFromFile (dir.str (), centroid);
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Distance, PointInT, FeatureT>::getView (ModelT & model, int view_id, PointInTPtr & view)
+ {
+ view.reset (new pcl::PointCloud<PointInT>);
+ std::stringstream dir;
+ std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_);
+ dir << path << "/view_" << view_id << ".pcd";
+ pcl::io::loadPCDFile (dir.str (), *view);
+
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Distance, PointInT, FeatureT>::loadFeaturesAndCreateFLANN ()
+ {
+
+ boost::shared_ptr < std::vector<ModelT> > models = source_->getModels ();
+
+ std::map < std::string, boost::shared_ptr<std::vector<int> > > single_categories;
+ if (use_single_categories_)
+ {
+ for (size_t i = 0; i < models->size (); i++)
+ {
+ std::map<std::string, boost::shared_ptr<std::vector<int> > >::iterator it;
+ std::string cat_model = models->at (i).class_;
+ it = single_categories.find (cat_model);
+ if (it == single_categories.end ())
+ {
+ boost::shared_ptr < std::vector<int> > v (new std::vector<int>);
+ single_categories[cat_model] = v;
+ }
+ }
+ }
+
+ for (size_t i = 0; i < models->size (); i++)
+ {
+ std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_);
+ bf::path inside = path;
+ bf::directory_iterator end_itr;
+
+ for (bf::directory_iterator itr_in (inside); itr_in != end_itr; ++itr_in)
+ {
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string file_name = (itr_in->path ().filename ()).string();
+#else
+ std::string file_name = (itr_in->path ()).filename ();
+#endif
+
+ std::vector < std::string > strs;
+ boost::split (strs, file_name, boost::is_any_of ("_"));
+
+ if (strs[0] == "descriptor")
+ {
+
+ int view_id = atoi (strs[1].c_str ());
+ std::vector < std::string > strs1;
+ boost::split (strs1, strs[2], boost::is_any_of ("."));
+ int descriptor_id = atoi (strs1[0].c_str ());
+
+ std::string full_file_name = itr_in->path ().string ();
+ typename pcl::PointCloud<FeatureT>::Ptr signature (new pcl::PointCloud<FeatureT>);
+ pcl::io::loadPCDFile (full_file_name, *signature);
+
+ flann_model descr_model;
+ descr_model.model = models->at (i);
+ descr_model.view_id = view_id;
+ descr_model.descriptor_id = descriptor_id;
+
+ int size_feat = sizeof(signature->points[0].histogram) / sizeof(float);
+ descr_model.descr.resize (size_feat);
+ memcpy (&descr_model.descr[0], &signature->points[0].histogram[0], size_feat * sizeof(float));
+
+ if (use_single_categories_)
+ {
+ std::map<std::string, boost::shared_ptr<std::vector<int> > >::iterator it;
+ std::string cat_model = models->at (i).class_;
+ it = single_categories.find (cat_model);
+ if (it == single_categories.end ())
+ {
+ std::cout << cat_model << std::endl;
+ std::cout << "Should not happen..." << std::endl;
+ }
+ else
+ {
+ it->second->push_back (static_cast<int> (flann_models_.size ()));
+ }
+ }
+
+ flann_models_.push_back (descr_model);
+
+ if (use_cache_)
+ {
+
+ std::stringstream dir_pose;
+ dir_pose << path << "/pose_" << descr_model.view_id << ".txt";
+
+ Eigen::Matrix4f pose_matrix;
+ PersistenceUtils::readMatrixFromFile (dir_pose.str (), pose_matrix);
+
+ std::pair<std::string, int> pair_model_view = std::make_pair (models->at (i).id_, descr_model.view_id);
+ poses_cache_[pair_model_view] = pose_matrix;
+ }
+ }
+ }
+ }
+
+ convertToFLANN (flann_models_, flann_data_);
+ flann_index_ = new flann::Index<DistT> (flann_data_, flann::LinearIndexParams ());
+ flann_index_->buildIndex ();
+
+ //single categories...
+ if (use_single_categories_)
+ {
+ std::map<std::string, boost::shared_ptr<std::vector<int> > >::iterator it;
+
+ single_categories_data_.resize (single_categories.size ());
+ single_categories_index_.resize (single_categories.size ());
+ single_categories_pointers_to_models_.resize (single_categories.size ());
+
+ int kk = 0;
+ for (it = single_categories.begin (); it != single_categories.end (); it++)
+ {
+ //create index and flann data
+ convertToFLANN (flann_models_, it->second, single_categories_data_[kk]);
+ single_categories_index_[kk] = new flann::Index<DistT> (single_categories_data_[kk], flann::LinearIndexParams ());
+ single_categories_pointers_to_models_[kk] = it->second;
+
+ category_to_vectors_indices_[it->first] = kk;
+ kk++;
+ }
+ }
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Distance, PointInT, FeatureT>::nearestKSearch (flann::Index<DistT> * index, const flann_model &model,
+ int k, flann::Matrix<int> &indices,
+ flann::Matrix<float> &distances)
+ {
+ flann::Matrix<float> p = flann::Matrix<float> (new float[model.descr.size ()], 1, model.descr.size ());
+ memcpy (&p.ptr ()[0], &model.descr[0], p.cols * p.rows * sizeof(float));
+
+ indices = flann::Matrix<int> (new int[k], 1, k);
+ distances = flann::Matrix<float> (new float[k], 1, k);
+ index->knnSearch (p, indices, distances, k, flann::SearchParams (512));
+ delete[] p.ptr ();
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Distance, PointInT, FeatureT>::recognize ()
+ {
+
+ models_.reset (new std::vector<ModelT>);
+ transforms_.reset (new std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >);
+
+ PointInTPtr processed (new pcl::PointCloud<PointInT>);
+ PointInTPtr in (new pcl::PointCloud<PointInT>);
+
+ std::vector<pcl::PointCloud<FeatureT>, Eigen::aligned_allocator<pcl::PointCloud<FeatureT> > > signatures;
+ std::vector < Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > centroids;
+
+ if (indices_.size ())
+ pcl::copyPointCloud (*input_, indices_, *in);
+ else
+ in = input_;
+
+ {
+ pcl::ScopeTime t ("Estimate feature");
+ micvfh_estimator_->estimate (in, processed, signatures, centroids);
+ }
+
+ std::vector<index_score> indices_scores;
+ descriptor_distances_.clear ();
+
+ if (signatures.size () > 0)
+ {
+
+ {
+ pcl::ScopeTime t_matching ("Matching and roll...");
+
+ if (use_single_categories_ && (categories_to_be_searched_.size () > 0))
+ {
+
+ //perform search of the different signatures in the categories_to_be_searched_
+ for (size_t c = 0; c < categories_to_be_searched_.size (); c++)
+ {
+ std::cout << "Using category:" << categories_to_be_searched_[c] << std::endl;
+ for (size_t idx = 0; idx < signatures.size (); idx++)
+ {
+ /*float* hist = signatures[idx].points[0].histogram;
+ std::vector<float> std_hist (hist, hist + getHistogramLength (dummy));
+ flann_model histogram ("", std_hist);
+ flann::Matrix<int> indices;
+ flann::Matrix<float> distances;
+
+ std::map<std::string, int>::iterator it;
+ it = category_to_vectors_indices_.find (categories_to_be_searched_[c]);
+
+ assert (it != category_to_vectors_indices_.end ());
+ nearestKSearch (single_categories_index_[it->second], histogram, nmodels_, indices, distances);*/
+
+ float* hist = signatures[idx].points[0].histogram;
+ int size_feat = sizeof(signatures[idx].points[0].histogram) / sizeof(float);
+ std::vector<float> std_hist (hist, hist + size_feat);
+ //ModelT empty;
+
+ flann_model histogram;
+ histogram.descr = std_hist;
+ flann::Matrix<int> indices;
+ flann::Matrix<float> distances;
+
+ std::map<std::string, int>::iterator it;
+ it = category_to_vectors_indices_.find (categories_to_be_searched_[c]);
+ assert (it != category_to_vectors_indices_.end ());
+
+ nearestKSearch (single_categories_index_[it->second], histogram, NN_, indices, distances);
+ //gather NN-search results
+ double score = 0;
+ for (size_t i = 0; i < NN_; ++i)
+ {
+ score = distances[0][i];
+ index_score is;
+ is.idx_models_ = single_categories_pointers_to_models_[it->second]->at (indices[0][i]);
+ is.idx_input_ = static_cast<int> (idx);
+ is.score_ = score;
+ indices_scores.push_back (is);
+ }
+ }
+
+ //we cannot add more than nmodels per category, so sort here and remove offending ones...
+ std::sort (indices_scores.begin (), indices_scores.end (), sortIndexScoresOp);
+ indices_scores.resize ((c + 1) * NN_);
+ }
+ }
+ else
+ {
+ for (size_t idx = 0; idx < signatures.size (); idx++)
+ {
+
+ float* hist = signatures[idx].points[0].histogram;
+ int size_feat = sizeof(signatures[idx].points[0].histogram) / sizeof(float);
+ std::vector<float> std_hist (hist, hist + size_feat);
+ //ModelT empty;
+
+ flann_model histogram;
+ histogram.descr = std_hist;
+
+ flann::Matrix<int> indices;
+ flann::Matrix<float> distances;
+ nearestKSearch (flann_index_, histogram, NN_, indices, distances);
+
+ //gather NN-search results
+ double score = 0;
+ for (int i = 0; i < NN_; ++i)
+ {
+ score = distances[0][i];
+ index_score is;
+ is.idx_models_ = indices[0][i];
+ is.idx_input_ = static_cast<int> (idx);
+ is.score_ = score;
+ indices_scores.push_back (is);
+
+ //ModelT m = flann_models_[indices[0][i]].model;
+ }
+ }
+ }
+
+ std::sort (indices_scores.begin (), indices_scores.end (), sortIndexScoresOp);
+
+ /*
+ * There might be duplicated candidates, in those cases it makes sense to take
+ * the closer one in descriptor space
+ */
+
+ typename std::map<flann_model, bool> found;
+ typename std::map<flann_model, bool>::iterator it_map;
+ for (size_t i = 0; i < indices_scores.size (); i++)
+ {
+ flann_model m = flann_models_[indices_scores[i].idx_models_];
+ it_map = found.find (m);
+ if (it_map == found.end ())
+ {
+ indices_scores[found.size ()] = indices_scores[i];
+ found[m] = true;
+ }
+ }
+ indices_scores.resize (found.size ());
+
+ int num_n = std::min (NN_, static_cast<int> (indices_scores.size ()));
+
+ /*
+ * Filter some hypothesis regarding to their distance to the first neighbour
+ */
+
+ /*std::vector<index_score> indices_scores_filtered;
+ indices_scores_filtered.resize (num_n);
+ indices_scores_filtered[0] = indices_scores[0];
+
+ float best_score = indices_scores[0].score_;
+ int kept = 1;
+ for (int i = 1; i < num_n; ++i)
+ {
+ //std::cout << best_score << indices_scores[i].score_ << (best_score / indices_scores[i].score_) << std::endl;
+ if ((best_score / indices_scores[i].score_) > 0.65)
+ {
+ indices_scores_filtered[i] = indices_scores[i];
+ kept++;
+ }
+
+ //best_score = indices_scores[i].score_;
+ }
+
+ indices_scores_filtered.resize (kept);
+ //std::cout << indices_scores_filtered.size () << " § " << num_n << std::endl;
+
+ indices_scores = indices_scores_filtered;
+ num_n = indices_scores.size ();*/
+ std::cout << "Number of object hypotheses... " << num_n << std::endl;
+
+ std::vector<bool> valid_trans;
+ std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transformations;
+
+ micvfh_estimator_->getValidTransformsVec (valid_trans);
+ micvfh_estimator_->getTransformsVec (transformations);
+
+ for (int i = 0; i < num_n; ++i)
+ {
+ ModelT m = flann_models_[indices_scores[i].idx_models_].model;
+ int view_id = flann_models_[indices_scores[i].idx_models_].view_id;
+ int desc_id = flann_models_[indices_scores[i].idx_models_].descriptor_id;
+
+ int idx_input = indices_scores[i].idx_input_;
+
+ std::cout << m.class_ << "/" << m.id_ << " ==> " << indices_scores[i].score_ << std::endl;
+
+ Eigen::Matrix4f roll_view_pose;
+ bool roll_pose_found = getRollPose (m, view_id, desc_id, roll_view_pose);
+
+ if (roll_pose_found && valid_trans[idx_input])
+ {
+ Eigen::Matrix4f transposed = roll_view_pose.transpose ();
+
+ //std::cout << transposed << std::endl;
+
+ PointInTPtr view;
+ getView (m, view_id, view);
+
+ Eigen::Matrix4f model_view_pose;
+ getPose (m, view_id, model_view_pose);
+
+ Eigen::Matrix4f scale_mat;
+ scale_mat.setIdentity (4, 4);
+
+ if (compute_scale_)
+ {
+ //compute scale using the whole view
+ PointInTPtr view_transformed (new pcl::PointCloud<PointInT>);
+ Eigen::Matrix4f hom_from_OVC_to_CC;
+ hom_from_OVC_to_CC = transformations[idx_input].inverse () * transposed;
+ pcl::transformPointCloud (*view, *view_transformed, hom_from_OVC_to_CC);
+
+ Eigen::Vector3f input_centroid = centroids[indices_scores[i].idx_input_];
+ Eigen::Vector3f view_centroid;
+ getCentroid (m, view_id, desc_id, view_centroid);
+
+ Eigen::Vector4f cmatch4f (view_centroid[0], view_centroid[1], view_centroid[2], 0);
+ Eigen::Vector4f cinput4f (input_centroid[0], input_centroid[1], input_centroid[2], 0);
+
+ Eigen::Vector4f max_pt_input;
+ pcl::getMaxDistance (*processed, cinput4f, max_pt_input);
+ max_pt_input[3] = 0;
+ float max_dist_input = (cinput4f - max_pt_input).norm ();
+
+ //compute max dist for transformed model_view
+ pcl::getMaxDistance (*view, cmatch4f, max_pt_input);
+ max_pt_input[3] = 0;
+ float max_dist_view = (cmatch4f - max_pt_input).norm ();
+
+ cmatch4f = hom_from_OVC_to_CC * cmatch4f;
+ std::cout << max_dist_view << " " << max_dist_input << std::endl;
+
+ float scale_factor_view = max_dist_input / max_dist_view;
+ std::cout << "Scale factor:" << scale_factor_view << std::endl;
+
+ Eigen::Matrix4f center, center_inv;
+
+ center.setIdentity (4, 4);
+ center (0, 3) = -cinput4f[0];
+ center (1, 3) = -cinput4f[1];
+ center (2, 3) = -cinput4f[2];
+
+ center_inv.setIdentity (4, 4);
+ center_inv (0, 3) = cinput4f[0];
+ center_inv (1, 3) = cinput4f[1];
+ center_inv (2, 3) = cinput4f[2];
+
+ scale_mat (0, 0) = scale_factor_view;
+ scale_mat (1, 1) = scale_factor_view;
+ scale_mat (2, 2) = scale_factor_view;
+
+ scale_mat = center_inv * scale_mat * center;
+ }
+
+ Eigen::Matrix4f hom_from_OC_to_CC;
+ hom_from_OC_to_CC = scale_mat * transformations[idx_input].inverse () * transposed * model_view_pose;
+
+ models_->push_back (m);
+ transforms_->push_back (hom_from_OC_to_CC);
+ descriptor_distances_.push_back (static_cast<float> (indices_scores[i].score_));
+ }
+ else
+ {
+ PCL_WARN("The roll pose was not found, should use CRH here... \n");
+ }
+ }
+ }
+
+ std::cout << "Number of object hypotheses:" << models_->size () << std::endl;
+
+ /**
+ * POSE REFINEMENT
+ **/
+
+ if (ICP_iterations_ > 0)
+ {
+ pcl::ScopeTime t ("Pose refinement");
+
+ //Prepare scene and model clouds for the pose refinement step
+ float VOXEL_SIZE_ICP_ = 0.005f;
+ PointInTPtr cloud_voxelized_icp (new pcl::PointCloud<PointInT> ());
+
+ {
+ pcl::ScopeTime t ("Voxelize stuff...");
+ pcl::VoxelGrid<PointInT> voxel_grid_icp;
+ voxel_grid_icp.setInputCloud (processed);
+ voxel_grid_icp.setLeafSize (VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_);
+ voxel_grid_icp.filter (*cloud_voxelized_icp);
+ source_->voxelizeAllModels (VOXEL_SIZE_ICP_);
+ }
+
+#pragma omp parallel for num_threads(omp_get_num_procs())
+ for (int i = 0; i < static_cast<int> (models_->size ()); i++)
+ {
+
+ ConstPointInTPtr model_cloud;
+ PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);
+
+ if (compute_scale_)
+ {
+ model_cloud = models_->at (i).getAssembled (-1);
+ PointInTPtr model_aligned_m (new pcl::PointCloud<PointInT>);
+ pcl::transformPointCloud (*model_cloud, *model_aligned_m, transforms_->at (i));
+ pcl::VoxelGrid<PointInT> voxel_grid_icp;
+ voxel_grid_icp.setInputCloud (model_aligned_m);
+ voxel_grid_icp.setLeafSize (VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_);
+ voxel_grid_icp.filter (*model_aligned);
+ }
+ else
+ {
+ model_cloud = models_->at (i).getAssembled (VOXEL_SIZE_ICP_);
+ pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
+ }
+
+ pcl::IterativeClosestPoint<PointInT, PointInT> reg;
+ reg.setInputSource (model_aligned); //model
+ reg.setInputTarget (cloud_voxelized_icp); //scene
+ reg.setMaximumIterations (ICP_iterations_);
+ reg.setMaxCorrespondenceDistance (VOXEL_SIZE_ICP_ * 3.f);
+ reg.setTransformationEpsilon (1e-6);
+
+ typename pcl::PointCloud<PointInT>::Ptr output_ (new pcl::PointCloud<PointInT> ());
+ reg.align (*output_);
+
+ Eigen::Matrix4f icp_trans = reg.getFinalTransformation ();
+ transforms_->at (i) = icp_trans * transforms_->at (i);
+ }
+ }
+
+ /**
+ * HYPOTHESES VERIFICATION
+ **/
+
+ if (hv_algorithm_)
+ {
+
+ pcl::ScopeTime t ("HYPOTHESES VERIFICATION");
+
+ std::vector<typename pcl::PointCloud<PointInT>::ConstPtr> aligned_models;
+ aligned_models.resize (models_->size ());
+
+ for (size_t i = 0; i < models_->size (); i++)
+ {
+ ConstPointInTPtr model_cloud;
+ PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);
+
+ if (compute_scale_)
+ {
+ model_cloud = models_->at (i).getAssembled (-1);
+ PointInTPtr model_aligned_m (new pcl::PointCloud<PointInT>);
+ pcl::transformPointCloud (*model_cloud, *model_aligned_m, transforms_->at (i));
+ pcl::VoxelGrid<PointInT> voxel_grid_icp;
+ voxel_grid_icp.setInputCloud (model_aligned_m);
+ voxel_grid_icp.setLeafSize (0.005f, 0.005f, 0.005f);
+ voxel_grid_icp.filter (*model_aligned);
+ }
+ else
+ {
+ model_cloud = models_->at (i).getAssembled (0.005f);
+ pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
+ }
+
+ //ConstPointInTPtr model_cloud = models_->at (i).getAssembled (0.005f);
+ //PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);
+ //pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
+ aligned_models[i] = model_aligned;
+ }
+
+ std::vector<bool> mask_hv;
+ hv_algorithm_->setSceneCloud (input_);
+ hv_algorithm_->addModels (aligned_models, true);
+ hv_algorithm_->verify ();
+ hv_algorithm_->getMask (mask_hv);
+
+ boost::shared_ptr < std::vector<ModelT> > models_temp;
+ boost::shared_ptr < std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > > transforms_temp;
+
+ models_temp.reset (new std::vector<ModelT>);
+ transforms_temp.reset (new std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >);
+
+ for (size_t i = 0; i < models_->size (); i++)
+ {
+ if (!mask_hv[i])
+ continue;
+
+ models_temp->push_back (models_->at (i));
+ transforms_temp->push_back (transforms_->at (i));
+ }
+
+ models_ = models_temp;
+ transforms_ = transforms_temp;
+ }
+
+ }
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Distance, PointInT, FeatureT>::initialize (bool force_retrain)
+ {
+
+ //use the source to know what has to be trained and what not, checking if the descr_name directory exists
+ //unless force_retrain is true, then train everything
+ boost::shared_ptr < std::vector<ModelT> > models = source_->getModels ();
+ std::cout << "Models size:" << models->size () << std::endl;
+
+ if (force_retrain)
+ {
+ for (size_t i = 0; i < models->size (); i++)
+ {
+ source_->removeDescDirectory (models->at (i), training_dir_, descr_name_);
+ }
+ }
+
+ for (size_t i = 0; i < models->size (); i++)
+ {
+ if (!source_->modelAlreadyTrained (models->at (i), training_dir_, descr_name_))
+ {
+ for (size_t v = 0; v < models->at (i).views_->size (); v++)
+ {
+ PointInTPtr processed (new pcl::PointCloud<PointInT>);
+ PointInTPtr view = models->at (i).views_->at (v);
+
+ if (view->points.size () == 0)
+ PCL_WARN("View has no points!!!\n");
+
+ if (noisify_)
+ {
+ double noise_std = noise_;
+ boost::posix_time::ptime time = boost::posix_time::microsec_clock::local_time();
+ boost::posix_time::time_duration duration( time.time_of_day() );
+ boost::mt19937 rng;
+ rng.seed (static_cast<unsigned int> (duration.total_milliseconds()));
+ boost::normal_distribution<> nd (0.0, noise_std);
+ boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
+ // Noisify each point in the dataset
+ for (size_t cp = 0; cp < view->points.size (); ++cp)
+ view->points[cp].z += static_cast<float> (var_nor ());
+
+ }
+
+ //pro view, compute signatures
+ std::vector<pcl::PointCloud<FeatureT>, Eigen::aligned_allocator<pcl::PointCloud<FeatureT> > > signatures;
+ std::vector < Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > centroids;
+ micvfh_estimator_->estimate (view, processed, signatures, centroids);
+
+ std::vector<bool> valid_trans;
+ std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transforms;
+
+ micvfh_estimator_->getValidTransformsVec (valid_trans);
+ micvfh_estimator_->getTransformsVec (transforms);
+
+ std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_);
+
+ bf::path desc_dir = path;
+ if (!bf::exists (desc_dir))
+ bf::create_directory (desc_dir);
+
+ std::stringstream path_view;
+ path_view << path << "/view_" << v << ".pcd";
+ pcl::io::savePCDFileBinary (path_view.str (), *processed);
+
+ std::stringstream path_pose;
+ path_pose << path << "/pose_" << v << ".txt";
+ PersistenceUtils::writeMatrixToFile (path_pose.str (), models->at (i).poses_->at (v));
+
+ std::stringstream path_entropy;
+ path_entropy << path << "/entropy_" << v << ".txt";
+ PersistenceUtils::writeFloatToFile (path_entropy.str (), models->at (i).self_occlusions_->at (v));
+
+ //save signatures and centroids to disk
+ for (size_t j = 0; j < signatures.size (); j++)
+ {
+ if (valid_trans[j])
+ {
+ std::stringstream path_centroid;
+ path_centroid << path << "/centroid_" << v << "_" << j << ".txt";
+ Eigen::Vector3f centroid (centroids[j][0], centroids[j][1], centroids[j][2]);
+ PersistenceUtils::writeCentroidToFile (path_centroid.str (), centroid);
+
+ std::stringstream path_descriptor;
+ path_descriptor << path << "/descriptor_" << v << "_" << j << ".pcd";
+ pcl::io::savePCDFileBinary (path_descriptor.str (), signatures[j]);
+
+ //save roll transform
+ std::stringstream path_pose;
+ path_pose << path << "/roll_trans_" << v << "_" << j << ".txt";
+ PersistenceUtils::writeMatrixToFile (path_pose.str (), transforms[j]);
+ }
+ }
+ }
+
+ }
+ else
+ {
+ //else skip model
+ std::cout << "The model has already been trained..." << std::endl;
+ //there is no need to keep the views in memory once the model has been trained
+ models->at (i).views_->clear ();
+ }
+ }
+
+ //load features from disk
+ //initialize FLANN structure
+ loadFeaturesAndCreateFLANN ();
+ }
--- /dev/null
+#include <pcl/apps/3d_rec_framework/pipeline/local_recognizer.h>
+#include <pcl/apps/3d_rec_framework/utils/vtk_model_sampling.h>
+#include <pcl/registration/correspondence_rejection_sample_consensus.h>
+#include <pcl/registration/transformation_estimation_svd.h>
+#include <pcl/registration/icp.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/registration/transformation_estimation_point_to_plane_lls.h>
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::LocalRecognitionPipeline<Distance, PointInT, FeatureT>::loadFeaturesAndCreateFLANN ()
+ {
+ boost::shared_ptr < std::vector<ModelT> > models = source_->getModels ();
+ std::cout << "Models size:" << models->size () << std::endl;
+
+ for (size_t i = 0; i < models->size (); i++)
+ {
+ std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_);
+ bf::path inside = path;
+ bf::directory_iterator end_itr;
+
+ for (bf::directory_iterator itr_in (inside); itr_in != end_itr; ++itr_in)
+ {
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string file_name = (itr_in->path ().filename ()).string();
+#else
+ std::string file_name = (itr_in->path ()).filename ();
+#endif
+
+ std::vector < std::string > strs;
+ boost::split (strs, file_name, boost::is_any_of ("_"));
+
+ if (strs[0] == "descriptor")
+ {
+ std::string full_file_name = itr_in->path ().string ();
+ std::string name = file_name.substr (0, file_name.length () - 4);
+ std::vector < std::string > strs;
+ boost::split (strs, name, boost::is_any_of ("_"));
+
+ flann_model descr_model;
+ descr_model.model = models->at (i);
+ descr_model.view_id = atoi (strs[1].c_str ());
+
+ if (use_cache_)
+ {
+
+ std::stringstream dir_keypoints;
+ std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_);
+ dir_keypoints << path << "/keypoint_indices_" << descr_model.view_id << ".pcd";
+
+ std::stringstream dir_pose;
+ dir_pose << path << "/pose_" << descr_model.view_id << ".txt";
+
+ Eigen::Matrix4f pose_matrix;
+ PersistenceUtils::readMatrixFromFile (dir_pose.str (), pose_matrix);
+
+ std::pair<std::string, int> pair_model_view = std::make_pair (models->at (i).id_, descr_model.view_id);
+ poses_cache_[pair_model_view] = pose_matrix;
+
+ //load keypoints and save them to cache
+ typename pcl::PointCloud<PointInT>::Ptr keypoints (new pcl::PointCloud<PointInT> ());
+ pcl::io::loadPCDFile (dir_keypoints.str (), *keypoints);
+ keypoints_cache_[pair_model_view] = keypoints;
+ }
+
+ typename pcl::PointCloud<FeatureT>::Ptr signature (new pcl::PointCloud<FeatureT> ());
+ pcl::io::loadPCDFile (full_file_name, *signature);
+
+ int size_feat = sizeof(signature->points[0].histogram) / sizeof(float);
+
+ for (size_t dd = 0; dd < signature->points.size (); dd++)
+ {
+ descr_model.keypoint_id = static_cast<int> (dd);
+ descr_model.descr.resize (size_feat);
+
+ memcpy (&descr_model.descr[0], &signature->points[dd].histogram[0], size_feat * sizeof(float));
+
+ flann_models_.push_back (descr_model);
+ }
+ }
+ }
+ }
+
+ convertToFLANN (flann_models_, flann_data_);
+
+ flann_index_ = new flann::Index<DistT> (flann_data_, flann::KDTreeIndexParams (4));
+ flann_index_->buildIndex ();
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::LocalRecognitionPipeline<Distance, PointInT, FeatureT>::nearestKSearch (flann::Index<DistT> * index,
+ const flann_model &model, int k,
+ flann::Matrix<int> &indices,
+ flann::Matrix<float> &distances)
+ {
+ flann::Matrix<float> p = flann::Matrix<float> (new float[model.descr.size ()], 1, model.descr.size ());
+ memcpy (&p.ptr ()[0], &model.descr[0], p.cols * p.rows * sizeof(float));
+
+ indices = flann::Matrix<int> (new int[k], 1, k);
+ distances = flann::Matrix<float> (new float[k], 1, k);
+ index->knnSearch (p, indices, distances, k, flann::SearchParams (kdtree_splits_));
+ delete[] p.ptr ();
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::LocalRecognitionPipeline<Distance, PointInT, FeatureT>::initialize (bool force_retrain)
+ {
+ boost::shared_ptr < std::vector<ModelT> > models;
+
+ if(search_model_.compare("") == 0) {
+ models = source_->getModels ();
+ } else {
+ models = source_->getModels (search_model_);
+ //reset cache and flann structures
+ if(flann_index_ != 0)
+ delete flann_index_;
+
+ flann_models_.clear();
+ poses_cache_.clear();
+ keypoints_cache_.clear();
+ }
+
+ std::cout << "Models size:" << models->size () << std::endl;
+
+ if (force_retrain)
+ {
+ for (size_t i = 0; i < models->size (); i++)
+ {
+ source_->removeDescDirectory (models->at (i), training_dir_, descr_name_);
+ }
+ }
+
+ for (size_t i = 0; i < models->size (); i++)
+ {
+ std::cout << models->at (i).class_ << " " << models->at (i).id_ << std::endl;
+
+ if (!source_->modelAlreadyTrained (models->at (i), training_dir_, descr_name_))
+ {
+ for (size_t v = 0; v < models->at (i).views_->size (); v++)
+ {
+ PointInTPtr processed (new pcl::PointCloud<PointInT>);
+ typename pcl::PointCloud<FeatureT>::Ptr signatures (new pcl::PointCloud<FeatureT> ());
+ PointInTPtr keypoints_pointcloud;
+
+ bool success = estimator_->estimate (models->at (i).views_->at (v), processed, keypoints_pointcloud, signatures);
+
+ if (success)
+ {
+ std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_);
+
+ bf::path desc_dir = path;
+ if (!bf::exists (desc_dir))
+ bf::create_directory (desc_dir);
+
+ std::stringstream path_view;
+ path_view << path << "/view_" << v << ".pcd";
+ pcl::io::savePCDFileBinary (path_view.str (), *processed);
+
+ std::stringstream path_pose;
+ path_pose << path << "/pose_" << v << ".txt";
+ PersistenceUtils::writeMatrixToFile (path_pose.str (), models->at (i).poses_->at (v));
+
+ if(v < models->at (i).self_occlusions_->size()) {
+ std::stringstream path_entropy;
+ path_entropy << path << "/entropy_" << v << ".txt";
+ PersistenceUtils::writeFloatToFile (path_entropy.str (), models->at (i).self_occlusions_->at (v));
+ }
+
+ //save keypoints and signatures to disk
+ std::stringstream keypoints_sstr;
+ keypoints_sstr << path << "/keypoint_indices_" << v << ".pcd";
+
+ /*boost::shared_ptr < std::vector<int> > indices (new std::vector<int> ());
+ indices->resize (keypoints.points.size ());
+ for (size_t kk = 0; kk < indices->size (); kk++)
+ (*indices)[kk] = keypoints.points[kk];
+ typename pcl::PointCloud<PointInT> keypoints_pointcloud;
+ pcl::copyPointCloud (*processed, *indices, keypoints_pointcloud);*/
+ pcl::io::savePCDFileBinary (keypoints_sstr.str (), *keypoints_pointcloud);
+
+ std::stringstream path_descriptor;
+ path_descriptor << path << "/descriptor_" << v << ".pcd";
+ pcl::io::savePCDFileBinary (path_descriptor.str (), *signatures);
+ }
+ }
+ } else {
+ std::cout << "Model already trained..." << std::endl;
+ //there is no need to keep the views in memory once the model has been trained
+ models->at (i).views_->clear();
+ }
+ }
+
+ loadFeaturesAndCreateFLANN ();
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::LocalRecognitionPipeline<Distance, PointInT, FeatureT>::recognize ()
+ {
+
+ models_.reset (new std::vector<ModelT>);
+ transforms_.reset (new std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >);
+
+ PointInTPtr processed;
+ typename pcl::PointCloud<FeatureT>::Ptr signatures (new pcl::PointCloud<FeatureT> ());
+ //pcl::PointCloud<int> keypoints_input;
+ PointInTPtr keypoints_pointcloud;
+
+ if (signatures_ != 0 && processed_ != 0 && (signatures_->size () == keypoints_pointcloud->points.size ()))
+ {
+ keypoints_pointcloud = keypoints_input_;
+ signatures = signatures_;
+ processed = processed_;
+ std::cout << "Using the ISPK ..." << std::endl;
+ }
+ else
+ {
+ processed.reset( (new pcl::PointCloud<PointInT>));
+ if (indices_.size () > 0)
+ {
+ PointInTPtr sub_input (new pcl::PointCloud<PointInT>);
+ pcl::copyPointCloud (*input_, indices_, *sub_input);
+ estimator_->estimate (sub_input, processed, keypoints_pointcloud, signatures);
+ }
+ else
+ {
+ estimator_->estimate (input_, processed, keypoints_pointcloud, signatures);
+ }
+
+ processed_ = processed;
+
+ }
+
+ std::cout << "Number of keypoints:" << keypoints_pointcloud->points.size () << std::endl;
+
+ int size_feat = sizeof(signatures->points[0].histogram) / sizeof(float);
+
+ //feature matching and object hypotheses
+ std::map<std::string, ObjectHypothesis> object_hypotheses;
+ {
+ for (size_t idx = 0; idx < signatures->points.size (); idx++)
+ {
+ float* hist = signatures->points[idx].histogram;
+ std::vector<float> std_hist (hist, hist + size_feat);
+ flann_model histogram;
+ histogram.descr = std_hist;
+ flann::Matrix<int> indices;
+ flann::Matrix<float> distances;
+ nearestKSearch (flann_index_, histogram, 1, indices, distances);
+
+ //read view pose and keypoint coordinates, transform keypoint coordinates to model coordinates
+ Eigen::Matrix4f homMatrixPose;
+ getPose (flann_models_.at (indices[0][0]).model, flann_models_.at (indices[0][0]).view_id, homMatrixPose);
+
+ typename pcl::PointCloud<PointInT>::Ptr keypoints (new pcl::PointCloud<PointInT> ());
+ getKeypoints (flann_models_.at (indices[0][0]).model, flann_models_.at (indices[0][0]).view_id, keypoints);
+
+ PointInT view_keypoint = keypoints->points[flann_models_.at (indices[0][0]).keypoint_id];
+ PointInT model_keypoint;
+ model_keypoint.getVector4fMap () = homMatrixPose.inverse () * view_keypoint.getVector4fMap ();
+
+ typename std::map<std::string, ObjectHypothesis>::iterator it_map;
+ if ((it_map = object_hypotheses.find (flann_models_.at (indices[0][0]).model.id_)) != object_hypotheses.end ())
+ {
+ //if the object hypothesis already exists, then add information
+ ObjectHypothesis oh = (*it_map).second;
+ oh.correspondences_pointcloud->points.push_back (model_keypoint);
+ oh.correspondences_to_inputcloud->push_back (
+ pcl::Correspondence (static_cast<int> (oh.correspondences_pointcloud->points.size () - 1),
+ static_cast<int> (idx), distances[0][0]));
+ oh.feature_distances_->push_back (distances[0][0]);
+
+ }
+ else
+ {
+ //create object hypothesis
+ ObjectHypothesis oh;
+
+ typename pcl::PointCloud<PointInT>::Ptr correspondences_pointcloud (new pcl::PointCloud<PointInT> ());
+ correspondences_pointcloud->points.push_back (model_keypoint);
+
+ oh.model_ = flann_models_.at (indices[0][0]).model;
+ oh.correspondences_pointcloud = correspondences_pointcloud;
+ //last keypoint for this model is a correspondence the current scene keypoint
+
+ pcl::CorrespondencesPtr corr (new pcl::Correspondences ());
+ oh.correspondences_to_inputcloud = corr;
+ oh.correspondences_to_inputcloud->push_back (pcl::Correspondence (0, static_cast<int> (idx), distances[0][0]));
+
+ boost::shared_ptr < std::vector<float> > feat_dist (new std::vector<float>);
+ feat_dist->push_back (distances[0][0]);
+
+ oh.feature_distances_ = feat_dist;
+ object_hypotheses[oh.model_.id_] = oh;
+ }
+ }
+ }
+
+ typename std::map<std::string, ObjectHypothesis>::iterator it_map;
+
+ std::vector<float> feature_distance_avg;
+
+ {
+ //pcl::ScopeTime t("Geometric verification, RANSAC and transform estimation");
+ for (it_map = object_hypotheses.begin (); it_map != object_hypotheses.end (); it_map++)
+ {
+ std::vector < pcl::Correspondences > corresp_clusters;
+ cg_algorithm_->setSceneCloud (keypoints_pointcloud);
+ cg_algorithm_->setInputCloud ((*it_map).second.correspondences_pointcloud);
+ cg_algorithm_->setModelSceneCorrespondences ((*it_map).second.correspondences_to_inputcloud);
+ cg_algorithm_->cluster (corresp_clusters);
+
+ std::cout << "Instances:" << corresp_clusters.size () << " Total correspondences:" << (*it_map).second.correspondences_to_inputcloud->size () << " " << it_map->first << std::endl;
+ std::vector<bool> good_indices_for_hypothesis (corresp_clusters.size (), true);
+
+ if (threshold_accept_model_hypothesis_ < 1.f)
+ {
+ //sort the hypotheses for each model according to their correspondences and take those that are threshold_accept_model_hypothesis_ over the max cardinality
+ int max_cardinality = -1;
+ for (size_t i = 0; i < corresp_clusters.size (); i++)
+ {
+ //std::cout << (corresp_clusters[i]).size() << " -- " << (*(*it_map).second.correspondences_to_inputcloud).size() << std::endl;
+ if (max_cardinality < static_cast<int> (corresp_clusters[i].size ()))
+ {
+ max_cardinality = static_cast<int> (corresp_clusters[i].size ());
+ }
+ }
+
+ for (size_t i = 0; i < corresp_clusters.size (); i++)
+ {
+ if (static_cast<float> ((corresp_clusters[i]).size ()) < (threshold_accept_model_hypothesis_ * static_cast<float> (max_cardinality)))
+ {
+ good_indices_for_hypothesis[i] = false;
+ }
+ }
+ }
+
+ for (size_t i = 0; i < corresp_clusters.size (); i++)
+ {
+
+ if (!good_indices_for_hypothesis[i])
+ continue;
+
+ //drawCorrespondences (processed, it_map->second, keypoints_pointcloud, corresp_clusters[i]);
+
+ Eigen::Matrix4f best_trans;
+ typename pcl::registration::TransformationEstimationSVD < PointInT, PointInT > t_est;
+ t_est.estimateRigidTransformation (*(*it_map).second.correspondences_pointcloud, *keypoints_pointcloud, corresp_clusters[i], best_trans);
+
+ models_->push_back ((*it_map).second.model_);
+ transforms_->push_back (best_trans);
+
+ }
+ }
+ }
+
+ std::cout << "Number of hypotheses:" << models_->size() << std::endl;
+
+ /**
+ * POSE REFINEMENT
+ **/
+
+ if (ICP_iterations_ > 0)
+ {
+ pcl::ScopeTime ticp ("ICP ");
+
+ //Prepare scene and model clouds for the pose refinement step
+ PointInTPtr cloud_voxelized_icp (new pcl::PointCloud<PointInT> ());
+ pcl::VoxelGrid<PointInT> voxel_grid_icp;
+ voxel_grid_icp.setInputCloud (processed);
+ voxel_grid_icp.setLeafSize (VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_);
+ voxel_grid_icp.filter (*cloud_voxelized_icp);
+ source_->voxelizeAllModels (VOXEL_SIZE_ICP_);
+
+#pragma omp parallel for schedule(dynamic,1) num_threads(omp_get_num_procs())
+ for (int i = 0; i < static_cast<int>(models_->size ()); i++)
+ {
+
+ ConstPointInTPtr model_cloud;
+ PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);
+ model_cloud = models_->at (i).getAssembled (VOXEL_SIZE_ICP_);
+ pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
+
+ typename pcl::registration::CorrespondenceRejectorSampleConsensus<PointInT>::Ptr rej (
+ new pcl::registration::CorrespondenceRejectorSampleConsensus<PointInT> ());
+
+ rej->setInputTarget (cloud_voxelized_icp);
+ rej->setMaximumIterations (1000);
+ rej->setInlierThreshold (0.005f);
+ rej->setInputSource (model_aligned);
+
+ pcl::IterativeClosestPoint<PointInT, PointInT> reg;
+ reg.addCorrespondenceRejector (rej);
+ reg.setInputTarget (cloud_voxelized_icp); //scene
+ reg.setInputSource (model_aligned); //model
+ reg.setMaximumIterations (ICP_iterations_);
+ reg.setMaxCorrespondenceDistance (VOXEL_SIZE_ICP_ * 4.f);
+
+ typename pcl::PointCloud<PointInT>::Ptr output_ (new pcl::PointCloud<PointInT> ());
+ reg.align (*output_);
+
+ Eigen::Matrix4f icp_trans = reg.getFinalTransformation ();
+ transforms_->at (i) = icp_trans * transforms_->at (i);
+ }
+ }
+
+ /**
+ * HYPOTHESES VERIFICATION
+ **/
+
+ if (hv_algorithm_)
+ {
+
+ pcl::ScopeTime thv ("HV verification");
+
+ std::vector<typename pcl::PointCloud<PointInT>::ConstPtr> aligned_models;
+ aligned_models.resize (models_->size ());
+ for (size_t i = 0; i < models_->size (); i++)
+ {
+ ConstPointInTPtr model_cloud = models_->at (i).getAssembled (0.0025f);
+ //ConstPointInTPtr model_cloud = models_->at (i).getAssembled (VOXEL_SIZE_ICP_);
+ PointInTPtr model_aligned (new pcl::PointCloud<PointInT>);
+ pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i));
+ aligned_models[i] = model_aligned;
+ }
+
+ std::vector<bool> mask_hv;
+ hv_algorithm_->setSceneCloud (input_);
+ hv_algorithm_->addModels (aligned_models, true);
+ hv_algorithm_->verify ();
+ hv_algorithm_->getMask (mask_hv);
+
+ boost::shared_ptr < std::vector<ModelT> > models_temp;
+ boost::shared_ptr < std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > > transforms_temp;
+
+ models_temp.reset (new std::vector<ModelT>);
+ transforms_temp.reset (new std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >);
+
+ for (size_t i = 0; i < models_->size (); i++)
+ {
+ if (!mask_hv[i])
+ continue;
+
+ models_temp->push_back (models_->at (i));
+ transforms_temp->push_back (transforms_->at (i));
+ }
+
+ models_ = models_temp;
+ transforms_ = transforms_temp;
+
+ }
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::LocalRecognitionPipeline<Distance, PointInT, FeatureT>::getPose (ModelT & model, int view_id, Eigen::Matrix4f & pose_matrix)
+ {
+
+ if (use_cache_)
+ {
+ typedef std::pair<std::string, int> mv_pair;
+ mv_pair pair_model_view = std::make_pair (model.id_, view_id);
+
+ std::map<mv_pair, Eigen::Matrix4f, std::less<mv_pair>, Eigen::aligned_allocator<std::pair<mv_pair, Eigen::Matrix4f> > >::iterator it =
+ poses_cache_.find (pair_model_view);
+
+ if (it != poses_cache_.end ())
+ {
+ pose_matrix = it->second;
+ return;
+ }
+
+ }
+
+ std::stringstream dir;
+ std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_);
+ dir << path << "/pose_" << view_id << ".txt";
+
+ PersistenceUtils::readMatrixFromFile (dir.str (), pose_matrix);
+ }
+
+template<template<class > class Distance, typename PointInT, typename FeatureT>
+ void
+ pcl::rec_3d_framework::LocalRecognitionPipeline<Distance, PointInT, FeatureT>::getKeypoints (
+ ModelT & model,
+ int view_id,
+ typename pcl::PointCloud<PointInT>::Ptr & keypoints_cloud)
+ {
+
+ if (use_cache_)
+ {
+ std::pair<std::string, int> pair_model_view = std::make_pair (model.id_, view_id);
+ typename std::map<std::pair<std::string, int>, PointInTPtr>::iterator it = keypoints_cache_.find (pair_model_view);
+
+ if (it != keypoints_cache_.end ())
+ {
+ keypoints_cloud = it->second;
+ return;
+ }
+
+ }
+
+ std::stringstream dir;
+ std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_);
+ dir << path << "/keypoint_indices_" << view_id << ".pcd";
+
+ pcl::io::loadPCDFile (dir.str (), *keypoints_cloud);
+ }
--- /dev/null
+/*
+ * local_recognizer.h
+ *
+ * Created on: Mar 24, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_LOCAL_RECOGNIZER_H_
+#define REC_FRAMEWORK_LOCAL_RECOGNIZER_H_
+
+//#include <opencv2/opencv.hpp>
+#include <flann/flann.h>
+#include <pcl/common/common.h>
+#include <pcl/apps/3d_rec_framework/pc_source/source.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h>
+#include <pcl/recognition/cg/correspondence_grouping.h>
+#include <pcl/recognition/hv/hypotheses_verification.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+inline bool
+correspSorter (const pcl::Correspondence & i, const pcl::Correspondence & j)
+{
+ return (i.distance < j.distance);
+}
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+ /**
+ * \brief Object recognition + 6DOF pose based on local features, GC and HV
+ * Contains keypoints/local features computation, matching using FLANN,
+ * point-to-point correspondence grouping, pose refinement and hypotheses verification
+ * Available features: SHOT, FPFH
+ * See apps/3d_rec_framework/tools/apps for usage
+ * \author Aitor Aldoma, Federico Tombari
+ */
+
+ template<template<class > class Distance, typename PointInT, typename FeatureT>
+ class PCL_EXPORTS LocalRecognitionPipeline
+ {
+
+ typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
+ typedef typename pcl::PointCloud<PointInT>::ConstPtr ConstPointInTPtr;
+
+ typedef Distance<float> DistT;
+ typedef Model<PointInT> ModelT;
+
+ /** \brief Directory where the trained structure will be saved */
+ std::string training_dir_;
+
+ /** \brief Point cloud to be classified */
+ PointInTPtr input_;
+
+ /** \brief Model data source */
+ typename boost::shared_ptr<Source<PointInT> > source_;
+
+ /** \brief Computes a feature */
+ typename boost::shared_ptr<LocalEstimator<PointInT, FeatureT> > estimator_;
+
+ /** \brief Point-to-point correspondence grouping algorithm */
+ typename boost::shared_ptr<CorrespondenceGrouping<PointInT, PointInT> > cg_algorithm_;
+
+ /** \brief Hypotheses verification algorithm */
+ typename boost::shared_ptr<HypothesisVerification<PointInT, PointInT> > hv_algorithm_;
+
+ /** \brief Descriptor name */
+ std::string descr_name_;
+
+ /** \brief Id of the model to be used */
+ std::string search_model_;
+
+ bool compute_table_plane_;
+
+ class flann_model
+ {
+ public:
+ ModelT model;
+ int view_id;
+ int keypoint_id;
+ std::vector<float> descr;
+ };
+
+ flann::Matrix<float> flann_data_;
+ flann::Index<DistT> * flann_index_;
+ std::vector<flann_model> flann_models_;
+
+ std::vector<int> indices_;
+
+ bool use_cache_;
+ std::map<std::pair<std::string, int>, Eigen::Matrix4f, std::less<std::pair<std::string, int> >, Eigen::aligned_allocator<std::pair<std::pair<
+ std::string, int>, Eigen::Matrix4f> > > poses_cache_;
+ std::map<std::pair<std::string, int>, typename pcl::PointCloud<PointInT>::Ptr> keypoints_cache_;
+
+ float threshold_accept_model_hypothesis_;
+ int ICP_iterations_;
+
+ boost::shared_ptr<std::vector<ModelT> > models_;
+ boost::shared_ptr<std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > > transforms_;
+
+ int kdtree_splits_;
+ float VOXEL_SIZE_ICP_;
+
+ PointInTPtr keypoints_input_;
+ PointInTPtr processed_;
+ typename pcl::PointCloud<FeatureT>::Ptr signatures_;
+
+ //load features from disk and create flann structure
+ void
+ loadFeaturesAndCreateFLANN ();
+
+ inline void
+ convertToFLANN (const std::vector<flann_model> &models, flann::Matrix<float> &data)
+ {
+ data.rows = models.size ();
+ data.cols = models[0].descr.size (); // number of histogram bins
+
+ flann::Matrix<float> flann_data (new float[models.size () * models[0].descr.size ()], models.size (), models[0].descr.size ());
+
+ for (size_t i = 0; i < data.rows; ++i)
+ for (size_t j = 0; j < data.cols; ++j)
+ {
+ flann_data.ptr ()[i * data.cols + j] = models[i].descr[j];
+ }
+
+ data = flann_data;
+ }
+
+ void
+ nearestKSearch (flann::Index<DistT> * index, const flann_model &model, int k, flann::Matrix<int> &indices, flann::Matrix<float> &distances);
+
+ class ObjectHypothesis
+ {
+ public:
+ ModelT model_;
+ typename pcl::PointCloud<PointInT>::Ptr correspondences_pointcloud; //points in model coordinates
+ boost::shared_ptr<std::vector<float> > feature_distances_;
+ pcl::CorrespondencesPtr correspondences_to_inputcloud; //indices between correspondences_pointcloud and scene cloud
+ };
+
+ void
+ getPose (ModelT & model, int view_id, Eigen::Matrix4f & pose_matrix);
+
+ void
+ getKeypoints (ModelT & model, int view_id, typename pcl::PointCloud<PointInT>::Ptr & keypoints_cloud);
+
+ void
+ drawCorrespondences (PointInTPtr & cloud, ObjectHypothesis & oh, PointInTPtr & keypoints_pointcloud, pcl::Correspondences & correspondences)
+ {
+ pcl::visualization::PCLVisualizer vis_corresp_;
+ vis_corresp_.setWindowName("correspondences...");
+ pcl::visualization::PointCloudColorHandlerCustom<PointInT> random_handler (cloud, 255, 0, 0);
+ vis_corresp_.addPointCloud<PointInT> (cloud, random_handler, "points");
+
+ typename pcl::PointCloud<PointInT>::ConstPtr cloud_sampled;
+ cloud_sampled = oh.model_.getAssembled (0.0025f);
+
+ pcl::visualization::PointCloudColorHandlerCustom<PointInT> random_handler_sampled (cloud_sampled, 0, 0, 255);
+ vis_corresp_.addPointCloud<PointInT> (cloud_sampled, random_handler_sampled, "sampled");
+
+ for (size_t kk = 0; kk < correspondences.size (); kk++)
+ {
+ pcl::PointXYZ p;
+ p.getVector4fMap () = oh.correspondences_pointcloud->points[correspondences[kk].index_query].getVector4fMap ();
+ pcl::PointXYZ p_scene;
+ p_scene.getVector4fMap () = keypoints_pointcloud->points[correspondences[kk].index_match].getVector4fMap ();
+
+ std::stringstream line_name;
+ line_name << "line_" << kk;
+
+ vis_corresp_.addLine<pcl::PointXYZ, pcl::PointXYZ> (p_scene, p, line_name.str ());
+ }
+
+ vis_corresp_.spin ();
+ vis_corresp_.removeAllPointClouds();
+ vis_corresp_.removeAllShapes();
+ vis_corresp_.close();
+ }
+
+ public:
+
+ LocalRecognitionPipeline ()
+ {
+ use_cache_ = false;
+ threshold_accept_model_hypothesis_ = 0.2f;
+ ICP_iterations_ = 30;
+ kdtree_splits_ = 512;
+ search_model_ = "";
+ VOXEL_SIZE_ICP_ = 0.0025f;
+ compute_table_plane_ = false;
+ }
+
+ void setISPK(typename pcl::PointCloud<FeatureT>::Ptr & signatures, PointInTPtr & p, PointInTPtr & keypoints)
+ {
+ keypoints_input_ = keypoints;
+ signatures_ = signatures;
+ processed_ = p;
+ }
+
+ void setVoxelSizeICP(float s) {
+ VOXEL_SIZE_ICP_ = s;
+ }
+ void
+ setSearchModel (std::string & id)
+ {
+ search_model_ = id;
+ }
+
+ void
+ setThresholdAcceptHyp (float t)
+ {
+ threshold_accept_model_hypothesis_ = t;
+ }
+
+ ~LocalRecognitionPipeline ()
+ {
+
+ }
+
+ void
+ setKdtreeSplits (int n)
+ {
+ kdtree_splits_ = n;
+ }
+
+ void
+ setIndices (std::vector<int> & indices)
+ {
+ indices_ = indices;
+ }
+
+ void
+ setICPIterations (int it)
+ {
+ ICP_iterations_ = it;
+ }
+
+ void
+ setUseCache (bool u)
+ {
+ use_cache_ = u;
+ }
+
+ boost::shared_ptr<std::vector<ModelT> >
+ getModels ()
+ {
+ return models_;
+ }
+
+ boost::shared_ptr<std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > >
+ getTransforms ()
+ {
+ return transforms_;
+ }
+
+ /**
+ * \brief Sets the model data source_
+ */
+ void
+ setDataSource (typename boost::shared_ptr<Source<PointInT> > & source)
+ {
+ source_ = source;
+ }
+
+ typename boost::shared_ptr<Source<PointInT> >
+ getDataSource ()
+ {
+ return source_;
+ }
+
+ /**
+ * \brief Sets the local feature estimator
+ */
+ void
+ setFeatureEstimator (typename boost::shared_ptr<LocalEstimator<PointInT, FeatureT> > & feat)
+ {
+ estimator_ = feat;
+ }
+
+ /**
+ * \brief Sets the CG algorithm
+ */
+ void
+ setCGAlgorithm (typename boost::shared_ptr<CorrespondenceGrouping<PointInT, PointInT> > & alg)
+ {
+ cg_algorithm_ = alg;
+ }
+
+ /**
+ * \brief Sets the HV algorithm
+ */
+ void
+ setHVAlgorithm (typename boost::shared_ptr<HypothesisVerification<PointInT, PointInT> > & alg)
+ {
+ hv_algorithm_ = alg;
+ }
+
+ /**
+ * \brief Sets the input cloud to be classified
+ */
+ void
+ setInputCloud (const PointInTPtr & cloud)
+ {
+ input_ = cloud;
+ }
+
+ /**
+ * \brief Sets the descriptor name
+ */
+ void
+ setDescriptorName (std::string & name)
+ {
+ descr_name_ = name;
+ }
+
+ /**
+ * \brief Filesystem dir where to keep the generated training data
+ */
+ void
+ setTrainingDir (std::string & dir)
+ {
+ training_dir_ = dir;
+ }
+
+ void
+ setComputeTablePlane(bool b) {
+ compute_table_plane_ = b;
+ }
+
+ void
+ getProcessed(PointInTPtr & cloud) {
+ cloud = processed_;
+ }
+
+ /**
+ * \brief Initializes the FLANN structure from the provided source
+ * It does training for the models that havent been trained yet
+ */
+
+ void
+ initialize (bool force_retrain = false);
+
+ /**
+ * \brief Performs recognition and pose estimation on the input cloud
+ */
+
+ void
+ recognize ();
+ };
+ }
+}
+
+#endif /* REC_FRAMEWORK_LOCAL_RECOGNIZER_H_ */
--- /dev/null
+#ifndef OPENNI_CAPTURE_H
+#define OPENNI_CAPTURE_H
+
+#include <pcl/io/openni_grabber.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+namespace OpenNIFrameSource
+{
+
+ typedef pcl::PointXYZRGBA PointT;
+ typedef pcl::PointCloud<PointT> PointCloud;
+ typedef pcl::PointCloud<PointT>::Ptr PointCloudPtr;
+ typedef pcl::PointCloud<PointT>::ConstPtr PointCloudConstPtr;
+
+ /* A simple class for capturing data from an OpenNI camera */
+ class PCL_EXPORTS OpenNIFrameSource
+ {
+ public:
+ OpenNIFrameSource (const std::string& device_id = "");
+ ~OpenNIFrameSource ();
+
+ const PointCloudPtr
+ snap ();
+ bool
+ isActive ();
+ void
+ onKeyboardEvent (const pcl::visualization::KeyboardEvent & event);
+
+ protected:
+ void
+ onNewFrame (const PointCloudConstPtr &cloud);
+
+ pcl::OpenNIGrabber grabber_;
+ PointCloudPtr most_recent_frame_;
+ int frame_counter_;
+ boost::mutex mutex_;
+ bool active_;
+ };
+
+}
+
+#endif
--- /dev/null
+/*
+ * metrics.h
+ *
+ * Created on: Jun 22, 2011
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_METRICS_H_
+#define REC_FRAMEWORK_METRICS_H_
+
+#include <cmath>
+#include <cstdlib>
+
+namespace Metrics
+{
+ using ::std::abs;
+
+ template<typename T>
+ struct Accumulator
+ {
+ typedef T Type;
+ };
+
+ template<>
+ struct Accumulator<unsigned char>
+ {
+ typedef float Type;
+ };
+ template<>
+ struct Accumulator<unsigned short>
+ {
+ typedef float Type;
+ };
+ template<>
+ struct Accumulator<unsigned int>
+ {
+ typedef float Type;
+ };
+ template<>
+ struct Accumulator<char>
+ {
+ typedef float Type;
+ };
+ template<>
+ struct Accumulator<short>
+ {
+ typedef float Type;
+ };
+ template<>
+ struct Accumulator<int>
+ {
+ typedef float Type;
+ };
+
+ template<class T>
+ struct HistIntersectionUnionDistance
+ {
+ typedef T ElementType;
+ typedef typename Accumulator<T>::Type ResultType;
+
+ /**
+ * Compute a distance between two vectors using (1 - (1 + sum(min(a_i,b_i))) / (1 + sum(max(a_i, b_i))) )
+ *
+ * This distance is not a valid kdtree distance, it's not dimensionwise additive
+ * and ignores worst_dist parameter.
+ */
+
+ template<typename Iterator1, typename Iterator2>
+ ResultType
+ operator() (Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
+ {
+ (void)worst_dist;
+ ResultType result = ResultType ();
+ ResultType min0, min1, min2, min3;
+ ResultType max0, max1, max2, max3;
+ Iterator1 last = a + size;
+ Iterator1 lastgroup = last - 3;
+
+ ResultType sum_min, sum_max;
+ sum_min = 0;
+ sum_max = 0;
+
+ while (a < lastgroup)
+ {
+ min0 = (a[0] < b[0] ? a[0] : b[0]);
+ max0 = (a[0] > b[0] ? a[0] : b[0]);
+ min1 = (a[1] < b[1] ? a[1] : b[1]);
+ max1 = (a[1] > b[1] ? a[1] : b[1]);
+ min2 = (a[2] < b[2] ? a[2] : b[2]);
+ max2 = (a[2] > b[2] ? a[2] : b[2]);
+ min3 = (a[3] < b[3] ? a[3] : b[3]);
+ max3 = (a[3] > b[3] ? a[3] : b[3]);
+ sum_min += min0 + min1 + min2 + min3;
+ sum_max += max0 + max1 + max2 + max3;
+ a += 4;
+ b += 4;
+ /*if (worst_dist>0 && result>worst_dist) {
+ return result;
+ }*/
+ }
+
+ while (a < last)
+ {
+ min0 = *a < *b ? *a : *b;
+ max0 = *a > *b ? *a : *b;
+ sum_min += min0;
+ sum_max += max0;
+ a++;
+ b++;
+ //std::cout << a << " " << last << std::endl;
+ }
+
+ result = static_cast<ResultType> (1.0 - ((1 + sum_min) / (1 + sum_max)));
+ return result;
+ }
+
+ /* This distance functor is not dimension-wise additive, which
+ * makes it an invalid kd-tree distance, not implementing the accum_dist method */
+ /**
+ * Partial distance, used by the kd-tree.
+ */
+ template<typename U, typename V>
+ inline ResultType
+ accum_dist (const U& a, const V& b, int dim) const
+ {
+ //printf("New code being used, accum_dist\n");
+ ResultType min0;
+ ResultType max0;
+ min0 = (a < b ? a : b) + 1.0;
+ max0 = (a > b ? a : b) + 1.0;
+ return (1 - (min0 / max0));
+ //return (a+b-2*(a<b?a:b));
+ }
+ };
+}
+
+#endif /* REC_FRAMEWORK_METRICS_H_ */
--- /dev/null
+#include <fstream>
+#include <boost/filesystem.hpp>
+#include <boost/algorithm/string.hpp>
+#include <pcl/io/pcd_io.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+ namespace PersistenceUtils
+ {
+
+ inline bool
+ writeCentroidToFile (std::string file, Eigen::Vector3f & centroid)
+ {
+ std::ofstream out (file.c_str ());
+ if (!out)
+ {
+ std::cout << "Cannot open file.\n";
+ return false;
+ }
+
+ out << centroid[0] << " " << centroid[1] << " " << centroid[2] << std::endl;
+ out.close ();
+
+ return true;
+ }
+
+ inline bool
+ getCentroidFromFile (std::string file, Eigen::Vector3f & centroid)
+ {
+ std::ifstream in;
+ in.open (file.c_str (), std::ifstream::in);
+
+ char linebuf[256];
+ in.getline (linebuf, 256);
+ std::string line (linebuf);
+ std::vector < std::string > strs;
+ boost::split (strs, line, boost::is_any_of (" "));
+ centroid[0] = static_cast<float> (atof (strs[0].c_str ()));
+ centroid[1] = static_cast<float> (atof (strs[1].c_str ()));
+ centroid[2] = static_cast<float> (atof (strs[2].c_str ()));
+
+ return true;
+ }
+
+ inline bool
+ writeMatrixToFile (std::string file, Eigen::Matrix4f & matrix)
+ {
+ std::ofstream out (file.c_str ());
+ if (!out)
+ {
+ std::cout << "Cannot open file.\n";
+ return false;
+ }
+
+ for (size_t i = 0; i < 4; i++)
+ {
+ for (size_t j = 0; j < 4; j++)
+ {
+ out << matrix (i, j);
+ if (!(i == 3 && j == 3))
+ out << " ";
+ }
+ }
+ out.close ();
+
+ return true;
+ }
+
+ inline bool
+ writeFloatToFile (std::string file, float value)
+ {
+ std::ofstream out (file.c_str ());
+ if (!out)
+ {
+ std::cout << "Cannot open file.\n";
+ return false;
+ }
+
+ out << value;
+ out.close ();
+
+ return true;
+ }
+
+ inline std::string
+ getViewId (std::string id)
+ {
+ //descriptor_xxx_xx.pcd
+ //and we want xxx
+
+ std::vector < std::string > strs;
+ boost::split (strs, id, boost::is_any_of ("_"));
+
+ //std::cout << "id:" << id << std::endl;
+ //std::cout << "returned:" << strs[strs.size() - 2] << std::endl;
+
+ return strs[strs.size () - 2];
+ }
+
+ inline bool
+ readFloatFromFile (std::string dir, std::string file, float& value)
+ {
+
+ std::vector < std::string > strs;
+ boost::split (strs, file, boost::is_any_of ("/"));
+
+ std::string str;
+ for (size_t i = 0; i < (strs.size () - 1); i++)
+ {
+ str += strs[i] + "/";
+ }
+
+ std::stringstream matrix_file;
+ matrix_file << dir << "/" << str << "entropy_" << getViewId (file) << ".txt";
+
+ std::ifstream in;
+ in.open (matrix_file.str ().c_str (), std::ifstream::in);
+
+ char linebuf[1024];
+ in.getline (linebuf, 1024);
+ value = static_cast<float> (atof (linebuf));
+
+ return true;
+ }
+
+ inline bool
+ readFloatFromFile (std::string file, float& value)
+ {
+
+ std::ifstream in;
+ in.open (file.c_str (), std::ifstream::in);
+
+ char linebuf[1024];
+ in.getline (linebuf, 1024);
+ value = static_cast<float> (atof (linebuf));
+
+ return true;
+ }
+
+ inline bool
+ readMatrixFromFile (std::string dir, std::string file, Eigen::Matrix4f & matrix)
+ {
+
+ //get the descriptor name from dir
+ std::vector < std::string > path;
+ boost::split (path, dir, boost::is_any_of ("/"));
+
+ std::string dname = path[path.size () - 1];
+ std::string file_replaced;
+ for (size_t i = 0; i < (path.size () - 1); i++)
+ {
+ file_replaced += path[i] + "/";
+ }
+
+ boost::split (path, file, boost::is_any_of ("/"));
+ std::string id;
+
+ for (size_t i = 0; i < (path.size () - 1); i++)
+ {
+ id += path[i];
+ if (i < (path.size () - 1))
+ {
+ id += "/";
+ }
+ }
+
+ boost::split (path, file, boost::is_any_of ("/"));
+ std::string filename = path[path.size () - 1];
+
+ std::stringstream matrix_file;
+ matrix_file << file_replaced << id << "/" << dname << "/pose_" << getViewId (file) << ".txt";
+ //std::cout << matrix_file.str() << std::endl;
+
+ std::ifstream in;
+ in.open (matrix_file.str ().c_str (), std::ifstream::in);
+
+ char linebuf[1024];
+ in.getline (linebuf, 1024);
+ std::string line (linebuf);
+ std::vector < std::string > strs_2;
+ boost::split (strs_2, line, boost::is_any_of (" "));
+
+ for (int i = 0; i < 16; i++)
+ {
+ matrix (i % 4, i / 4) = static_cast<float> (atof (strs_2[i].c_str ()));
+ }
+
+ return true;
+ }
+
+ inline bool
+ readMatrixFromFile (std::string file, Eigen::Matrix4f & matrix)
+ {
+
+ std::ifstream in;
+ in.open (file.c_str (), std::ifstream::in);
+
+ char linebuf[1024];
+ in.getline (linebuf, 1024);
+ std::string line (linebuf);
+ std::vector < std::string > strs_2;
+ boost::split (strs_2, line, boost::is_any_of (" "));
+
+ for (int i = 0; i < 16; i++)
+ {
+ matrix (i % 4, i / 4) = static_cast<float> (atof (strs_2[i].c_str ()));
+ }
+
+ return true;
+ }
+
+ inline bool
+ readMatrixFromFile2 (std::string file, Eigen::Matrix4f & matrix)
+ {
+
+ std::ifstream in;
+ in.open (file.c_str (), std::ifstream::in);
+
+ char linebuf[1024];
+ in.getline (linebuf, 1024);
+ std::string line (linebuf);
+ std::vector < std::string > strs_2;
+ boost::split (strs_2, line, boost::is_any_of (" "));
+
+ for (int i = 0; i < 16; i++)
+ {
+ matrix (i / 4, i % 4) = static_cast<float> (atof (strs_2[i].c_str ()));
+ }
+
+ return true;
+ }
+
+ template<typename PointInT>
+ inline
+ void
+ getPointCloudFromFile (std::string dir, std::string file, typename pcl::PointCloud<PointInT>::Ptr & cloud)
+ {
+
+ //get the descriptor name from dir
+ std::vector < std::string > path;
+ boost::split (path, dir, boost::is_any_of ("/"));
+
+ std::string dname = path[path.size () - 1];
+ std::string file_replaced;
+ for (size_t i = 0; i < (path.size () - 1); i++)
+ {
+ file_replaced += path[i] + "/";
+ }
+
+ boost::split (path, file, boost::is_any_of ("/"));
+ std::string id;
+
+ for (size_t i = 0; i < (path.size () - 1); i++)
+ {
+ id += path[i];
+ if (i < (path.size () - 1))
+ {
+ id += "/";
+ }
+ }
+
+ std::stringstream view_file;
+ view_file << file_replaced << id << "/" << dname << "/view_" << getViewId (file) << ".pcd";
+
+ pcl::io::loadPCDFile (view_file.str (), *cloud);
+ }
+ }
+ }
+}
+
--- /dev/null
+/*
+ * uniform_sampling.h
+ *
+ * Created on: Mar 25, 2012
+ * Author: aitor
+ */
+
+#ifndef REC_FRAMEWORK_UNIFORM_SAMPLING_H_
+#define REC_FRAMEWORK_UNIFORM_SAMPLING_H_
+
+#include <vtkPolyData.h>
+#include <vtkTriangle.h>
+#include <vtkSmartPointer.h>
+#include <vtkCellArray.h>
+#include <vtkPLYReader.h>
+#include <vtkPolyDataMapper.h>
+#include <vtkTransform.h>
+#include <vtkTransformFilter.h>
+#include <pcl/common/common.h>
+
+namespace pcl
+{
+ namespace rec_3d_framework
+ {
+
+ inline double
+ uniform_deviate (int seed)
+ {
+ double ran = seed * (1.0 / (RAND_MAX + 1.0));
+ return ran;
+ }
+
+ inline void
+ randomPointTriangle (double a1, double a2, double a3, double b1, double b2, double b3, double c1, double c2, double c3, Eigen::Vector4f& p)
+ {
+ float r1 = static_cast<float> (uniform_deviate (rand ()));
+ float r2 = static_cast<float> (uniform_deviate (rand ()));
+ float r1sqr = sqrtf (r1);
+ float OneMinR1Sqr = (1 - r1sqr);
+ float OneMinR2 = (1 - r2);
+ a1 *= OneMinR1Sqr;
+ a2 *= OneMinR1Sqr;
+ a3 *= OneMinR1Sqr;
+ b1 *= OneMinR2;
+ b2 *= OneMinR2;
+ b3 *= OneMinR2;
+ c1 = r1sqr * (r2 * c1 + b1) + a1;
+ c2 = r1sqr * (r2 * c2 + b2) + a2;
+ c3 = r1sqr * (r2 * c3 + b3) + a3;
+ p[0] = static_cast<float> (c1);
+ p[1] = static_cast<float> (c2);
+ p[2] = static_cast<float> (c3);
+ p[3] = 0.f;
+ }
+
+ inline void
+ randPSurface (vtkPolyData * polydata, std::vector<double> * cumulativeAreas, double totalArea, Eigen::Vector4f& p)
+ {
+ float r = static_cast<float> (uniform_deviate (rand ()) * totalArea);
+
+ std::vector<double>::iterator low = std::lower_bound (cumulativeAreas->begin (), cumulativeAreas->end (), r);
+ vtkIdType el = static_cast<vtkIdType> (low - cumulativeAreas->begin ());
+
+ double A[3], B[3], C[3];
+ vtkIdType npts = 0;
+ vtkIdType *ptIds = NULL;
+ polydata->GetCellPoints (el, npts, ptIds);
+
+ if (ptIds == NULL)
+ return;
+
+ polydata->GetPoint (ptIds[0], A);
+ polydata->GetPoint (ptIds[1], B);
+ polydata->GetPoint (ptIds[2], C);
+ randomPointTriangle (A[0], A[1], A[2], B[0], B[1], B[2], C[0], C[1], C[2], p);
+ }
+
+ template<typename PointT>
+ inline void
+ uniform_sampling (vtkSmartPointer<vtkPolyData> polydata, size_t n_samples, typename pcl::PointCloud<PointT> & cloud_out)
+ {
+ polydata->BuildCells ();
+ vtkSmartPointer < vtkCellArray > cells = polydata->GetPolys ();
+
+ double p1[3], p2[3], p3[3], totalArea = 0;
+ std::vector<double> cumulativeAreas (cells->GetNumberOfCells (), 0);
+ size_t i = 0;
+ vtkIdType npts = 0, *ptIds = NULL;
+ for (cells->InitTraversal (); cells->GetNextCell (npts, ptIds); i++)
+ {
+ polydata->GetPoint (ptIds[0], p1);
+ polydata->GetPoint (ptIds[1], p2);
+ polydata->GetPoint (ptIds[2], p3);
+ totalArea += vtkTriangle::TriangleArea (p1, p2, p3);
+ cumulativeAreas[i] = totalArea;
+ }
+
+ cloud_out.points.resize (n_samples);
+ cloud_out.width = static_cast<int> (n_samples);
+ cloud_out.height = 1;
+
+ for (i = 0; i < n_samples; i++)
+ {
+ Eigen::Vector4f p (0.f, 0.f, 0.f, 0.f);
+ randPSurface (polydata, &cumulativeAreas, totalArea, p);
+ cloud_out.points[i].x = static_cast<float> (p[0]);
+ cloud_out.points[i].y = static_cast<float> (p[1]);
+ cloud_out.points[i].z = static_cast<float> (p[2]);
+ }
+ }
+
+ template<typename PointT>
+ inline void
+ uniform_sampling (std::string & file, size_t n_samples, typename pcl::PointCloud<PointT> & cloud_out, float scale = 1.f)
+ {
+
+ vtkSmartPointer < vtkPLYReader > reader = vtkSmartPointer<vtkPLYReader>::New ();
+ reader->SetFileName (file.c_str ());
+
+ vtkSmartPointer < vtkPolyDataMapper > mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
+
+ if (scale == 1.f)
+ {
+ mapper->SetInputConnection (reader->GetOutputPort ());
+ mapper->Update ();
+ }
+ else
+ {
+ vtkSmartPointer<vtkTransform> trans = vtkSmartPointer<vtkTransform>::New ();
+ trans->Scale (scale, scale, scale);
+ vtkSmartPointer < vtkTransformFilter > trans_filter = vtkSmartPointer<vtkTransformFilter>::New ();
+ trans_filter->SetTransform (trans);
+ trans_filter->SetInputConnection (reader->GetOutputPort ());
+ trans_filter->Update ();
+ mapper->SetInputConnection (trans_filter->GetOutputPort ());
+ mapper->Update ();
+ }
+
+ vtkSmartPointer<vtkPolyData> poly = mapper->GetInput ();
+
+ uniform_sampling (poly, n_samples, cloud_out);
+
+ }
+
+ inline void
+ getVerticesAsPointCloud (vtkSmartPointer<vtkPolyData> polydata, pcl::PointCloud<pcl::PointXYZ> & cloud_out)
+ {
+ vtkPoints *points = polydata->GetPoints ();
+ cloud_out.points.resize (points->GetNumberOfPoints ());
+ cloud_out.width = static_cast<int> (cloud_out.points.size ());
+ cloud_out.height = 1;
+ cloud_out.is_dense = false;
+
+ for (int i = 0; i < points->GetNumberOfPoints (); i++)
+ {
+ double p[3];
+ points->GetPoint (i, p);
+ cloud_out.points[i].x = static_cast<float> (p[0]);
+ cloud_out.points[i].y = static_cast<float> (p[1]);
+ cloud_out.points[i].z = static_cast<float> (p[2]);
+ }
+ }
+ }
+}
+
+#endif /* REC_FRAMEWORK_UNIFORM_SAMPLING_H_ */
--- /dev/null
+/*
+ * global_nn_classifier.cpp
+ *
+ * Created on: Mar 9, 2012
+ * Author: aitor
+ */
+
+#include <pcl/apps/3d_rec_framework/pipeline/impl/global_nn_classifier.hpp>
+#include "pcl/apps/3d_rec_framework/utils/metrics.h"
+
+//Instantiation
+template class pcl::rec_3d_framework::GlobalNNPipeline<flann::L1, pcl::PointXYZ, pcl::VFHSignature308>;
+template class pcl::rec_3d_framework::GlobalNNPipeline<Metrics::HistIntersectionUnionDistance, pcl::PointXYZ, pcl::VFHSignature308>;
+template class pcl::rec_3d_framework::GlobalNNPipeline<flann::L1, pcl::PointXYZ, pcl::ESFSignature640>;
+
+template class pcl::rec_3d_framework::GlobalClassifier<pcl::PointXYZ>;
--- /dev/null
+/*
+ * global_nn_classifier.cpp
+ *
+ * Created on: Mar 9, 2012
+ * Author: aitor
+ */
+
+#include <pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_crh.hpp>
+#include "pcl/apps/3d_rec_framework/utils/metrics.h"
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Histogram<90>,
+ (float[90], histogram, histogram90)
+)
+
+//Instantiation
+template class pcl::rec_3d_framework::GlobalNNCRHRecognizer<flann::L2, pcl::PointXYZ, pcl::VFHSignature308>;
+template class pcl::rec_3d_framework::GlobalNNCRHRecognizer<flann::L1, pcl::PointXYZ, pcl::VFHSignature308>;
+template class pcl::rec_3d_framework::GlobalNNCRHRecognizer<Metrics::HistIntersectionUnionDistance, pcl::PointXYZ, pcl::VFHSignature308>;
+template class pcl::rec_3d_framework::GlobalNNCRHRecognizer<flann::ChiSquareDistance, pcl::PointXYZ, pcl::VFHSignature308>;
+template class pcl::rec_3d_framework::GlobalNNCRHRecognizer<flann::L1, pcl::PointXYZ, pcl::ESFSignature640>;
+template class pcl::rec_3d_framework::GlobalNNCRHRecognizer<flann::L2, pcl::PointXYZ, pcl::ESFSignature640>;
--- /dev/null
+/*
+ * global_nn_classifier.cpp
+ *
+ * Created on: Mar 9, 2012
+ * Author: aitor
+ */
+
+#include <pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_cvfh.hpp>
+#include "pcl/apps/3d_rec_framework/utils/metrics.h"
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Histogram<367>,
+ (float[367], histogram, histogram367)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Histogram<431>,
+ (float[431], histogram, histogram431)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Histogram<559>,
+ (float[559], histogram, histogram559)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Histogram<815>,
+ (float[815], histogram, histogram815)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Histogram<879>,
+ (float[879], histogram, histogram879)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Histogram<1327>,
+ (float[1327], histogram, histogram1327)
+)
+
+
+//Instantiation
+//template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer<flann::L2, pcl::PointXYZ, pcl::VFHSignature308>;
+//template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer<flann::L1, pcl::PointXYZ, pcl::VFHSignature308>;
+template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Metrics::HistIntersectionUnionDistance, pcl::PointXYZ, pcl::VFHSignature308>;
+template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Metrics::HistIntersectionUnionDistance, pcl::PointXYZRGB, pcl::VFHSignature308>;
+template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Metrics::HistIntersectionUnionDistance, pcl::PointXYZRGB, pcl::Histogram<431> >;
+template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Metrics::HistIntersectionUnionDistance, pcl::PointXYZRGB, pcl::Histogram<559> >;
+//template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Metrics::HistIntersectionUnionDistance, pcl::PointXYZRGB, pcl::Histogram<815> >;
+//template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Metrics::HistIntersectionUnionDistance, pcl::PointXYZRGB, pcl::Histogram<879> >;
+template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer<Metrics::HistIntersectionUnionDistance, pcl::PointXYZRGB, pcl::Histogram<1327> >;
+//template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer<flann::L2, pcl::PointXYZRGB, pcl::Histogram<431> >;
+//template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer<flann::L2, pcl::PointXYZRGB, pcl::Histogram<431> >;
+//template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer<flann::L1, pcl::PointXYZRGB, pcl::Histogram<431> >;
--- /dev/null
+#include <pcl/apps/3d_rec_framework/pipeline/impl/local_recognizer.hpp>
+
+//This stuff is needed to be able to make the SHOT histograms persistent
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Histogram<352>,
+ (float[352], histogram, histogram352)
+)
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Histogram<1344>,
+ (float[1344], histogram, histogram1344)
+)
+
+template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline<flann::L1, pcl::PointXYZ, pcl::Histogram<352> >;
+template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline<flann::L1, pcl::PointXYZRGB, pcl::Histogram<352> >;
+template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline<flann::L1, pcl::PointXYZRGB, pcl::Histogram<1344> >;
+template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline<flann::L1, pcl::PointXYZ, pcl::FPFHSignature33>;
+template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline<flann::L1, pcl::PointXYZRGB, pcl::FPFHSignature33>;
+
+template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline<flann::L2, pcl::PointXYZ, pcl::Histogram<352> >;
+template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline<flann::L2, pcl::PointXYZRGB, pcl::Histogram<352> >;
+template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline<flann::L2, pcl::PointXYZRGB, pcl::Histogram<1344> >;
+template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline<flann::L2, pcl::PointXYZ, pcl::FPFHSignature33>;
+template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline<flann::L2, pcl::PointXYZRGB, pcl::FPFHSignature33>;
--- /dev/null
+#include "pcl/apps/3d_rec_framework/tools/openni_frame_source.h"
+#include <pcl/io/pcd_io.h>
+#include <boost/thread/mutex.hpp>
+#include <boost/make_shared.hpp>
+
+namespace OpenNIFrameSource
+{
+
+ OpenNIFrameSource::OpenNIFrameSource (const std::string& device_id) :
+ grabber_ (device_id), most_recent_frame_ (), frame_counter_ (0), active_ (true)
+ {
+ boost::function<void
+ (const PointCloudConstPtr&)> frame_cb = boost::bind (&OpenNIFrameSource::onNewFrame, this, _1);
+ grabber_.registerCallback (frame_cb);
+ grabber_.start ();
+ }
+
+ OpenNIFrameSource::~OpenNIFrameSource ()
+ {
+ // Stop the grabber when shutting down
+ grabber_.stop ();
+ }
+
+ bool
+ OpenNIFrameSource::isActive ()
+ {
+ return active_;
+ }
+
+ const PointCloudPtr
+ OpenNIFrameSource::snap ()
+ {
+ return (most_recent_frame_);
+ }
+
+ void
+ OpenNIFrameSource::onNewFrame (const PointCloudConstPtr &cloud)
+ {
+ mutex_.lock ();
+ ++frame_counter_;
+ most_recent_frame_ = boost::make_shared<PointCloud> (*cloud); // Make a copy of the frame
+ mutex_.unlock ();
+ }
+
+ void
+ OpenNIFrameSource::onKeyboardEvent (const pcl::visualization::KeyboardEvent & event)
+ {
+ // When the spacebar is pressed, trigger a frame capture
+ mutex_.lock ();
+ if (event.keyDown () && event.getKeySym () == "e")
+ {
+ active_ = false;
+ }
+ mutex_.unlock ();
+ }
+
+}
--- /dev/null
+add_subdirectory(apps)
\ No newline at end of file
--- /dev/null
+if (QHULL_FOUND)
+ add_executable(pcl_global_classification src/global_classification.cpp)
+ target_link_libraries(pcl_global_classification pcl_apps pcl_3d_rec_framework pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_surface)
+endif()
+
+add_executable(pcl_local_or_mian src/local_recognition_mian_dataset.cpp)
+target_link_libraries(pcl_local_or_mian pcl_apps pcl_3d_rec_framework pcl_recognition pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_surface pcl_keypoints)
+
+#add_executable(pcl_local_face src/local_recognition_faces.cpp)
+#target_link_libraries(pcl_local_face pcl_apps pcl_3d_rec_framework pcl_recognition pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_surface pcl_keypoints)
--- /dev/null
+/*
+ * test_training.cpp
+ *
+ * Created on: Mar 9, 2012
+ * Author: aitor
+ */
+
+#include <pcl/pcl_macros.h>
+#include <pcl/apps/3d_rec_framework/pipeline/global_nn_classifier.h>
+#include <pcl/apps/3d_rec_framework/pc_source/mesh_source.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/global/vfh_estimator.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/global/esf_estimator.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/global/cvfh_estimator.h>
+#include <pcl/apps/3d_rec_framework/tools/openni_frame_source.h>
+#include <pcl/apps/3d_rec_framework/utils/metrics.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/apps/dominant_plane_segmentation.h>
+#include <pcl/console/parse.h>
+
+template<template<class > class DistT, typename PointT, typename FeatureT>
+void
+segmentAndClassify (typename pcl::rec_3d_framework::GlobalNNPipeline<DistT, PointT, FeatureT> & global)
+{
+ //get point cloud from the kinect, segment it and classify it
+ OpenNIFrameSource::OpenNIFrameSource camera;
+ OpenNIFrameSource::PointCloudPtr frame;
+
+ pcl::visualization::PCLVisualizer vis ("kinect");
+
+ //keyboard callback to stop getting frames and finalize application
+ boost::function<void
+ (const pcl::visualization::KeyboardEvent&)> keyboard_cb = boost::bind (&OpenNIFrameSource::OpenNIFrameSource::onKeyboardEvent, &camera, _1);
+ vis.registerKeyboardCallback (keyboard_cb);
+ size_t previous_cluster_size = 0;
+ size_t previous_categories_size = 0;
+
+ float Z_DIST_ = 1.25f;
+ float text_scale = 0.015f;
+
+ while (camera.isActive ())
+ {
+ pcl::ScopeTime frame_process ("Global frame processing ------------- ");
+ frame = camera.snap ();
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_points (new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::copyPointCloud (*frame, *xyz_points);
+
+ //Step 1 -> Segment
+ pcl::apps::DominantPlaneSegmentation<pcl::PointXYZ> dps;
+ dps.setInputCloud (xyz_points);
+ dps.setMaxZBounds (Z_DIST_);
+ dps.setObjectMinHeight (0.005);
+ dps.setMinClusterSize (1000);
+ dps.setWSize (9);
+ dps.setDistanceBetweenClusters (0.1f);
+
+ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters;
+ std::vector<pcl::PointIndices> indices;
+ dps.setDownsamplingSize (0.02f);
+ dps.compute_fast (clusters);
+ dps.getIndicesClusters (indices);
+ Eigen::Vector4f table_plane_;
+ Eigen::Vector3f normal_plane_ = Eigen::Vector3f (table_plane_[0], table_plane_[1], table_plane_[2]);
+ dps.getTableCoefficients (table_plane_);
+
+ vis.removePointCloud ("frame");
+ vis.addPointCloud<OpenNIFrameSource::PointT> (frame, "frame");
+
+ for (size_t i = 0; i < previous_cluster_size; i++)
+ {
+ std::stringstream cluster_name;
+ cluster_name << "cluster_" << i;
+ vis.removePointCloud (cluster_name.str ());
+
+ cluster_name << "_ply_model_";
+ vis.removeShape (cluster_name.str ());
+ }
+
+ for (size_t i = 0; i < previous_categories_size; i++)
+ {
+ std::stringstream cluster_text;
+ cluster_text << "cluster_" << i << "_text";
+ vis.removeText3D (cluster_text.str ());
+ }
+
+ previous_categories_size = 0;
+ float dist_ = 0.03f;
+
+ for (size_t i = 0; i < clusters.size (); i++)
+ {
+ std::stringstream cluster_name;
+ cluster_name << "cluster_" << i;
+ pcl::visualization::PointCloudColorHandlerRandom<pcl::PointXYZ> random_handler (clusters[i]);
+ vis.addPointCloud<pcl::PointXYZ> (clusters[i], random_handler, cluster_name.str ());
+
+ global.setInputCloud (xyz_points);
+ global.setIndices (indices[i].indices);
+ global.classify ();
+
+ std::vector < std::string > categories;
+ std::vector<float> conf;
+
+ global.getCategory (categories);
+ global.getConfidence (conf);
+
+ std::string category = categories[0];
+ Eigen::Vector4f centroid;
+ pcl::compute3DCentroid (*xyz_points, indices[i].indices, centroid);
+ for (size_t kk = 0; kk < categories.size (); kk++)
+ {
+
+ pcl::PointXYZ pos;
+ pos.x = centroid[0] + normal_plane_[0] * static_cast<float> (kk + 1) * dist_;
+ pos.y = centroid[1] + normal_plane_[1] * static_cast<float> (kk + 1) * dist_;
+ pos.z = centroid[2] + normal_plane_[2] * static_cast<float> (kk + 1) * dist_;
+
+ std::ostringstream prob_str;
+ prob_str.precision (1);
+ prob_str << categories[kk] << " [" << conf[kk] << "]";
+
+ std::stringstream cluster_text;
+ cluster_text << "cluster_" << previous_categories_size << "_text";
+ vis.addText3D (prob_str.str (), pos, text_scale, 1, 0, 1, cluster_text.str (), 0);
+ previous_categories_size++;
+ }
+ }
+
+ previous_cluster_size = clusters.size ();
+
+ vis.spinOnce ();
+ }
+}
+
+//bin/pcl_global_classification -models_dir /home/aitor/data/3d-net_one_class/ -descriptor_name esf -training_dir /home/aitor/data/3d-net_one_class_trained_level_1 -nn 10
+
+int
+main (int argc, char ** argv)
+{
+
+ std::string path = "models/";
+ std::string desc_name = "esf";
+ std::string training_dir = "trained_models/";
+ int NN = 1;
+
+ pcl::console::parse_argument (argc, argv, "-models_dir", path);
+ pcl::console::parse_argument (argc, argv, "-training_dir", training_dir);
+ pcl::console::parse_argument (argc, argv, "-descriptor_name", desc_name);
+ pcl::console::parse_argument (argc, argv, "-nn", NN);
+
+ //pcl::console::parse_argument (argc, argv, "-z_dist", chop_at_z_);
+ //pcl::console::parse_argument (argc, argv, "-tesselation_level", views_level_);
+
+ boost::shared_ptr<pcl::rec_3d_framework::MeshSource<pcl::PointXYZ> > mesh_source (new pcl::rec_3d_framework::MeshSource<pcl::PointXYZ>);
+ mesh_source->setPath (path);
+ mesh_source->setResolution (150);
+ mesh_source->setTesselationLevel (1);
+ mesh_source->setViewAngle (57.f);
+ mesh_source->setRadiusSphere (1.5f);
+ mesh_source->setModelScale (1.f);
+ mesh_source->generate (training_dir);
+
+ boost::shared_ptr<pcl::rec_3d_framework::Source<pcl::PointXYZ> > cast_source;
+ cast_source = boost::static_pointer_cast<pcl::rec_3d_framework::MeshSource<pcl::PointXYZ> > (mesh_source);
+
+ boost::shared_ptr<pcl::rec_3d_framework::PreProcessorAndNormalEstimator<pcl::PointXYZ, pcl::Normal> > normal_estimator;
+ normal_estimator.reset (new pcl::rec_3d_framework::PreProcessorAndNormalEstimator<pcl::PointXYZ, pcl::Normal>);
+ normal_estimator->setCMR (true);
+ normal_estimator->setDoVoxelGrid (true);
+ normal_estimator->setRemoveOutliers (true);
+ normal_estimator->setFactorsForCMR (3, 7);
+
+ if (desc_name.compare ("vfh") == 0)
+ {
+ boost::shared_ptr<pcl::rec_3d_framework::VFHEstimation<pcl::PointXYZ, pcl::VFHSignature308> > vfh_estimator;
+ vfh_estimator.reset (new pcl::rec_3d_framework::VFHEstimation<pcl::PointXYZ, pcl::VFHSignature308>);
+ vfh_estimator->setNormalEstimator (normal_estimator);
+
+ boost::shared_ptr<pcl::rec_3d_framework::GlobalEstimator<pcl::PointXYZ, pcl::VFHSignature308> > cast_estimator;
+ cast_estimator = boost::dynamic_pointer_cast<pcl::rec_3d_framework::VFHEstimation<pcl::PointXYZ, pcl::VFHSignature308> > (vfh_estimator);
+
+ pcl::rec_3d_framework::GlobalNNPipeline<flann::L1, pcl::PointXYZ, pcl::VFHSignature308> global;
+ global.setDataSource (cast_source);
+ global.setTrainingDir (training_dir);
+ global.setDescriptorName (desc_name);
+ global.setNN (NN);
+ global.setFeatureEstimator (cast_estimator);
+ global.initialize (true);
+
+ segmentAndClassify<flann::L1, pcl::PointXYZ, pcl::VFHSignature308> (global);
+ }
+
+ if (desc_name.compare ("cvfh") == 0)
+ {
+ boost::shared_ptr<pcl::rec_3d_framework::CVFHEstimation<pcl::PointXYZ, pcl::VFHSignature308> > vfh_estimator;
+ vfh_estimator.reset (new pcl::rec_3d_framework::CVFHEstimation<pcl::PointXYZ, pcl::VFHSignature308>);
+ vfh_estimator->setNormalEstimator (normal_estimator);
+
+ boost::shared_ptr<pcl::rec_3d_framework::GlobalEstimator<pcl::PointXYZ, pcl::VFHSignature308> > cast_estimator;
+ cast_estimator = boost::dynamic_pointer_cast<pcl::rec_3d_framework::CVFHEstimation<pcl::PointXYZ, pcl::VFHSignature308> > (vfh_estimator);
+
+ pcl::rec_3d_framework::GlobalNNPipeline<Metrics::HistIntersectionUnionDistance, pcl::PointXYZ, pcl::VFHSignature308> global;
+ global.setDataSource (cast_source);
+ global.setTrainingDir (training_dir);
+ global.setDescriptorName (desc_name);
+ global.setFeatureEstimator (cast_estimator);
+ global.setNN (NN);
+ global.initialize (false);
+
+ segmentAndClassify<Metrics::HistIntersectionUnionDistance, pcl::PointXYZ, pcl::VFHSignature308> (global);
+ }
+
+ if (desc_name.compare ("esf") == 0)
+ {
+ boost::shared_ptr<pcl::rec_3d_framework::ESFEstimation<pcl::PointXYZ, pcl::ESFSignature640> > estimator;
+ estimator.reset (new pcl::rec_3d_framework::ESFEstimation<pcl::PointXYZ, pcl::ESFSignature640>);
+
+ boost::shared_ptr<pcl::rec_3d_framework::GlobalEstimator<pcl::PointXYZ, pcl::ESFSignature640> > cast_estimator;
+ cast_estimator = boost::dynamic_pointer_cast<pcl::rec_3d_framework::ESFEstimation<pcl::PointXYZ, pcl::ESFSignature640> > (estimator);
+
+ pcl::rec_3d_framework::GlobalNNPipeline<flann::L1, pcl::PointXYZ, pcl::ESFSignature640> global;
+ global.setDataSource (cast_source);
+ global.setTrainingDir (training_dir);
+ global.setDescriptorName (desc_name);
+ global.setFeatureEstimator (cast_estimator);
+ global.setNN (NN);
+ global.initialize (false);
+
+ segmentAndClassify<flann::L1, pcl::PointXYZ, pcl::ESFSignature640> (global);
+ }
+
+}
--- /dev/null
+/*
+ * local_recognition_mian_dataset.cpp
+ *
+ * Created on: Mar 24, 2012
+ * Author: aitor
+ */
+#include <pcl/recognition/hv/hv_papazov.h>
+#include <pcl/console/parse.h>
+#include <pcl/apps/3d_rec_framework/pipeline/local_recognizer.h>
+#include <pcl/apps/3d_rec_framework/pc_source/mesh_source.h>
+#include <pcl/recognition/cg/geometric_consistency.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/local/shot_local_estimator.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/local/shot_local_estimator_omp.h>
+#include <pcl/apps/3d_rec_framework/feature_wrapper/local/fpfh_local_estimator.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/recognition/cg/correspondence_grouping.h>
+#include <pcl/recognition/cg/geometric_consistency.h>
+#include <pcl/recognition/cg/hough_3d.h>
+#include <pcl/recognition/hv/hv_papazov.h>
+#include <pcl/recognition/hv/hv_go.h>
+#include <pcl/recognition/hv/greedy_verification.h>
+
+void
+getScenesInDirectory (bf::path & dir, std::string & rel_path_so_far, std::vector<std::string> & relative_paths)
+{
+ //list models in MODEL_FILES_DIR_ and return list
+ bf::directory_iterator end_itr;
+ for (bf::directory_iterator itr (dir); itr != end_itr; ++itr)
+ {
+ //check if its a directory, then get models in it
+ if (bf::is_directory (*itr))
+ {
+
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string so_far = rel_path_so_far + (itr->path ().filename ()).string() + "/";
+#else
+ std::string so_far = rel_path_so_far + (itr->path ()).filename () + "/";
+#endif
+ bf::path curr_path = itr->path ();
+ getScenesInDirectory (curr_path, so_far, relative_paths);
+ }
+ else
+ {
+ //check that it is a ply file and then add, otherwise ignore..
+ std::vector < std::string > strs;
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string file = (itr->path ().filename ()).string();
+#else
+ std::string file = (itr->path ().filename ());
+#endif
+
+ boost::split (strs, file, boost::is_any_of ("."));
+ std::string extension = strs[strs.size () - 1];
+
+ if (extension == "pcd")
+ {
+
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string path = rel_path_so_far + (itr->path ().filename ()).string();
+#else
+ std::string path = rel_path_so_far + (itr->path ()).filename ();
+#endif
+
+ relative_paths.push_back (path);
+ }
+ }
+ }
+}
+
+inline bool
+sortFiles (const std::string & file1, const std::string & file2)
+{
+ std::vector < std::string > strs1;
+ boost::split (strs1, file1, boost::is_any_of ("/"));
+
+ std::vector < std::string > strs2;
+ boost::split (strs2, file2, boost::is_any_of ("/"));
+
+ std::string id_1 = strs1[strs1.size () - 1];
+ std::string id_2 = strs2[strs2.size () - 1];
+
+ size_t pos1 = id_1.find (".ply.pcd");
+ size_t pos2 = id_2.find (".ply.pcd");
+
+ id_1 = id_1.substr (0, pos1);
+ id_2 = id_2.substr (0, pos2);
+
+ id_1 = id_1.substr (2);
+ id_2 = id_2.substr (2);
+
+ return atoi (id_1.c_str ()) < atoi (id_2.c_str ());
+}
+
+template<template<class > class DistT, typename PointT, typename FeatureT>
+ void
+ recognizeAndVisualize (typename pcl::rec_3d_framework::LocalRecognitionPipeline<DistT, PointT, FeatureT> & local, std::string & scenes_dir,
+ int scene = -1, bool single_model = false)
+ {
+
+ //read mians scenes
+ bf::path ply_files_dir = scenes_dir;
+ std::vector < std::string > files;
+ std::string start = "";
+ getScenesInDirectory (ply_files_dir, start, files);
+
+ std::sort (files.begin (), files.end (), sortFiles);
+
+ typename boost::shared_ptr<pcl::rec_3d_framework::Source<PointT> > model_source_ = local.getDataSource ();
+ typedef typename pcl::PointCloud<PointT>::ConstPtr ConstPointInTPtr;
+ typedef pcl::rec_3d_framework::Model<PointT> ModelT;
+
+ if (!single_model)
+ {
+ pcl::visualization::PCLVisualizer vis ("Mians dataset");
+
+ for (size_t i = 0; i < files.size (); i++)
+ {
+ std::cout << files[i] << std::endl;
+ if (scene != -1)
+ if (scene != i)
+ continue;
+
+ std::stringstream file;
+ file << ply_files_dir.string () << files[i];
+
+ typename pcl::PointCloud<PointT>::Ptr scene (new pcl::PointCloud<PointT> ());
+ pcl::io::loadPCDFile (file.str (), *scene);
+
+ local.setVoxelSizeICP (0.005f);
+ local.setInputCloud (scene);
+ {
+ pcl::ScopeTime ttt ("Recognition");
+ local.recognize ();
+ }
+
+ std::stringstream scene_name;
+ scene_name << "Scene " << (i + 1);
+ vis.addPointCloud<PointT> (scene, "scene_cloud");
+ vis.addText (scene_name.str (), 1, 30, 24, 1, 0, 0, "scene_text");
+
+ //visualize results
+ boost::shared_ptr < std::vector<ModelT> > models = local.getModels ();
+ boost::shared_ptr < std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > > transforms = local.getTransforms ();
+
+ for (size_t j = 0; j < models->size (); j++)
+ {
+ std::stringstream name;
+ name << "cloud_" << j;
+
+ ConstPointInTPtr model_cloud = models->at (j).getAssembled (0.0025f);
+ typename pcl::PointCloud<PointT>::Ptr model_aligned (new pcl::PointCloud<PointT>);
+ pcl::transformPointCloud (*model_cloud, *model_aligned, transforms->at (j));
+
+ float r, g, b;
+ std::cout << models->at (j).id_ << std::endl;
+ r = 255.0f;
+ g = 0.0f;
+ b = 0.0f;
+
+ if (models->at (j).id_.compare ("cheff") == 0)
+ {
+ r = 0.0f;
+ g = 255.0f;
+ b = 0.0f;
+ }
+ else if (models->at (j).id_.compare ("chicken_high") == 0)
+ {
+ r = 0.0f;
+ g = 255.0f;
+ b = 255.0f;
+ }
+ else if (models->at (j).id_.compare ("parasaurolophus_high") == 0)
+ {
+ r = 255.0f;
+ g = 255.0f;
+ b = 0.f;
+ }
+ else
+ {
+
+ }
+
+ pcl::visualization::PointCloudColorHandlerCustom<PointT> random_handler (model_aligned, r, g, b);
+ vis.addPointCloud<PointT> (model_aligned, random_handler, name.str ());
+ }
+
+ vis.spin ();
+
+ vis.removePointCloud ("scene_cloud");
+ vis.removeShape ("scene_text");
+ for (size_t j = 0; j < models->size (); j++)
+ {
+ std::stringstream name;
+ name << "cloud_" << j;
+ vis.removePointCloud (name.str ());
+ }
+ }
+ }
+
+ if (single_model)
+ {
+ //some applications prefer to search for a single object only
+ std::string id = "chicken_high";
+ pcl::visualization::PCLVisualizer vis ("Single model - chicken");
+ local.setSearchModel (id);
+ local.initialize ();
+
+ for (size_t i = 0; i < files.size (); i++)
+ {
+ std::stringstream file;
+ file << ply_files_dir.string () << files[i];
+
+ typename pcl::PointCloud<PointT>::Ptr scene (new pcl::PointCloud<PointT> ());
+ pcl::io::loadPCDFile (file.str (), *scene);
+
+ local.setInputCloud (scene);
+ local.recognize ();
+
+ std::stringstream scene_name;
+ scene_name << "Scene " << (i + 1);
+ vis.addPointCloud<PointT> (scene, "scene_cloud");
+ vis.addText (scene_name.str (), 1, 30, 24, 1, 0, 0, "scene_text");
+
+ //visualize results
+ boost::shared_ptr < std::vector<ModelT> > models = local.getModels ();
+ boost::shared_ptr < std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > > transforms = local.getTransforms ();
+
+ for (size_t j = 0; j < models->size (); j++)
+ {
+ std::stringstream name;
+ name << "cloud_" << j;
+
+ ConstPointInTPtr model_cloud = models->at (j).getAssembled (0.0025f);
+ typename pcl::PointCloud<PointT>::Ptr model_aligned (new pcl::PointCloud<PointT>);
+ pcl::transformPointCloud (*model_cloud, *model_aligned, transforms->at (j));
+
+ pcl::visualization::PointCloudColorHandlerRandom<PointT> random_handler (model_aligned);
+ vis.addPointCloud<PointT> (model_aligned, random_handler, name.str ());
+ }
+
+ vis.spin ();
+
+ vis.removePointCloud ("scene_cloud");
+ vis.removeShape ("scene_text");
+ for (size_t j = 0; j < models->size (); j++)
+ {
+ std::stringstream name;
+ name << "cloud_" << j;
+ vis.removePointCloud (name.str ());
+ }
+ }
+ }
+ }
+
+void
+getModelsInDirectory (bf::path & dir, std::string & rel_path_so_far, std::vector<std::string> & relative_paths, std::string & ext)
+{
+ bf::directory_iterator end_itr;
+ for (bf::directory_iterator itr (dir); itr != end_itr; ++itr)
+ {
+ //check if its a directory, then get models in it
+ if (bf::is_directory (*itr))
+ {
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string so_far = rel_path_so_far + (itr->path ().filename ()).string() + "/";
+#else
+ std::string so_far = rel_path_so_far + (itr->path ()).filename () + "/";
+#endif
+
+ bf::path curr_path = itr->path ();
+ getModelsInDirectory (curr_path, so_far, relative_paths, ext);
+ }
+ else
+ {
+ //check that it is a ply file and then add, otherwise ignore..
+ std::vector < std::string > strs;
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string file = (itr->path ().filename ()).string();
+#else
+ std::string file = (itr->path ()).filename ();
+#endif
+
+ boost::split (strs, file, boost::is_any_of ("."));
+ std::string extension = strs[strs.size () - 1];
+
+ if (extension.compare (ext) == 0)
+ {
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string path = rel_path_so_far + (itr->path ().filename ()).string();
+#else
+ std::string path = rel_path_so_far + (itr->path ()).filename ();
+#endif
+
+ relative_paths.push_back (path);
+ }
+ }
+ }
+}
+
+typedef pcl::ReferenceFrame RFType;
+
+int CG_SIZE_ = 3;
+float CG_THRESHOLD_ = 0.005f;
+
+/** Based on the paper:
+ * "A Global Hypotheses Verification Method for 3D Object Recognition",
+ * A. Aldoma and F. Tombari and L. Di Stefano and Markus Vincze, ECCV 2012
+ *
+ * Note: Due to changes PCL experimented between submission and the current status,
+ * you might find some inconsistencies between parameter value in code and in the paper.
+ * (tested on revision 7453)
+ */
+
+int
+main (int argc, char ** argv)
+{
+ std::string path = "";
+ std::string desc_name = "shot_omp";
+ std::string training_dir = "trained_models/";
+ std::string mians_scenes = "";
+ int force_retrain = 0;
+ int icp_iterations = 20;
+ int use_cache = 1;
+ int splits = 512;
+ int scene = -1;
+ int detect_clutter = 1;
+ int hv_method = 0;
+ int use_hv = 1;
+ float thres_hyp_ = 0.2f;
+ float desc_radius = 0.04f;
+
+ pcl::console::parse_argument (argc, argv, "-models_dir", path);
+ pcl::console::parse_argument (argc, argv, "-training_dir", training_dir);
+ pcl::console::parse_argument (argc, argv, "-descriptor_name", desc_name);
+ pcl::console::parse_argument (argc, argv, "-mians_scenes_dir", mians_scenes);
+ pcl::console::parse_argument (argc, argv, "-force_retrain", force_retrain);
+ pcl::console::parse_argument (argc, argv, "-icp_iterations", icp_iterations);
+ pcl::console::parse_argument (argc, argv, "-use_cache", use_cache);
+ pcl::console::parse_argument (argc, argv, "-splits", splits);
+ pcl::console::parse_argument (argc, argv, "-gc_size", CG_SIZE_);
+ pcl::console::parse_argument (argc, argv, "-gc_threshold", CG_THRESHOLD_);
+ pcl::console::parse_argument (argc, argv, "-scene", scene);
+ pcl::console::parse_argument (argc, argv, "-detect_clutter", detect_clutter);
+ pcl::console::parse_argument (argc, argv, "-hv_method", hv_method);
+ pcl::console::parse_argument (argc, argv, "-use_hv", use_hv);
+ pcl::console::parse_argument (argc, argv, "-thres_hyp", thres_hyp_);
+
+ if (mians_scenes.compare ("") == 0)
+ {
+ PCL_ERROR("Set the directory containing mians scenes using the -mians_scenes_dir [dir] option\n");
+ return -1;
+ }
+
+ if (path.compare ("") == 0)
+ {
+ PCL_ERROR("Set the directory containing the models of mian dataset using the -models_dir [dir] option\n");
+ return -1;
+ }
+
+ bf::path models_dir_path = path;
+ if (!bf::exists (models_dir_path))
+ {
+ PCL_ERROR("Models dir path %s does not exist, use -models_dir [dir] option\n", path.c_str());
+ return -1;
+ }
+ else
+ {
+ std::vector < std::string > files;
+ std::string start = "";
+ std::string ext = std::string ("ply");
+ bf::path dir = models_dir_path;
+ getModelsInDirectory (dir, start, files, ext);
+ assert (files.size () == 4);
+ }
+
+ //configure mesh source
+ boost::shared_ptr<pcl::rec_3d_framework::MeshSource<pcl::PointXYZ> > mesh_source (new pcl::rec_3d_framework::MeshSource<pcl::PointXYZ>);
+ mesh_source->setPath (path);
+ mesh_source->setResolution (250);
+ mesh_source->setTesselationLevel (1);
+ mesh_source->setViewAngle (57.f);
+ mesh_source->setRadiusSphere (1.5f);
+ mesh_source->setModelScale (0.001f);
+ mesh_source->generate (training_dir);
+
+ boost::shared_ptr<pcl::rec_3d_framework::Source<pcl::PointXYZ> > cast_source;
+ cast_source = boost::static_pointer_cast<pcl::rec_3d_framework::MeshSource<pcl::PointXYZ> > (mesh_source);
+
+ //configure normal estimator
+ boost::shared_ptr<pcl::rec_3d_framework::PreProcessorAndNormalEstimator<pcl::PointXYZ, pcl::Normal> > normal_estimator;
+ normal_estimator.reset (new pcl::rec_3d_framework::PreProcessorAndNormalEstimator<pcl::PointXYZ, pcl::Normal>);
+ normal_estimator->setCMR (false);
+ normal_estimator->setDoVoxelGrid (true);
+ normal_estimator->setRemoveOutliers (true);
+ normal_estimator->setValuesForCMRFalse (0.003f, 0.012f);
+
+ //configure keypoint extractor
+ boost::shared_ptr<pcl::rec_3d_framework::UniformSamplingExtractor<pcl::PointXYZ> >
+ uniform_keypoint_extractor (
+ new pcl::rec_3d_framework::UniformSamplingExtractor<
+ pcl::PointXYZ>);
+ //uniform_keypoint_extractor->setSamplingDensity (0.01f);
+ uniform_keypoint_extractor->setSamplingDensity (0.005f);
+ uniform_keypoint_extractor->setFilterPlanar (true);
+
+ boost::shared_ptr<pcl::rec_3d_framework::KeypointExtractor<pcl::PointXYZ> > keypoint_extractor;
+ keypoint_extractor = boost::static_pointer_cast<pcl::rec_3d_framework::KeypointExtractor<pcl::PointXYZ> > (uniform_keypoint_extractor);
+
+ //configure cg algorithm (geometric consistency grouping)
+ boost::shared_ptr<pcl::CorrespondenceGrouping<pcl::PointXYZ, pcl::PointXYZ> > cast_cg_alg;
+ boost::shared_ptr<pcl::GeometricConsistencyGrouping<pcl::PointXYZ, pcl::PointXYZ> > gcg_alg (
+ new pcl::GeometricConsistencyGrouping<pcl::PointXYZ,
+ pcl::PointXYZ>);
+ gcg_alg->setGCThreshold (CG_SIZE_);
+ gcg_alg->setGCSize (CG_THRESHOLD_);
+ cast_cg_alg = boost::static_pointer_cast<pcl::CorrespondenceGrouping<pcl::PointXYZ, pcl::PointXYZ> > (gcg_alg);
+
+ //configure hypothesis verificator
+ boost::shared_ptr<pcl::PapazovHV<pcl::PointXYZ, pcl::PointXYZ> > papazov (new pcl::PapazovHV<pcl::PointXYZ, pcl::PointXYZ>);
+ papazov->setResolution (0.005f);
+ papazov->setInlierThreshold (0.005f);
+ papazov->setSupportThreshold (0.08f);
+ papazov->setPenaltyThreshold (0.05f);
+ papazov->setConflictThreshold (0.02f);
+ papazov->setOcclusionThreshold (0.01f);
+
+ boost::shared_ptr<pcl::GlobalHypothesesVerification<pcl::PointXYZ, pcl::PointXYZ> > go (
+ new pcl::GlobalHypothesesVerification<pcl::PointXYZ,
+ pcl::PointXYZ>);
+ go->setResolution (0.005f);
+ go->setMaxIterations (7000);
+ go->setInlierThreshold (0.005f);
+ go->setRadiusClutter (0.04f);
+ go->setRegularizer (3.f);
+ go->setClutterRegularizer (7.5f);
+ go->setDetectClutter (detect_clutter);
+ go->setOcclusionThreshold (0.01f);
+
+ boost::shared_ptr<pcl::GreedyVerification<pcl::PointXYZ, pcl::PointXYZ> > greedy (new pcl::GreedyVerification<pcl::PointXYZ, pcl::PointXYZ> (3.f));
+ greedy->setResolution (0.005f);
+ greedy->setInlierThreshold (0.005f);
+ greedy->setOcclusionThreshold (0.01f);
+
+ boost::shared_ptr<pcl::HypothesisVerification<pcl::PointXYZ, pcl::PointXYZ> > cast_hv_alg;
+
+ switch (hv_method)
+ {
+ case 1:
+ cast_hv_alg = boost::static_pointer_cast<pcl::HypothesisVerification<pcl::PointXYZ, pcl::PointXYZ> > (greedy);
+ break;
+ case 2:
+ cast_hv_alg = boost::static_pointer_cast<pcl::HypothesisVerification<pcl::PointXYZ, pcl::PointXYZ> > (papazov);
+ break;
+ default:
+ cast_hv_alg = boost::static_pointer_cast<pcl::HypothesisVerification<pcl::PointXYZ, pcl::PointXYZ> > (go);
+ }
+
+ if (desc_name.compare ("shot") == 0)
+ {
+ boost::shared_ptr<pcl::rec_3d_framework::SHOTLocalEstimation<pcl::PointXYZ, pcl::Histogram<352> > > estimator;
+ estimator.reset (new pcl::rec_3d_framework::SHOTLocalEstimation<pcl::PointXYZ, pcl::Histogram<352> >);
+ estimator->setNormalEstimator (normal_estimator);
+ estimator->addKeypointExtractor (keypoint_extractor);
+ estimator->setSupportRadius (0.04f);
+
+ boost::shared_ptr<pcl::rec_3d_framework::LocalEstimator<pcl::PointXYZ, pcl::Histogram<352> > > cast_estimator;
+ cast_estimator = boost::dynamic_pointer_cast<pcl::rec_3d_framework::LocalEstimator<pcl::PointXYZ, pcl::Histogram<352> > > (estimator);
+
+ pcl::rec_3d_framework::LocalRecognitionPipeline<flann::L1, pcl::PointXYZ, pcl::Histogram<352> > local;
+ local.setDataSource (cast_source);
+ local.setTrainingDir (training_dir);
+ local.setDescriptorName (desc_name);
+ local.setFeatureEstimator (cast_estimator);
+ local.setCGAlgorithm (cast_cg_alg);
+ if (use_hv)
+ local.setHVAlgorithm (cast_hv_alg);
+ local.setUseCache (static_cast<bool> (use_cache));
+ local.initialize (static_cast<bool> (force_retrain));
+
+ uniform_keypoint_extractor->setSamplingDensity (0.005f);
+ local.setICPIterations (icp_iterations);
+ local.setKdtreeSplits (splits);
+
+ recognizeAndVisualize<flann::L1, pcl::PointXYZ, pcl::Histogram<352> > (local, mians_scenes);
+
+ }
+
+ if (desc_name.compare ("shot_omp") == 0)
+ {
+ desc_name = std::string ("shot");
+ boost::shared_ptr<pcl::rec_3d_framework::SHOTLocalEstimationOMP<pcl::PointXYZ, pcl::Histogram<352> > > estimator;
+ estimator.reset (new pcl::rec_3d_framework::SHOTLocalEstimationOMP<pcl::PointXYZ, pcl::Histogram<352> >);
+ estimator->setNormalEstimator (normal_estimator);
+ estimator->addKeypointExtractor (keypoint_extractor);
+ //estimator->setSupportRadius (0.04f);
+ estimator->setSupportRadius (desc_radius);
+
+ boost::shared_ptr<pcl::rec_3d_framework::LocalEstimator<pcl::PointXYZ, pcl::Histogram<352> > > cast_estimator;
+ cast_estimator = boost::dynamic_pointer_cast<pcl::rec_3d_framework::LocalEstimator<pcl::PointXYZ, pcl::Histogram<352> > > (estimator);
+
+ pcl::rec_3d_framework::LocalRecognitionPipeline<flann::L1, pcl::PointXYZ, pcl::Histogram<352> > local;
+ local.setDataSource (cast_source);
+ local.setTrainingDir (training_dir);
+ local.setDescriptorName (desc_name);
+ local.setFeatureEstimator (cast_estimator);
+ local.setCGAlgorithm (cast_cg_alg);
+ if (use_hv)
+ local.setHVAlgorithm (cast_hv_alg);
+
+ local.setUseCache (static_cast<bool> (use_cache));
+ local.initialize (static_cast<bool> (force_retrain));
+ local.setThresholdAcceptHyp (thres_hyp_);
+
+ uniform_keypoint_extractor->setSamplingDensity (0.005f);
+ local.setICPIterations (icp_iterations);
+ local.setKdtreeSplits (splits);
+
+ recognizeAndVisualize<flann::L1, pcl::PointXYZ, pcl::Histogram<352> > (local, mians_scenes, scene);
+
+ }
+
+ if (desc_name.compare ("fpfh") == 0)
+ {
+ boost::shared_ptr<pcl::rec_3d_framework::FPFHLocalEstimation<pcl::PointXYZ, pcl::FPFHSignature33> > estimator;
+ estimator.reset (new pcl::rec_3d_framework::FPFHLocalEstimation<pcl::PointXYZ, pcl::FPFHSignature33>);
+ estimator->setNormalEstimator (normal_estimator);
+ estimator->addKeypointExtractor (keypoint_extractor);
+ estimator->setSupportRadius (0.04f);
+
+ boost::shared_ptr<pcl::rec_3d_framework::LocalEstimator<pcl::PointXYZ, pcl::FPFHSignature33> > cast_estimator;
+ cast_estimator = boost::dynamic_pointer_cast<pcl::rec_3d_framework::LocalEstimator<pcl::PointXYZ, pcl::FPFHSignature33> > (estimator);
+
+ pcl::rec_3d_framework::LocalRecognitionPipeline<flann::L1, pcl::PointXYZ, pcl::FPFHSignature33> local;
+ local.setDataSource (cast_source);
+ local.setTrainingDir (training_dir);
+ local.setDescriptorName (desc_name);
+ local.setFeatureEstimator (cast_estimator);
+ local.setCGAlgorithm (cast_cg_alg);
+ if (use_hv)
+ local.setHVAlgorithm (cast_hv_alg);
+
+ local.setUseCache (static_cast<bool> (use_cache));
+ local.initialize (static_cast<bool> (force_retrain));
+
+ uniform_keypoint_extractor->setSamplingDensity (0.005f);
+ local.setICPIterations (icp_iterations);
+ local.setKdtreeSplits (splits);
+
+ recognizeAndVisualize<flann::L1, pcl::PointXYZ, pcl::FPFHSignature33> (local, mians_scenes);
+ }
+}
set(SUBSYS_NAME apps)
set(SUBSYS_DESC "Application examples/samples that show how PCL works")
-set(SUBSYS_DEPS common geometry io filters sample_consensus segmentation visualization kdtree features surface octree registration keypoints tracking search recognition)
-
-# Find VTK
-if(NOT VTK_FOUND)
- set(DEFAULT FALSE)
- set(REASON "VTK was not found.")
-else(NOT VTK_FOUND)
- set(DEFAULT TRUE)
- set(REASON)
- include("${VTK_USE_FILE}")
-endif(NOT VTK_FOUND)
-
-# OpenNI found?
-if(NOT OPENNI_FOUND)
- set(DEFAULT FALSE)
- set(REASON "OpenNI was not found.")
-else(NOT OPENNI_FOUND)
- set(DEFAULT TRUE)
- set(REASON)
-endif(NOT OPENNI_FOUND)
+set(SUBSYS_DEPS common geometry io filters sample_consensus segmentation visualization kdtree features surface octree registration keypoints tracking search recognition ml stereo)
set(DEFAULT FALSE)
PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}")
endif(LIBUSB_1_FOUND)
if (VTK_FOUND)
+
+ include("${VTK_USE_FILE}")
+
+ set(incs "include/pcl/${SUBSYS_NAME}/render_views_tesselated_sphere.h")
+ set(srcs "src/render_views_tesselated_sphere.cpp")
+
PCL_ADD_EXECUTABLE(pcl_ppf_object_recognition "${SUBSYS_NAME}" src/ppf_object_recognition.cpp)
target_link_libraries(pcl_ppf_object_recognition pcl_common pcl_io pcl_filters pcl_features pcl_registration pcl_visualization pcl_sample_consensus pcl_segmentation)
# PCL_ADD_EXECUTABLE(pcl_convolve "${SUBSYS_NAME}" src/convolve.cpp)
# target_link_libraries(pcl_convolve pcl_common pcl_io pcl_visualization)
+ PCL_ADD_EXECUTABLE(pcl_pcd_organized_edge_detection "${SUBSYS_NAME}" src/pcd_organized_edge_detection.cpp)
+ target_link_libraries(pcl_pcd_organized_edge_detection pcl_common pcl_io pcl_features pcl_visualization)
+
+ PCL_ADD_EXECUTABLE(pcl_face_trainer "${SUBSYS_NAME}" src/face_detection/face_trainer.cpp)
+ target_link_libraries(pcl_face_trainer pcl_features pcl_recognition pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_surface pcl_keypoints pcl_ml pcl_search pcl_kdtree ${VTK_LIBRARIES})
+
+ PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_fs_face_detector "${SUBSYS_NAME}" src/face_detection//filesystem_face_detection.cpp)
+ target_link_libraries(pcl_fs_face_detector pcl_features pcl_recognition pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_surface pcl_keypoints pcl_ml pcl_search pcl_kdtree ${VTK_LIBRARIES})
+
+ PCL_ADD_EXECUTABLE(pcl_stereo_ground_segmentation "${SUBSYS_NAME}" src/stereo_ground_segmentation.cpp)
+ target_link_libraries(pcl_stereo_ground_segmentation pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_features pcl_stereo)
+
if (QT4_FOUND AND VTK_USE_QVTK)
# Manual registration demo
endif (QT4_FOUND AND VTK_USE_QVTK)
- if (OPENNI_FOUND AND BUILD_OPENNI)
+ if (WITH_OPENNI)
# PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_grab_frame "${SUBSYS_NAME}" src/openni_grab_frame.cpp)
# target_link_libraries(pcl_openni_grab_frame pcl_common pcl_io pcl_visualization)
PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_boundary_estimation "${SUBSYS_NAME}" src/openni_boundary_estimation.cpp)
target_link_libraries(pcl_openni_boundary_estimation pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_features pcl_surface)
+ PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_organized_edge_detection "${SUBSYS_NAME}" src/openni_organized_edge_detection.cpp)
+ target_link_libraries(pcl_openni_organized_edge_detection pcl_common pcl_io pcl_features pcl_visualization)
+
+ PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_face_detector "${SUBSYS_NAME}" src/face_detection//openni_face_detection.cpp src/face_detection//openni_frame_source.cpp)
+ target_link_libraries(pcl_openni_face_detector pcl_features pcl_recognition pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_surface pcl_keypoints pcl_ml pcl_search pcl_kdtree ${VTK_LIBRARIES})
+
if (QT4_FOUND AND VTK_USE_QVTK)
# OpenNI Passthrough application demo
QT4_WRAP_UI(openni_passthrough_ui src/openni_passthrough.ui)
endif ()
- set(incs
- include/pcl/${SUBSYS_NAME}/render_views_tesselated_sphere.h
- include/pcl/${SUBSYS_NAME}/timer.h)
- set(srcs src/render_views_tesselated_sphere.cpp)
-
if (QHULL_FOUND)
PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_3d_convex_hull "${SUBSYS_NAME}" src/openni_3d_convex_hull.cpp)
target_link_libraries(pcl_openni_3d_convex_hull pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_features pcl_surface)
PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_tracking "${SUBSYS_NAME}" src/openni_tracking.cpp)
target_link_libraries(pcl_openni_tracking pcl_common pcl_io pcl_surface pcl_visualization pcl_filters pcl_features pcl_segmentation pcl_tracking pcl_search)
- set(incs "include/pcl/${SUBSYS_NAME}/dominant_plane_segmentation.h" ${incs})
- set(impl_incs "include/pcl/${SUBSYS_NAME}/impl/dominant_plane_segmentation.hpp")
- set(srcs src/dominant_plane_segmentation.cpp ${srcs})
-
PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_planar_convex_hull "${SUBSYS_NAME}" src/openni_planar_convex_hull.cpp)
target_link_libraries(pcl_openni_planar_convex_hull pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_surface)
endif() # QHULL_FOUND
- # Install include files
- PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs})
- PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs})
-
- set(LIB_NAME "pcl_${SUBSYS_NAME}")
- PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${impl_incs} ${incs})
- target_link_libraries("${LIB_NAME}" pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_surface pcl_features pcl_sample_consensus pcl_search)
-
- PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "" "" "" "" "")
-
-
PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ni_agast "${SUBSYS_NAME}" src/ni_agast.cpp)
target_link_libraries(pcl_ni_agast pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_features pcl_keypoints pcl_surface pcl_search)
+ PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ni_brisk "${SUBSYS_NAME}" src/ni_brisk.cpp)
+ target_link_libraries(pcl_ni_brisk pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_features pcl_keypoints pcl_surface pcl_search)
+
PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ni_susan "${SUBSYS_NAME}" src/ni_susan.cpp)
target_link_libraries(pcl_ni_susan pcl_common pcl_visualization pcl_features pcl_keypoints pcl_search)
PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_klt "${SUBSYS_NAME}" src/openni_klt.cpp)
target_link_libraries(pcl_openni_klt pcl_common pcl_io pcl_visualization pcl_tracking)
- endif() # OPENNI_FOUND + BUILD_OPENNI
+ endif() # WITH_OPENNI
endif() # VTK_FOUND
add_subdirectory("${CMAKE_CURRENT_SOURCE_DIR}/${subdir}")
endforeach(subdir)
+ if(QHULL_FOUND)
+ set(incs
+ "include/pcl/${SUBSYS_NAME}/dominant_plane_segmentation.h"
+ "include/pcl/${SUBSYS_NAME}/timer.h"
+ ${incs}
+ )
+ set(impl_incs "include/pcl/${SUBSYS_NAME}/impl/dominant_plane_segmentation.hpp")
+ set(srcs "src/dominant_plane_segmentation.cpp" ${srcs})
+ endif()
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+ PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${impl_incs} ${incs})
+ target_link_libraries("${LIB_NAME}" pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_surface pcl_features pcl_sample_consensus pcl_search)
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "" "" "" "" "")
+ # Install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs})
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs})
+
endif(build)
+
ColorHandler::ConstPtr color_handler_;
GeometryHandler::ConstPtr geometry_handler_;
-
-
//We keep actual local copies of these.
Eigen::Vector4f origin_;
Eigen::Quaternionf orientation_;
+ bool template_cloud_set_;
+
//Internal Storage of the templated type of this cloud
int point_type_;
- bool template_cloud_set_;
bool
checkIfFinite ();
point_type_ = PointTypeFlags::NONE;
}
-
};
template <> inline void
#include <QThread>
#include <QMessageBox>
#include <QPair>
+#include <QStyledItemDelegate>
+#include <QUndoGroup>
+#include <QFileDialog>
+#include <QAction>
+#include <QGridLayout>
#endif // CLOUD_COMPOSER_QT_H_
getToolName () const { return "MergeCloudTool";}
};
+
}
}
}
}
+Q_DECLARE_METATYPE (pcl::cloud_composer::ToolFactory*);
+
Q_DECLARE_INTERFACE(pcl::cloud_composer::ToolFactory,
"cloud_composer.ToolFactory/1.0")
-#endif //TOOL_FACTORY_H_
\ No newline at end of file
+#endif //TOOL_FACTORY_H_
}
Q_DECLARE_METATYPE (pcl::cloud_composer::ToolBoxModel);
-Q_DECLARE_METATYPE (pcl::cloud_composer::ToolFactory*);
Q_DECLARE_METATYPE (QStandardItemModel*);
#endif //TOOLBOX_MODEL_H_
{
Q_OBJECT
Q_INTERFACES (pcl::cloud_composer::ToolFactory)
+#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0)
+ Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0")
+#endif
public:
SplitItemTool*
createTool (PropertiesModel* parameter_model, QObject* parent = 0)
-#endif //EUCLIDEAN_CLUSTERING_H_
\ No newline at end of file
+#endif //EUCLIDEAN_CLUSTERING_H_
{
Q_OBJECT
Q_INTERFACES (pcl::cloud_composer::ToolFactory)
+#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0)
+ Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0")
+#endif
public:
NewItemTool*
createTool (PropertiesModel* parameter_model, QObject* parent = 0)
-#endif //FPFH_ESTIMATION_H_
\ No newline at end of file
+#endif //FPFH_ESTIMATION_H_
-#endif //IMPL_SUPERVOXELS_HPP_
\ No newline at end of file
+#endif //IMPL_SUPERVOXELS_HPP_
{
Q_OBJECT
Q_INTERFACES (pcl::cloud_composer::ToolFactory)
+#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0)
+ Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0")
+#endif
public:
NewItemTool*
createTool (PropertiesModel* parameter_model, QObject* parent = 0)
-#endif //NORMAL_ESTIMATION_H_
\ No newline at end of file
+#endif //NORMAL_ESTIMATION_H_
{
Q_OBJECT
Q_INTERFACES (pcl::cloud_composer::ToolFactory)
+#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0)
+ Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0")
+#endif
public:
SplitItemTool*
createTool (PropertiesModel* parameter_model, QObject* parent = 0)
{
Q_OBJECT
Q_INTERFACES (pcl::cloud_composer::ToolFactory)
+#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0)
+ Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0")
+#endif
public:
ModifyItemTool*
createTool (PropertiesModel* parameter_model, QObject* parent = 0)
- #endif //SANITIZE_CLOUD_H_
\ No newline at end of file
+ #endif //SANITIZE_CLOUD_H_
{
Q_OBJECT
Q_INTERFACES (pcl::cloud_composer::ToolFactory)
+#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0)
+ Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0")
+#endif
public:
ModifyItemTool*
createTool (PropertiesModel* parameter_model, QObject* parent = 0)
-#endif //STATISTICAL_OUTLIER_REMOVAL_H_
\ No newline at end of file
+#endif //STATISTICAL_OUTLIER_REMOVAL_H_
{
Q_OBJECT
Q_INTERFACES (pcl::cloud_composer::ToolFactory)
+#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0)
+ Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0")
+#endif
public:
SplitItemTool*
createTool (PropertiesModel* parameter_model, QObject* parent = 0)
{
Q_OBJECT
Q_INTERFACES (pcl::cloud_composer::ToolFactory)
+#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0)
+ Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0")
+#endif
public:
ModifyItemTool*
createTool (PropertiesModel* parameter_model, QObject* parent = 0)
-#endif //VOXEL_GRID_DOWNSAMPLE_H_
\ No newline at end of file
+#endif //VOXEL_GRID_DOWNSAMPLE_H_
{
QStandardItem* new_item = parent_item->child(row);
CloudComposerItem* item = dynamic_cast<CloudComposerItem*> (new_item);
- item = dynamic_cast<CloudComposerItem*> (new_item);
if (item)
item->paintView (vis_);
}
void
-pcl::cloud_composer::CloudViewer::modelChanged (int index)
+pcl::cloud_composer::CloudViewer::modelChanged (int)
{
- CloudView* view = static_cast<CloudView*> (this->widget (index)); view = view;
emit newModelSelected (getModel ());
}
{
if (! template_cloud_set_ )
{
- int num_fields = cloud_blob_ptr_->fields.size ();
std::vector<pcl::PCLPointField>::iterator end = cloud_blob_ptr_->fields.end ();
std::vector<pcl::PCLPointField>::iterator itr = cloud_blob_ptr_->fields.begin ();
QStringList field_names;
selected_mapper->SetInputData (all_points);
#endif
selected_mapper->ScalarVisibilityOff ();
-
- vtkIdTypeArray* ids = vtkIdTypeArray::SafeDownCast (all_points->GetPointData ()->GetArray ("OriginalIds"));
selected_actor->GetProperty ()->SetColor (0.0, 1.0, 0.0); //(R,G,B)
selected_actor->GetProperty ()->SetPointSize (3);
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
-Q_EXPORT_PLUGIN2(cloud_composer_euclidean_clustering_tool, pcl::cloud_composer::EuclideanClusteringToolFactory)
+#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
+ Q_EXPORT_PLUGIN2(cloud_composer_euclidean_clustering_tool, pcl::cloud_composer::EuclideanClusteringToolFactory)
+#else
+ Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0")
+#endif
pcl::cloud_composer::EuclideanClusteringTool::EuclideanClusteringTool (PropertiesModel* parameter_model, QObject* parent)
: SplitItemTool (parameter_model, parent)
#include <pcl/filters/filter.h>
-
-Q_EXPORT_PLUGIN2(cloud_composer_fpfh_estimation_tool, pcl::cloud_composer::FPFHEstimationToolFactory)
-
+#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
+ Q_EXPORT_PLUGIN2(cloud_composer_fpfh_estimation_tool, pcl::cloud_composer::FPFHEstimationToolFactory)
+#else
+ Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0")
+#endif
pcl::cloud_composer::FPFHEstimationTool::FPFHEstimationTool (PropertiesModel* parameter_model, QObject* parent)
: NewItemTool (parameter_model, parent)
}
QList <pcl::cloud_composer::CloudComposerItem*>
-pcl::cloud_composer::FPFHEstimationTool::performAction (ConstItemList input_data, PointTypeFlags::PointType type)
+pcl::cloud_composer::FPFHEstimationTool::performAction (ConstItemList input_data, PointTypeFlags::PointType)
{
QList <CloudComposerItem*> output;
const CloudComposerItem* input_item;
#include <pcl/features/normal_3d.h>
#include <pcl/point_types.h>
-
-Q_EXPORT_PLUGIN2(cloud_composer_normal_estimation_tool, pcl::cloud_composer::NormalEstimationToolFactory)
-
+#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
+ Q_EXPORT_PLUGIN2(cloud_composer_normal_estimation_tool, pcl::cloud_composer::NormalEstimationToolFactory)
+#else
+ Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0")
+#endif
pcl::cloud_composer::NormalEstimationTool::NormalEstimationTool (PropertiesModel* parameter_model, QObject* parent)
: NewItemTool (parameter_model, parent)
}
QList <pcl::cloud_composer::CloudComposerItem*>
-pcl::cloud_composer::NormalEstimationTool::performAction (ConstItemList input_data, PointTypeFlags::PointType type)
+pcl::cloud_composer::NormalEstimationTool::performAction (ConstItemList input_data, PointTypeFlags::PointType)
{
QList <CloudComposerItem*> output;
const CloudComposerItem* input_item;
#include <pcl/apps/cloud_composer/tools/impl/organized_segmentation.hpp>
-Q_EXPORT_PLUGIN2(cloud_composer_organized_segmentation_tool, pcl::cloud_composer::OrganizedSegmentationToolFactory)
+#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
+ Q_EXPORT_PLUGIN2(cloud_composer_organized_segmentation_tool, pcl::cloud_composer::OrganizedSegmentationToolFactory)
+#else
+ Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0")
+#endif
pcl::cloud_composer::OrganizedSegmentationTool::OrganizedSegmentationTool (PropertiesModel* parameter_model, QObject* parent)
: SplitItemTool (parameter_model, parent)
return parameter_model;
-}
\ No newline at end of file
+}
#include <pcl/filters/passthrough.h>
-
-Q_EXPORT_PLUGIN2(cloud_composer_sanitize_cloud_tool, pcl::cloud_composer::SanitizeCloudToolFactory)
-
+#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
+ Q_EXPORT_PLUGIN2(cloud_composer_sanitize_cloud_tool, pcl::cloud_composer::SanitizeCloudToolFactory)
+#else
+ Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0")
+#endif
pcl::cloud_composer::SanitizeCloudTool::SanitizeCloudTool (PropertiesModel* parameter_model, QObject* parent)
: ModifyItemTool (parameter_model, parent)
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/point_types.h>
-
-Q_EXPORT_PLUGIN2(cloud_composer_statistical_outlier_removal_tool, pcl::cloud_composer::StatisticalOutlierRemovalToolFactory)
-
+#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
+ Q_EXPORT_PLUGIN2(cloud_composer_statistical_outlier_removal_tool, pcl::cloud_composer::StatisticalOutlierRemovalToolFactory)
+#else
+ Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0")
+#endif
pcl::cloud_composer::StatisticalOutlierRemovalTool::StatisticalOutlierRemovalTool (PropertiesModel* parameter_model, QObject* parent)
: ModifyItemTool (parameter_model, parent)
}
QList <pcl::cloud_composer::CloudComposerItem*>
-pcl::cloud_composer::StatisticalOutlierRemovalTool::performAction (ConstItemList input_data, PointTypeFlags::PointType type)
+pcl::cloud_composer::StatisticalOutlierRemovalTool::performAction (ConstItemList input_data, PointTypeFlags::PointType)
{
QList <CloudComposerItem*> output;
const CloudComposerItem* input_item;
parameter_model->addProperty ("Std Dev Thresh", 1.0, Qt::ItemIsEditable | Qt::ItemIsEnabled);
return parameter_model;
-}
\ No newline at end of file
+}
#include <pcl/apps/cloud_composer/tools/impl/supervoxels.hpp>
-Q_EXPORT_PLUGIN2(cloud_composer_voxel_superpixels_tool, pcl::cloud_composer::SupervoxelsToolFactory)
+#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
+ Q_EXPORT_PLUGIN2(cloud_composer_voxel_superpixels_tool, pcl::cloud_composer::SupervoxelsToolFactory)
+#else
+ Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0")
+#endif
pcl::cloud_composer::SupervoxelsTool::SupervoxelsTool (PropertiesModel* parameter_model, QObject* parent)
: SplitItemTool (parameter_model, parent)
parameter_model->addProperty ("Spatial Weight", 0.4, Qt::ItemIsEditable | Qt::ItemIsEnabled);
return parameter_model;
-}
\ No newline at end of file
+}
#include <pcl/point_types.h>
-
-Q_EXPORT_PLUGIN2(cloud_composer_voxel_grid_downsample_tool, pcl::cloud_composer::VoxelGridDownsampleToolFactory)
-
+#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
+ Q_EXPORT_PLUGIN2(cloud_composer_voxel_grid_downsample_tool, pcl::cloud_composer::VoxelGridDownsampleToolFactory)
+#else
+ Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0")
+#endif
pcl::cloud_composer::VoxelGridDownsampleTool::VoxelGridDownsampleTool (PropertiesModel* parameter_model, QObject* parent)
: ModifyItemTool (parameter_model, parent)
endif()
#OpenNI
-if(NOT OPENNI_FOUND OR NOT BUILD_OPENNI)
+if(NOT WITH_OPENNI)
set(DEFAULT AUTO_OFF)
set(REASON "OpenNI was not found or was disabled by the user.")
elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF")
for (Mesh::VertexDataCloud::const_iterator it=cloud.begin (); it!=cloud.end (); ++it)
{
// Don't consider points that are facing away from the camera.
- if ((T_inv * it->getNormalVector4fMap ()).z () < 0.f)
+ if ((T_inv.lazyProduct (it->getNormalVector4fMap ())).z () < 0.f)
{
PointNormal pt;
pt.getVector4fMap () = it->getVector4fMap ();
// acos (angle) = dot (a, b) / (norm (a) * norm (b)
// m_sphere_vertices are already normalized
unsigned int index = 0;
- (aligned_direction.transpose () * pcl::ihs::dome.getVertices ()).maxCoeff (&index);
+ aligned_direction.transpose ().lazyProduct (pcl::ihs::dome.getVertices ()).maxCoeff (&index);
// Set the observed direction bit at 'index'
// http://stackoverflow.com/questions/47981/how-do-you-set-clear-and-toggle-a-single-bit-in-c/47990#47990
*
*/
+#ifndef Q_MOC_RUN
#pragma once
#include <boost/thread/thread.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
+#endif
#include <pcl/apps/dominant_plane_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <QTimer>
// Boost
+#ifndef Q_MOC_RUN
#include <boost/thread/thread.hpp>
+#endif
// PCL
#include <pcl/console/print.h>
set(SUBSUBSYS_DEPS common geometry io filters sample_consensus segmentation visualization kdtree features surface octree registration keypoints tracking search apps)
set(REASON "")
-# Find VTK and QVTK
-if(VTK_FOUND AND VTK_USE_QVTK)
+# Find VTK
+if(NOT VTK_FOUND)
+ set(DEFAULT AUTO_OFF)
+ set(REASON "VTK was not found.")
+else(NOT VTK_FOUND)
set(DEFAULT TRUE)
set(REASON)
set(VTK_USE_FILE "${VTK_USE_FILE}" CACHE INTERNAL "VTK_USE_FILE")
include("${VTK_USE_FILE}")
-elseif(NOT VTK_FOUND)
+endif(NOT VTK_FOUND)
+
+# QT4 Found?
+if(NOT QT4_FOUND)
set(DEFAULT AUTO_OFF)
- set(REASON "VTK was not found.")
-elseif(NOT VTK_USE_QVTK)
+ set(REASON "Qt4 was not found.")
+elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF")
+ set(DEFAULT TRUE)
+ set(REASON)
+endif(NOT QT4_FOUND)
+
+# QVTK?
+if(NOT VTK_USE_QVTK)
set(DEFAULT AUTO_OFF)
set(REASON "VTK was not built with Qt support.")
-endif(VTK_FOUND AND VTK_USE_QVTK)
+elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF")
+ set(DEFAULT TRUE)
+ set(REASON)
+endif(NOT VTK_USE_QVTK)
# Default to not building for now
if (${DEFAULT} STREQUAL "TRUE")
if(build)
include_directories("${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}/include")
- # Set Qt files and resources here
+ # Set Qt files and resources here
set(uis main_window.ui)
set(moc_incs "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/main_window.h"
"include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/scene_tree.h"
std::set<RenderWindowItem*> previous_parents;
for (QList<CloudMeshItem*>::iterator selected_cloud_meshes_it = selected_cloud_meshes.begin();
selected_cloud_meshes_it != selected_cloud_meshes.end();
- selected_cloud_meshes_it ++)
+ ++ selected_cloud_meshes_it)
{
CloudMeshItem* cloud_mesh_item = *selected_cloud_meshes_it;
RenderWindowItem* render_window_item = dynamic_cast<RenderWindowItem*>(cloud_mesh_item->parent());
std::vector<CloudMeshItem*> cloud_mesh_items;
for (QList<CloudMeshItem*>::iterator selected_cloud_meshes_it = selected_cloud_meshes.begin();
selected_cloud_meshes_it != selected_cloud_meshes.end();
- selected_cloud_meshes_it ++)
+ ++ selected_cloud_meshes_it)
{
CloudMeshItem* cloud_mesh_item = *selected_cloud_meshes_it;
if (dynamic_cast<RenderWindowItem*>(cloud_mesh_item->parent()) == NULL)
for (std::set<RenderWindowItem*>::iterator previous_parents_it = previous_parents.begin();
previous_parents_it != previous_parents.end();
- previous_parents_it ++)
+ ++ previous_parents_it)
{
(*previous_parents_it)->getRenderWindow()->updateAxes();
(*previous_parents_it)->getRenderWindow()->render();
for (QList<CloudMeshItem*>::iterator selected_cloud_meshes_it = selected_cloud_meshes.begin();
selected_cloud_meshes_it != selected_cloud_meshes.end();
- selected_cloud_meshes_it ++)
+ ++ selected_cloud_meshes_it)
{
CloudMeshItem* cloud_mesh_item_copy = new CloudMeshItem(render_window_item, *(*selected_cloud_meshes_it));
render_window_item->addChild(cloud_mesh_item_copy);
include_directories("${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}/include")
- # Set Qt files and resources here
+ # Set Qt files and resources here
# set(uis main_window.ui)
set(moc_incs "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/main_window.h"
"include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/filter_window.h"
/// @param pts a vector used to store the points whose coordinates are
/// transformed.
void
- getDisplaySpacePoints (std::vector<Point3D>& pts) const;
+ getDisplaySpacePoints (Point3DVector& pts) const;
/// @brief Returns a const reference to the internal representation of this
/// object.
/// The type used as internal representation of a cloud object.
typedef pcl::PointCloud<Point3D> Cloud3D;
+/// The type for the 3D point vector in the point cloud.
+typedef pcl::PointCloud<Point3D>::VectorType Point3DVector;
+
/// The type for boost shared pointer pointing to a PCL cloud object.
typedef Cloud3D::Ptr PclCloudPtr;
#include <QtGui>
#include <QMainWindow>
+#include <QActionGroup>
+#include <QSpinBox>
+#include <QSlider>
+#include <QMessageBox>
+#include <QMenu>
+#include <QMenuBar>
+#include <QToolBar>
#include <pcl/apps/point_cloud_editor/localTypes.h>
// Forward declaration to prevent circular inclusion
void
Cloud::draw (bool disable_highlight) const
{
- SelectionPtr selection_ptr;
- try
- {
- selection_ptr = selection_wk_ptr_.lock();
- }
- catch (boost::bad_weak_ptr)
- {
- selection_ptr.reset();
- }
+ SelectionPtr selection_ptr = selection_wk_ptr_.lock();
+
glPushAttrib(GL_CURRENT_BIT | GL_POINT_BIT | GL_COLOR_BUFFER_BIT);
{
glPointSize(point_size_);
}
void
-Cloud::getDisplaySpacePoints (std::vector<Point3D>& pts) const
+Cloud::getDisplaySpacePoints (Point3DVector& pts) const
{
for(unsigned int i = 0; i < cloud_.size(); ++i)
pts.push_back(getDisplaySpacePoint(i));
GLfloat project[16];
glGetFloatv(GL_PROJECTION_MATRIX, project);
- std::vector<Point3D> ptsvec;
+ Point3DVector ptsvec;
cloud_ptr_->getDisplaySpacePoints(ptsvec);
for(unsigned int i = 0; i < ptsvec.size(); ++i)
{
--- /dev/null
+/*
+ * face_trainer.cpp
+ *
+ * Created on: 22 Sep 2012
+ * Author: Aitor Aldoma
+ */
+
+#include "pcl/recognition/face_detection/rf_face_detector_trainer.h"
+#include <pcl/console/parse.h>
+
+int main(int argc, char ** argv)
+{
+ int ntrees = 10;
+ std::string forest_fn = "forest.txt";
+ int n_features = 1000;
+ std::string directory = "";
+ int use_normals = 0;
+ int num_images = 3000;
+
+ pcl::console::parse_argument (argc, argv, "-ntrees", ntrees);
+ pcl::console::parse_argument (argc, argv, "-forest_fn", forest_fn);
+ pcl::console::parse_argument (argc, argv, "-n_features", n_features);
+ pcl::console::parse_argument (argc, argv, "-directory", directory);
+ pcl::console::parse_argument (argc, argv, "-use_normals", use_normals);
+ pcl::console::parse_argument (argc, argv, "-num_images", num_images);
+
+ pcl::RFFaceDetectorTrainer fdrf;
+ fdrf.setForestFilename (forest_fn);
+ fdrf.setDirectory (directory);
+ fdrf.setWSize (80);
+ fdrf.setNumTrees (ntrees);
+ fdrf.setNumFeatures (n_features);
+ fdrf.setUseNormals (static_cast<bool> (use_normals));
+ fdrf.setNumTrainingImages (num_images);
+
+ //TODO: do some checks here..., fn file exists, directory exists, etc...
+ fdrf.trainWithDataProvider ();
+}
+
--- /dev/null
+/*
+ * openni_face_detection.cpp
+ *
+ * Created on: 22 Sep 2012
+ * Author: Aitor Aldoma
+ */
+
+#include "pcl/recognition/face_detection/rf_face_detector_trainer.h"
+#include <pcl/console/parse.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <boost/filesystem.hpp>
+namespace bf = boost::filesystem;
+
+#include "pcl/apps/face_detection/face_detection_apps_utils.h"
+
+bool SHOW_GT = false;
+bool VIDEO = false;
+
+template<class PointInT>
+void run(pcl::RFFaceDetectorTrainer & fdrf, typename pcl::PointCloud<PointInT>::Ptr & scene_vis, pcl::visualization::PCLVisualizer & vis, bool heat_map,
+ bool show_votes, const std::string & filename)
+{
+ pcl::PointCloud<pcl::PointXYZ>::Ptr scene (new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::copyPointCloud (*scene_vis, *scene);
+
+ fdrf.setInputCloud (scene);
+
+ if (heat_map)
+ {
+ pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ fdrf.setFaceHeatMapCloud (intensity_cloud);
+ }
+
+ fdrf.detectFaces ();
+
+ typedef typename pcl::traits::fieldList<PointInT>::type FieldListM;
+
+ float rgb_m;
+ bool exists_m;
+ pcl::for_each_type < FieldListM > (pcl::CopyIfFieldExists<PointInT, float> (scene_vis->points[0], "rgb", exists_m, rgb_m));
+
+ std::cout << "Color exists:" << static_cast<int> (exists_m) << std::endl;
+ if (exists_m)
+ {
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr to_visualize (new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::copyPointCloud (*scene_vis, *to_visualize);
+
+ pcl::visualization::PointCloudColorHandlerRGBField < pcl::PointXYZRGB > handler_keypoints (to_visualize);
+ vis.addPointCloud < pcl::PointXYZRGB > (to_visualize, handler_keypoints, "scene_cloud");
+ } else
+ {
+ vis.addPointCloud (scene_vis, "scene_cloud");
+ }
+
+ if (heat_map)
+ {
+ pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ fdrf.getFaceHeatMap (intensity_cloud);
+
+ pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_keypoints (intensity_cloud, "intensity");
+ vis.addPointCloud < pcl::PointXYZI > (intensity_cloud, handler_keypoints, "heat_map");
+ }
+
+ if (show_votes)
+ {
+ //display votes_
+ /*pcl::PointCloud<pcl::PointXYZ>::Ptr votes_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+ fdrf.getVotes(votes_cloud);
+ pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZ > handler_votes(votes_cloud, 255, 0, 0);
+ vis.addPointCloud < pcl::PointXYZ > (votes_cloud, handler_votes, "votes_cloud");
+ vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud");
+ vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "votes_cloud");
+ vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.75, "votes_cloud");*/
+
+ pcl::PointCloud<pcl::PointXYZI>::Ptr votes_cloud (new pcl::PointCloud<pcl::PointXYZI> ());
+ fdrf.getVotes2 (votes_cloud);
+ pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_votes (votes_cloud, "intensity");
+ vis.addPointCloud < pcl::PointXYZI > (votes_cloud, handler_votes, "votes_cloud");
+ vis.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud");
+ }
+
+ vis.addCoordinateSystem (0.1, "global");
+
+ std::vector<Eigen::VectorXf> heads;
+ fdrf.getDetectedFaces (heads);
+ face_detection_apps_utils::displayHeads (heads, vis);
+
+ if (SHOW_GT)
+ {
+ //check if there is ground truth data
+ std::string pose_file (filename);
+ boost::replace_all (pose_file, ".pcd", "_pose.txt");
+
+ Eigen::Matrix4f pose_mat;
+ pose_mat.setIdentity (4, 4);
+ bool result = face_detection_apps_utils::readMatrixFromFile (pose_file, pose_mat);
+
+ if (result)
+ {
+ Eigen::Vector3f ea = pose_mat.block<3, 3> (0, 0).eulerAngles (0, 1, 2);
+ Eigen::Vector3f trans_vector = Eigen::Vector3f (pose_mat (0, 3), pose_mat (1, 3), pose_mat (2, 3));
+ std::cout << ea << std::endl;
+ std::cout << trans_vector << std::endl;
+
+ pcl::PointXYZ center_point;
+ center_point.x = trans_vector[0];
+ center_point.y = trans_vector[1];
+ center_point.z = trans_vector[2];
+ vis.addSphere (center_point, 0.05, 255, 0, 0, "sphere");
+
+ pcl::ModelCoefficients cylinder_coeff;
+ cylinder_coeff.values.resize (7); // We need 7 values
+ cylinder_coeff.values[0] = center_point.x;
+ cylinder_coeff.values[1] = center_point.y;
+ cylinder_coeff.values[2] = center_point.z;
+
+ cylinder_coeff.values[3] = ea[0];
+ cylinder_coeff.values[4] = ea[1];
+ cylinder_coeff.values[5] = ea[2];
+
+ Eigen::Vector3f vec = Eigen::Vector3f::UnitZ () * -1.f;
+ Eigen::Matrix3f matrixxx;
+
+ matrixxx = Eigen::AngleAxisf (ea[0], Eigen::Vector3f::UnitX ()) * Eigen::AngleAxisf (ea[1], Eigen::Vector3f::UnitY ())
+ * Eigen::AngleAxisf (ea[2], Eigen::Vector3f::UnitZ ());
+
+ //matrixxx = pose_mat.block<3,3>(0,0);
+ vec = matrixxx * vec;
+
+ cylinder_coeff.values[3] = vec[0];
+ cylinder_coeff.values[4] = vec[1];
+ cylinder_coeff.values[5] = vec[2];
+
+ cylinder_coeff.values[6] = 0.01f;
+ vis.addCylinder (cylinder_coeff, "cylinder");
+ }
+ }
+
+ vis.setRepresentationToSurfaceForAllActors ();
+
+ if (VIDEO)
+ {
+ vis.spinOnce (50, true);
+ } else
+ {
+ vis.spin ();
+ }
+
+ vis.removeAllPointClouds ();
+ vis.removeAllShapes ();
+}
+
+int main(int argc, char ** argv)
+{
+ int STRIDE_SW = 5;
+ std::string forest_fn = "forest.txt";
+ int use_normals = 0;
+ float trans_max_variance = 800.f;
+ int min_votes_size = 400;
+ float face_threshold = 0.95f;
+ int heat_map = 1;
+ int show_votes = 0;
+ std::string test_directory;
+ std::string filename;
+ int rgb_exists = 0;
+ int pose_refinement_ = 0;
+ int icp_iterations = 5;
+ std::string model_path_;
+
+ pcl::console::parse_argument (argc, argv, "-forest_fn", forest_fn);
+ pcl::console::parse_argument (argc, argv, "-max_variance", trans_max_variance);
+ pcl::console::parse_argument (argc, argv, "-min_votes_size", min_votes_size);
+ pcl::console::parse_argument (argc, argv, "-use_normals", use_normals);
+ pcl::console::parse_argument (argc, argv, "-face_threshold", face_threshold);
+ pcl::console::parse_argument (argc, argv, "-stride_sw", STRIDE_SW);
+ pcl::console::parse_argument (argc, argv, "-heat_map", heat_map);
+ pcl::console::parse_argument (argc, argv, "-show_votes", show_votes);
+ pcl::console::parse_argument (argc, argv, "-test_directory", test_directory);
+ pcl::console::parse_argument (argc, argv, "-filename", filename);
+ pcl::console::parse_argument (argc, argv, "-rgb_exists", rgb_exists);
+ pcl::console::parse_argument (argc, argv, "-show_gt", SHOW_GT);
+ pcl::console::parse_argument (argc, argv, "-pose_refinement", pose_refinement_);
+ pcl::console::parse_argument (argc, argv, "-model_path", model_path_);
+ pcl::console::parse_argument (argc, argv, "-icp_iterations", icp_iterations);
+ pcl::console::parse_argument (argc, argv, "-video", VIDEO);
+
+ pcl::RFFaceDetectorTrainer fdrf;
+ fdrf.setForestFilename (forest_fn);
+ fdrf.setWSize (80);
+ fdrf.setUseNormals (static_cast<bool> (use_normals));
+ fdrf.setWStride (STRIDE_SW);
+ fdrf.setLeavesFaceMaxVariance (trans_max_variance);
+ fdrf.setLeavesFaceThreshold (face_threshold);
+ fdrf.setFaceMinVotes (min_votes_size);
+
+ if (pose_refinement_)
+ {
+ fdrf.setPoseRefinement (true, icp_iterations);
+ fdrf.setModelPath (model_path_);
+ }
+
+//load forest from file and pass it to the detector
+ std::filebuf fb;
+ fb.open (forest_fn.c_str (), std::ios::in);
+ std::istream os (&fb);
+
+ typedef pcl::face_detection::RFTreeNode<pcl::face_detection::FeatureType> NodeType;
+ pcl::DecisionForest<NodeType> forest;
+ forest.deserialize (os);
+ fb.close ();
+
+ fdrf.setForest (forest);
+
+ pcl::visualization::PCLVisualizer vis ("PCL Face detection");
+
+ if (test_directory.compare ("") != 0)
+ {
+ //recognize all files in directory...
+ std::string start = "";
+ std::string ext = std::string ("pcd");
+ bf::path dir = test_directory;
+
+ std::vector<std::string> files;
+ face_detection_apps_utils::getFilesInDirectory (dir, start, files, ext);
+
+ std::sort (files.begin (), files.end (), face_detection_apps_utils::sortFiles);
+
+ for (size_t i = 0; i < files.size (); i++)
+ {
+ std::stringstream file_to_process;
+ file_to_process << test_directory << "/" << files[i];
+ std::string file = file_to_process.str ();
+ std::cout << file << std::endl;
+
+ if (rgb_exists)
+ {
+ std::cout << "Loading a PointXYZRGBA cloud..." << std::endl;
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::io::loadPCDFile (file, *cloud);
+ run<pcl::PointXYZRGB> (fdrf, cloud, vis, heat_map, show_votes, file);
+ } else
+ {
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::io::loadPCDFile (file, *cloud);
+ run<pcl::PointXYZ> (fdrf, cloud, vis, heat_map, show_votes, file);
+ }
+ }
+ } else
+ {
+ if (rgb_exists)
+ {
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::io::loadPCDFile (filename, *cloud);
+ run<pcl::PointXYZRGB> (fdrf, cloud, vis, heat_map, show_votes, filename);
+ } else
+ {
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::io::loadPCDFile (filename, *cloud);
+ run<pcl::PointXYZ> (fdrf, cloud, vis, heat_map, show_votes, filename);
+ }
+ }
+}
+
--- /dev/null
+/*
+ * openni_face_detection.cpp
+ *
+ * Created on: 22 Sep 2012
+ * Author: Aitor Aldoma
+ */
+
+#include "pcl/recognition/face_detection/rf_face_detector_trainer.h"
+#include "pcl/apps/face_detection/openni_frame_source.h"
+#include "pcl/apps/face_detection/face_detection_apps_utils.h"
+#include <pcl/common/time.h>
+#include <pcl/console/parse.h>
+#include <pcl/features/integral_image_normal.h>
+
+void run(pcl::RFFaceDetectorTrainer & fdrf, bool heat_map = false, bool show_votes = false)
+{
+ OpenNIFrameSource::OpenNIFrameSource camera;
+ OpenNIFrameSource::PointCloudPtr scene_vis;
+
+ pcl::visualization::PCLVisualizer vis ("Face dection");
+ vis.addCoordinateSystem (0.1, "global");
+
+ //keyboard callback to stop getting frames and finalize application
+ boost::function<void(const pcl::visualization::KeyboardEvent&)> keyboard_cb = boost::bind (&OpenNIFrameSource::OpenNIFrameSource::onKeyboardEvent, &camera,
+ _1);
+ vis.registerKeyboardCallback (keyboard_cb);
+
+ while (camera.isActive ())
+ {
+ scene_vis = camera.snap ();
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr scene (new pcl::PointCloud<pcl::PointXYZ> ());
+ pcl::copyPointCloud (*scene_vis, *scene);
+
+ fdrf.setInputCloud (scene);
+
+ if (heat_map)
+ {
+ pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ fdrf.setFaceHeatMapCloud (intensity_cloud);
+ }
+
+ {
+ pcl::ScopeTime t ("Detect faces...");
+ fdrf.detectFaces ();
+ }
+ pcl::visualization::PointCloudColorHandlerRGBField<OpenNIFrameSource::PointT> handler_keypoints (scene_vis);
+ vis.addPointCloud < OpenNIFrameSource::PointT > (scene_vis, handler_keypoints, "scene_cloud");
+
+ if (heat_map)
+ {
+ pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ fdrf.getFaceHeatMap (intensity_cloud);
+ pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_keypoints (intensity_cloud, "intensity");
+ vis.addPointCloud < pcl::PointXYZI > (intensity_cloud, handler_keypoints, "heat_map");
+ }
+
+ if (show_votes)
+ {
+ //display votes_
+ pcl::PointCloud<pcl::PointXYZ>::Ptr votes_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
+ fdrf.getVotes (votes_cloud);
+ pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZ > handler_votes (votes_cloud, 255, 0, 0);
+ vis.addPointCloud < pcl::PointXYZ > (votes_cloud, handler_votes, "votes_cloud");
+ vis.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud");
+ vis.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "votes_cloud");
+ vis.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.75, "votes_cloud");
+ }
+
+ std::vector<Eigen::VectorXf> heads;
+ fdrf.getDetectedFaces (heads);
+
+ face_detection_apps_utils::displayHeads (heads, vis);
+
+ vis.setRepresentationToSurfaceForAllActors ();
+ vis.spinOnce ();
+
+ vis.removeAllPointClouds ();
+ vis.removeAllShapes ();
+ }
+}
+
+//./bin/pcl_openni_face_detector -face_threshold 0.99 -max_variance 2400 -min_votes_size 300 -stride_sw 4 -heat_map 0 -show_votes 1 -pose_refinement 1 -icp_iterations 5 -model_path face_model.pcd -forest_fn forest.txt
+int main(int argc, char ** argv)
+{
+ int STRIDE_SW = 4;
+ int use_normals = 0;
+ float trans_max_variance = 1600.f;
+ int min_votes_size = 300;
+ float face_threshold = 0.99f;
+ int heat_map = 1;
+ int show_votes = 0;
+ int pose_refinement_ = 0;
+ int icp_iterations = 5;
+
+ std::string forest_fn = "../data/forests/forest.txt";
+ std::string model_path_ = "../data/ply_models/face.pcd";
+
+ pcl::console::parse_argument (argc, argv, "-forest_fn", forest_fn);
+ pcl::console::parse_argument (argc, argv, "-max_variance", trans_max_variance);
+ pcl::console::parse_argument (argc, argv, "-min_votes_size", min_votes_size);
+ pcl::console::parse_argument (argc, argv, "-use_normals", use_normals);
+ pcl::console::parse_argument (argc, argv, "-face_threshold", face_threshold);
+ pcl::console::parse_argument (argc, argv, "-stride_sw", STRIDE_SW);
+ pcl::console::parse_argument (argc, argv, "-heat_map", heat_map);
+ pcl::console::parse_argument (argc, argv, "-show_votes", show_votes);
+ pcl::console::parse_argument (argc, argv, "-pose_refinement", pose_refinement_);
+ pcl::console::parse_argument (argc, argv, "-model_path", model_path_);
+ pcl::console::parse_argument (argc, argv, "-icp_iterations", icp_iterations);
+
+ pcl::RFFaceDetectorTrainer fdrf;
+ fdrf.setForestFilename (forest_fn);
+ fdrf.setWSize (80);
+ fdrf.setUseNormals (static_cast<bool> (use_normals));
+ fdrf.setWStride (STRIDE_SW);
+ fdrf.setLeavesFaceMaxVariance (trans_max_variance);
+ fdrf.setLeavesFaceThreshold (face_threshold);
+ fdrf.setFaceMinVotes (min_votes_size);
+
+ if (pose_refinement_)
+ {
+ fdrf.setPoseRefinement (true, icp_iterations);
+ fdrf.setModelPath (model_path_);
+ }
+
+//load forest from file and pass it to the detector
+ std::filebuf fb;
+ fb.open (forest_fn.c_str (), std::ios::in);
+ std::istream os (&fb);
+
+ typedef pcl::face_detection::RFTreeNode<pcl::face_detection::FeatureType> NodeType;
+ pcl::DecisionForest<NodeType> forest;
+ forest.deserialize (os);
+ fb.close ();
+
+ fdrf.setForest (forest);
+
+ run (fdrf, heat_map, show_votes);
+}
+
--- /dev/null
+#include "pcl/apps/face_detection/openni_frame_source.h"
+#include <pcl/io/pcd_io.h>
+#include <boost/thread/mutex.hpp>
+#include <boost/make_shared.hpp>
+
+namespace OpenNIFrameSource
+{
+
+ OpenNIFrameSource::OpenNIFrameSource(const std::string& device_id) :
+ grabber_ (device_id), most_recent_frame_ (), frame_counter_ (0), active_ (true)
+ {
+ boost::function<void(const PointCloudConstPtr&)> frame_cb = boost::bind (&OpenNIFrameSource::onNewFrame, this, _1);
+ grabber_.registerCallback (frame_cb);
+ grabber_.start ();
+ }
+
+ OpenNIFrameSource::~OpenNIFrameSource()
+ {
+ // Stop the grabber when shutting down
+ grabber_.stop ();
+ }
+
+ bool OpenNIFrameSource::isActive()
+ {
+ return active_;
+ }
+
+ const PointCloudPtr OpenNIFrameSource::snap()
+ {
+ return (most_recent_frame_);
+ }
+
+ void OpenNIFrameSource::onNewFrame(const PointCloudConstPtr &cloud)
+ {
+ mutex_.lock ();
+ ++frame_counter_;
+ most_recent_frame_ = boost::make_shared < PointCloud > (*cloud); // Make a copy of the frame
+ mutex_.unlock ();
+ }
+
+ void OpenNIFrameSource::onKeyboardEvent(const pcl::visualization::KeyboardEvent & event)
+ {
+ // When the spacebar is pressed, trigger a frame capture
+ mutex_.lock ();
+ if (event.keyDown () && event.getKeySym () == "e")
+ {
+ active_ = false;
+ }
+ mutex_.unlock ();
+ }
+
+}
keypoints3d.height = keypoints->height;
keypoints3d.is_dense = true;
- int j = 0;
+ size_t j = 0;
for (size_t i = 0; i < keypoints->size (); ++i)
{
const PointT &pt = (*cloud)(static_cast<long unsigned int> (keypoints->points[i].u),
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: openni_viewer.cpp 5059 2012-03-14 02:12:17Z gedikli $
+ *
+ */
+
+#define SHOW_FPS 1
+#include <pcl/apps/timer.h>
+#include <pcl/common/common.h>
+#include <pcl/common/angles.h>
+#include <pcl/common/time.h>
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/keypoints/brisk_2d.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/visualization/image_viewer.h>
+#include <pcl/console/print.h>
+#include <pcl/console/parse.h>
+
+using namespace pcl;
+using namespace std;
+
+typedef PointXYZRGBA PointT;
+typedef PointWithScale KeyPointT;
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+class BRISKDemo
+{
+ public:
+ typedef PointCloud<PointT> Cloud;
+ typedef Cloud::Ptr CloudPtr;
+ typedef Cloud::ConstPtr CloudConstPtr;
+
+ BRISKDemo (Grabber& grabber)
+ : cloud_viewer_ ("BRISK 2D Keypoints -- PointCloud")
+ , grabber_ (grabber)
+ , image_viewer_ ("BRISK 2D Keypoints -- Image")
+ {
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ cloud_callback (const CloudConstPtr& cloud)
+ {
+ FPS_CALC ("cloud callback");
+ boost::mutex::scoped_lock lock (cloud_mutex_);
+ cloud_ = cloud;
+
+ // Compute BRISK keypoints
+ BriskKeypoint2D<PointT> brisk;
+ brisk.setThreshold (60);
+ brisk.setOctaves (4);
+ brisk.setInputCloud (cloud);
+
+ keypoints_.reset (new PointCloud<KeyPointT>);
+ brisk.compute (*keypoints_);
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ init ()
+ {
+ boost::function<void (const CloudConstPtr&) > cloud_cb = boost::bind (&BRISKDemo::cloud_callback, this, _1);
+ cloud_connection = grabber_.registerCallback (cloud_cb);
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ string
+ getStrBool (bool state)
+ {
+ stringstream ss;
+ ss << state;
+ return (ss.str ());
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ inline PointT
+ bilinearInterpolation (const CloudConstPtr &cloud, float x, float y)
+ {
+ int u = int (x);
+ int v = int (y);
+
+ PointT pt;
+ pt.x = pt.y = pt.z = 0;
+
+ const PointT &p1 = (*cloud)(u, v);
+ const PointT &p2 = (*cloud)(u+1, v);
+ const PointT &p3 = (*cloud)(u, v+1);
+ const PointT &p4 = (*cloud)(u+1, v+1);
+
+ float fx = x - float (u), fy = y - float (v);
+ float fx1 = 1.0f - fx, fy1 = 1.0f - fy;
+
+ float w1 = fx1 * fy1, w2 = fx * fy1, w3 = fx1 * fy, w4 = fx * fy;
+ float weight = 0;
+
+ if (isFinite (p1))
+ {
+ pt.x += p1.x * w1;
+ pt.y += p1.y * w1;
+ pt.z += p1.z * w1;
+ weight += w1;
+ }
+ if (isFinite (p2))
+ {
+ pt.x += p2.x * w2;
+ pt.y += p2.y * w2;
+ pt.z += p2.z * w2;
+ weight += w2;
+ }
+ if (isFinite (p3))
+ {
+ pt.x += p3.x * w3;
+ pt.y += p3.y * w3;
+ pt.z += p3.z * w3;
+ weight += w3;
+ }
+ if (isFinite (p4))
+ {
+ pt.x += p4.x * w4;
+ pt.y += p4.y * w4;
+ pt.z += p4.z * w4;
+ weight += w4;
+ }
+
+ if (weight == 0)
+ pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
+ else
+ {
+ weight = 1.0f / weight;
+ pt.x *= weight; pt.y *= weight; pt.z *= weight;
+ }
+
+ return (pt);
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ get3DKeypoints (
+ const CloudConstPtr &cloud,
+ const PointCloud<KeyPointT>::Ptr &keypoints, PointCloud<PointT> &keypoints3d)
+ {
+ keypoints3d.resize (keypoints->size ());
+ keypoints3d.width = keypoints->width;
+ keypoints3d.height = keypoints->height;
+ keypoints3d.is_dense = true;
+
+ size_t j = 0;
+ for (size_t i = 0; i < keypoints->size (); ++i)
+ {
+ PointT pt = bilinearInterpolation (cloud, keypoints->points[i].x, keypoints->points[i].y);
+
+ keypoints3d.points[j].x = pt.x;
+ keypoints3d.points[j].y = pt.y;
+ keypoints3d.points[j].z = pt.z;
+ ++j;
+ }
+
+ if (j != keypoints->size ())
+ {
+ keypoints3d.resize (j);
+ keypoints3d.width = j;
+ keypoints3d.height = 1;
+ }
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ void
+ run ()
+ {
+ grabber_.start ();
+
+ bool image_init = false, cloud_init = false;
+ bool keypts = true;
+
+ PointCloud<KeyPointT>::Ptr keypoints;
+ CloudConstPtr cloud;
+ CloudPtr keypoints3d (new Cloud);
+
+ while (!cloud_viewer_.wasStopped () && !image_viewer_.wasStopped ())
+ {
+ if (cloud_mutex_.try_lock ())
+ {
+ if (cloud_)
+ cloud_.swap (cloud);
+
+ if (keypoints_)
+ keypoints_.swap (keypoints);
+
+ cloud_mutex_.unlock ();
+
+ if (!cloud)
+ continue;
+
+ if (!cloud_init)
+ {
+ cloud_viewer_.setPosition (0, 0);
+ cloud_viewer_.setSize (cloud->width, cloud->height);
+ cloud_init = !cloud_init;
+ }
+
+ if (!cloud_viewer_.updatePointCloud (cloud, "OpenNICloud"))
+ {
+ cloud_viewer_.addPointCloud (cloud, "OpenNICloud");
+ cloud_viewer_.resetCameraViewpoint ("OpenNICloud");
+ }
+
+ if (!image_init)
+ {
+ image_viewer_.setPosition (cloud->width, 0);
+ image_viewer_.setSize (cloud->width, cloud->height);
+ image_init = !image_init;
+ }
+
+ image_viewer_.showRGBImage<PointT> (cloud);
+
+ image_viewer_.removeLayer (getStrBool (keypts));
+ for (size_t i = 0; i < keypoints->size (); ++i)
+ {
+ int u = int (keypoints->points[i].x);
+ int v = int (keypoints->points[i].y);
+ image_viewer_.markPoint (u, v, visualization::red_color, visualization::blue_color, 10, getStrBool (!keypts));
+ }
+ keypts = !keypts;
+
+
+ get3DKeypoints (cloud, keypoints, *keypoints3d);
+ visualization::PointCloudColorHandlerCustom<PointT> blue (keypoints3d, 0, 0, 255);
+ if (!cloud_viewer_.updatePointCloud (keypoints3d, blue, "keypoints"))
+ cloud_viewer_.addPointCloud (keypoints3d, blue, "keypoints");
+ cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_POINT_SIZE, 10, "keypoints");
+ cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_OPACITY, 0.5, "keypoints");
+ }
+
+ cloud_viewer_.spinOnce ();
+ image_viewer_.spinOnce ();
+ boost::this_thread::sleep (boost::posix_time::microseconds (100));
+ }
+
+ grabber_.stop ();
+
+ cloud_connection.disconnect ();
+ }
+
+ visualization::PCLVisualizer cloud_viewer_;
+ Grabber& grabber_;
+ boost::mutex cloud_mutex_;
+ CloudConstPtr cloud_;
+
+ visualization::ImageViewer image_viewer_;
+
+ PointCloud<KeyPointT>::Ptr keypoints_;
+
+ private:
+ boost::signals2::connection cloud_connection;
+};
+
+/* ---[ */
+int
+main (int, char**)
+{
+ string device_id ("#1");
+ OpenNIGrabber grabber (device_id);
+ BRISKDemo openni_viewer (grabber);
+
+ openni_viewer.init ();
+ openni_viewer.run ();
+
+ return (0);
+}
+/* ]--- */
filtered_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBA> (*cloud));
filtered_cloud->points.reserve(newPointIdxVector->size());
- for (std::vector<int>::iterator it = newPointIdxVector->begin (); it != newPointIdxVector->end (); it++)
+ for (std::vector<int>::iterator it = newPointIdxVector->begin (); it != newPointIdxVector->end (); ++it)
filtered_cloud->points[*it].rgba = 255<<16;
if (!viewer.wasStopped())
filtered_cloud->points.reserve(newPointIdxVector->size());
- for (std::vector<int>::iterator it = newPointIdxVector->begin (); it != newPointIdxVector->end (); it++)
+ for (std::vector<int>::iterator it = newPointIdxVector->begin (); it != newPointIdxVector->end (); ++it)
filtered_cloud->points.push_back(cloud->points[*it]);
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+
+#include <pcl/common/time.h>
+#include <pcl/features/integral_image_normal.h>
+#include <pcl/features/organized_edge_detection.h>
+#include <pcl/io/io.h>
+#include <pcl/io/openni_grabber.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+typedef pcl::PointXYZRGBA PointT;
+
+class OpenNIOrganizedEdgeDetection
+{
+ private:
+ boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+ pcl::PointCloud<PointT> cloud_;
+ boost::mutex cloud_mutex;
+
+ public:
+ OpenNIOrganizedEdgeDetection ()
+ : viewer (new pcl::visualization::PCLVisualizer ("PCL Organized Edge Detection"))
+ {
+
+ }
+ ~OpenNIOrganizedEdgeDetection ()
+ {
+ }
+
+ boost::shared_ptr<pcl::visualization::PCLVisualizer>
+ initCloudViewer (pcl::PointCloud<PointT>::ConstPtr cloud)
+ {
+ viewer->setSize (640, 480);
+ viewer->addPointCloud<PointT> (cloud, "cloud");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");
+ viewer->addCoordinateSystem (0.2f, "global");
+ viewer->initCameraParameters ();
+ viewer->registerKeyboardCallback (&OpenNIOrganizedEdgeDetection::keyboard_callback, *this);
+ viewer->resetCameraViewpoint ("cloud");
+
+ const int point_size = 2;
+ viewer->addPointCloud<PointT> (cloud, "nan boundary edges");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "nan boundary edges");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 0.0f, 1.0f, "nan boundary edges");
+
+ viewer->addPointCloud<PointT> (cloud, "occluding edges");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluding edges");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 0.0f, "occluding edges");
+
+ viewer->addPointCloud<PointT> (cloud, "occluded edges");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluded edges");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 0.0f, 0.0f, "occluded edges");
+
+ viewer->addPointCloud<PointT> (cloud, "high curvature edges");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "high curvature edges");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 1.0f, 0.0f, "high curvature edges");
+
+ viewer->addPointCloud<PointT> (cloud, "rgb edges");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "rgb edges");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 1.0f, "rgb edges");
+
+ return (viewer);
+ }
+
+ void
+ keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
+ {
+ double opacity;
+ if (event.keyUp())
+ {
+ switch (event.getKeyCode())
+ {
+ case '1':
+ viewer->getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "nan boundary edges");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "nan boundary edges");
+ break;
+ case '2':
+ viewer->getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluding edges");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "occluding edges");
+ break;
+ case '3':
+ viewer->getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluded edges");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "occluded edges");
+ break;
+ case '4':
+ viewer->getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "high curvature edges");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "high curvature edges");
+ break;
+ case '5':
+ viewer->getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "rgb edges");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "rgb edges");
+ break;
+ }
+ }
+ }
+
+ void
+ cloud_cb_ (const pcl::PointCloud<PointT>::ConstPtr& cloud)
+ {
+ if (!viewer->wasStopped ())
+ {
+ cloud_mutex.lock ();
+ cloud_ = *cloud;
+ cloud_mutex.unlock ();
+ }
+ }
+
+ void
+ run ()
+ {
+ pcl::Grabber* interface = new pcl::OpenNIGrabber ();
+
+ boost::function<void(const pcl::PointCloud<PointT>::ConstPtr&)> f = boost::bind (&OpenNIOrganizedEdgeDetection::cloud_cb_, this, _1);
+
+ // Make and initialize a cloud viewer
+ pcl::PointCloud<PointT>::Ptr init_cloud_ptr (new pcl::PointCloud<PointT>);
+ viewer = initCloudViewer (init_cloud_ptr);
+ boost::signals2::connection c = interface->registerCallback (f);
+
+ interface->start ();
+
+ pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
+ ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
+ ne.setNormalSmoothingSize (10.0f);
+ ne.setBorderPolicy (ne.BORDER_POLICY_MIRROR);
+
+ float th_dd = 0.04f;
+ int max_search = 100;
+ pcl::OrganizedEdgeFromRGBNormals<PointT, pcl::Normal, pcl::Label> oed;
+ oed.setDepthDisconThreshold (th_dd);
+ oed.setMaxSearchNeighbors (max_search);
+
+ oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING | oed.EDGELABEL_OCCLUDED);
+ //oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING | oed.EDGELABEL_OCCLUDED | oed.EDGELABEL_HIGH_CURVATURE | oed.EDGELABEL_RGB_CANNY);
+
+ pcl::PointCloud<pcl::Label> labels;
+ std::vector<pcl::PointIndices> label_indices;
+
+ while (!viewer->wasStopped ())
+ {
+ viewer->spinOnce ();
+
+ if (cloud_mutex.try_lock ())
+ {
+ labels.clear ();
+ label_indices.clear ();
+
+ double normal_start = pcl::getTime ();
+
+ if (oed.getEdgeType () & oed.EDGELABEL_HIGH_CURVATURE)
+ {
+ pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
+ ne.setInputCloud (cloud_.makeShared ());
+ ne.compute (*normal_cloud);
+ double normal_end = pcl::getTime ();
+ std::cout << "Normal Estimation took " << double (normal_end - normal_start) << std::endl;
+
+ oed.setInputNormals (normal_cloud);
+ }
+
+ double oed_start = pcl::getTime ();
+
+ oed.setInputCloud (cloud_.makeShared ());
+ oed.compute (labels, label_indices);
+
+ double oed_end = pcl::getTime ();
+ std::cout << "Edge Detection took " << double (oed_end - oed_start) << std::endl;
+ std::cout << "Frame took " << double (oed_end - normal_start) << std::endl;
+
+ // Make gray point cloud
+ for (size_t idx = 0; idx < cloud_.points.size (); idx++)
+ {
+ pcl::uint8_t gray = pcl::uint8_t((cloud_.points[idx].r + cloud_.points[idx].g + cloud_.points[idx].b)/3);
+ cloud_.points[idx].r = cloud_.points[idx].g = cloud_.points[idx].b = gray;
+ }
+
+ // Show the gray point cloud
+ if (!viewer->updatePointCloud (cloud_.makeShared (), "cloud"))
+ viewer->addPointCloud (cloud_.makeShared (), "cloud");
+
+ // Show edges
+ pcl::PointCloud<PointT>::Ptr occluding_edges (new pcl::PointCloud<PointT>),
+ occluded_edges (new pcl::PointCloud<PointT>),
+ nan_boundary_edges (new pcl::PointCloud<PointT>),
+ high_curvature_edges (new pcl::PointCloud<PointT>),
+ rgb_edges (new pcl::PointCloud<PointT>);
+
+ pcl::copyPointCloud (cloud_, label_indices[0].indices, *nan_boundary_edges);
+ pcl::copyPointCloud (cloud_, label_indices[1].indices, *occluding_edges);
+ pcl::copyPointCloud (cloud_, label_indices[2].indices, *occluded_edges);
+ pcl::copyPointCloud (cloud_, label_indices[3].indices, *high_curvature_edges);
+ pcl::copyPointCloud (cloud_, label_indices[4].indices, *rgb_edges);
+
+ if (!viewer->updatePointCloud<PointT> (nan_boundary_edges, "nan boundary edges"))
+ viewer->addPointCloud<PointT> (nan_boundary_edges, "nan boundary edges");
+
+ if (!viewer->updatePointCloud<PointT> (occluding_edges, "occluding edges"))
+ viewer->addPointCloud<PointT> (occluding_edges, "occluding edges");
+
+ if (!viewer->updatePointCloud<PointT> (occluded_edges, "occluded edges"))
+ viewer->addPointCloud<PointT> (occluded_edges, "occluded edges");
+
+ if (!viewer->updatePointCloud<PointT> (high_curvature_edges, "high curvature edges"))
+ viewer->addPointCloud<PointT> (high_curvature_edges, "high curvature edges");
+
+ if (!viewer->updatePointCloud<PointT> (rgb_edges, "rgb edges"))
+ viewer->addPointCloud<PointT> (rgb_edges, "rgb edges");
+
+ cloud_mutex.unlock ();
+ }
+ }
+
+ interface->stop ();
+ }
+};
+
+void
+usage (char ** argv)
+{
+ std::cout << "usage: " << argv[0] << " <device_id> <options>\n\n";
+
+ openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
+ if (driver.getNumberDevices () > 0)
+ {
+ for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
+ {
+ cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
+ << ", connected: " << driver.getBus (deviceIdx) << " @ " << driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << endl;
+ cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << endl
+ << " bus@address for the device connected to a specific usb-bus / address combination (works only in Linux) or" << endl
+ << " <serial-number> (only in Linux and for devices which provide serial numbers)" << endl;
+ }
+ }
+ else
+ cout << "No devices connected." << endl;
+}
+
+int
+main (int argc, char ** argv)
+{
+ std::string arg;
+ if (argc > 1)
+ arg = std::string (argv[1]);
+
+ if (arg == "--help" || arg == "-h")
+ {
+ usage (argv);
+ return 1;
+ }
+
+ std::cout << "Press following keys to enable/disable the different edge types:" << std::endl;
+ std::cout << "<1> EDGELABEL_NAN_BOUNDARY edge" << std::endl;
+ std::cout << "<2> EDGELABEL_OCCLUDING edge" << std::endl;
+ std::cout << "<3> EDGELABEL_OCCLUDED edge" << std::endl;
+ //std::cout << "<4> EDGELABEL_HIGH_CURVATURE edge" << std::endl;
+ //std::cout << "<5> EDGELABEL_RGB_CANNY edge" << std::endl;
+ std::cout << "<Q,q> quit" << std::endl;
+
+
+ pcl::OpenNIGrabber grabber ("");
+ if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgb> ())
+ {
+ OpenNIOrganizedEdgeDetection app;
+ app.run ();
+ }
+ else
+ PCL_ERROR ("The input device does not provide a PointXYZRGBA mode.\n");
+
+ return (0);
+}
}
void addNormalToCloud (const CloudConstPtr &cloud,
- const pcl::PointCloud<pcl::Normal>::ConstPtr &normals,
+ const pcl::PointCloud<pcl::Normal>::ConstPtr &,
RefCloud &result)
{
result.width = cloud->width;
#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/openni_camera/openni_driver.h>
-#include <pcl/keypoints/uniform_sampling.h>
+#include <pcl/filters/uniform_sampling.h>
#include <pcl/console/parse.h>
#include <pcl/common/time.h>
FPS_CALC ("computation");
cloud_.reset (new Cloud);
- indices_.reset (new pcl::PointCloud<int>);
keypoints_.reset (new pcl::PointCloud<pcl::PointXYZ>);
// Computation goes here
pass_.setInputCloud (cloud);
- pass_.compute (*indices_);
+ pcl::PointCloud<pcl::PointXYZRGBA> sampled;
+ pass_.filter (sampled);
*cloud_ = *cloud;
- pcl::copyPointCloud<pcl::PointXYZRGBA, pcl::PointXYZ> (*cloud, indices_->points, *keypoints_);
+ pcl::copyPointCloud<pcl::PointXYZRGBA, pcl::PointXYZ> (sampled, *keypoints_);
}
void
boost::mutex mtx_;
CloudPtr cloud_;
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_;
- pcl::PointCloud<int>::Ptr indices_;
};
void
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ *
+ */
+
+#include <pcl/console/print.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/time.h>
+#include <pcl/features/organized_edge_detection.h>
+#include <pcl/features/integral_image_normal.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/PCLPointCloud2.h>
+
+using namespace pcl;
+using namespace pcl::io;
+using namespace pcl::console;
+
+float default_th_dd = 0.02f;
+int default_max_search = 50;
+
+typedef pcl::PointCloud<pcl::PointXYZRGBA> Cloud;
+typedef Cloud::Ptr CloudPtr;
+typedef Cloud::ConstPtr CloudConstPtr;
+
+pcl::visualization::PCLVisualizer viewer ("3D Edge Viewer");
+
+void
+printHelp (int, char **argv)
+{
+ print_error ("Syntax is: %s input.pcd output.pcd <options>\n", argv[0]);
+ print_info (" where options are:\n");
+ print_info (" -th_dd X = the tolerance in meters for difference in depth values between neighboring points (The value is set for 1 meter and is adapted with respect to depth value linearly. (e.g. 2.0*th_dd in 2 meter depth)) (default: ");
+ print_value ("%f", default_th_dd); print_info (")\n");
+ print_info (" -max_search X = the max search distance for deciding occluding and occluded edges (default: ");
+ print_value ("%d", default_max_search); print_info (")\n");
+}
+
+bool
+loadCloud (const std::string &filename, pcl::PCLPointCloud2 &cloud)
+{
+ TicToc tt;
+ print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
+
+ tt.tic ();
+ if (loadPCDFile (filename, cloud) < 0)
+ return (false);
+ print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
+ print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());
+
+ return (true);
+}
+
+void
+saveCloud (const std::string &filename, const pcl::PCLPointCloud2 &output)
+{
+ TicToc tt;
+ tt.tic ();
+
+ print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
+
+ pcl::io::savePCDFile (filename, output, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true); // Save as binary
+
+ print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
+}
+
+void
+keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
+{
+ double opacity;
+ if (event.keyUp())
+ {
+ switch (event.getKeyCode())
+ {
+ case '1':
+ viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "nan boundary edges");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "nan boundary edges");
+ break;
+ case '2':
+ viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluding edges");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "occluding edges");
+ break;
+ case '3':
+ viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluded edges");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "occluded edges");
+ break;
+ case '4':
+ viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "high curvature edges");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "high curvature edges");
+ break;
+ case '5':
+ viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "rgb edges");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "rgb edges");
+ break;
+ }
+ }
+}
+
+void
+compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output,
+ float th_dd, int max_search)
+{
+ CloudPtr cloud (new Cloud);
+ fromPCLPointCloud2 (*input, *cloud);
+
+ pcl::PointCloud<pcl::Normal>::Ptr normal (new pcl::PointCloud<pcl::Normal>);
+ pcl::IntegralImageNormalEstimation<PointXYZRGBA, pcl::Normal> ne;
+ ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
+ ne.setNormalSmoothingSize (10.0f);
+ ne.setBorderPolicy (ne.BORDER_POLICY_MIRROR);
+ ne.setInputCloud (cloud);
+ ne.compute (*normal);
+
+ TicToc tt;
+ tt.tic ();
+
+ //OrganizedEdgeBase<PointXYZRGBA, Label> oed;
+ //OrganizedEdgeFromRGB<PointXYZRGBA, Label> oed;
+ //OrganizedEdgeFromNormals<PointXYZRGBA, Normal, Label> oed;
+ OrganizedEdgeFromRGBNormals<PointXYZRGBA, Normal, Label> oed;
+ oed.setInputNormals (normal);
+ oed.setInputCloud (cloud);
+ oed.setDepthDisconThreshold (th_dd);
+ oed.setMaxSearchNeighbors (max_search);
+ oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING | oed.EDGELABEL_OCCLUDED | oed.EDGELABEL_HIGH_CURVATURE | oed.EDGELABEL_RGB_CANNY);
+ PointCloud<Label> labels;
+ std::vector<PointIndices> label_indices;
+ oed.compute (labels, label_indices);
+ print_info ("Detecting all edges... [done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n");
+
+ // Make gray point clouds
+ for (size_t idx = 0; idx < cloud->points.size (); idx++)
+ {
+ uint8_t gray = uint8_t ((cloud->points[idx].r + cloud->points[idx].g + cloud->points[idx].b) / 3);
+ cloud->points[idx].r = cloud->points[idx].g = cloud->points[idx].b = gray;
+ }
+
+ // Display edges in PCLVisualizer
+ viewer.setSize (640, 480);
+ viewer.addCoordinateSystem (0.2f, "global");
+ viewer.addPointCloud (cloud, "original point cloud");
+ viewer.registerKeyboardCallback(&keyboard_callback);
+
+ pcl::PointCloud<pcl::PointXYZRGBA>::Ptr occluding_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
+ occluded_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
+ nan_boundary_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
+ high_curvature_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
+ rgb_edges (new pcl::PointCloud<pcl::PointXYZRGBA>);
+
+ pcl::copyPointCloud (*cloud, label_indices[0].indices, *nan_boundary_edges);
+ pcl::copyPointCloud (*cloud, label_indices[1].indices, *occluding_edges);
+ pcl::copyPointCloud (*cloud, label_indices[2].indices, *occluded_edges);
+ pcl::copyPointCloud (*cloud, label_indices[3].indices, *high_curvature_edges);
+ pcl::copyPointCloud (*cloud, label_indices[4].indices, *rgb_edges);
+
+ const int point_size = 2;
+ viewer.addPointCloud<pcl::PointXYZRGBA> (nan_boundary_edges, "nan boundary edges");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "nan boundary edges");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 0.0f, 1.0f, "nan boundary edges");
+
+ viewer.addPointCloud<pcl::PointXYZRGBA> (occluding_edges, "occluding edges");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluding edges");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 0.0f, "occluding edges");
+
+ viewer.addPointCloud<pcl::PointXYZRGBA> (occluded_edges, "occluded edges");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluded edges");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 0.0f, 0.0f, "occluded edges");
+
+ viewer.addPointCloud<pcl::PointXYZRGBA> (high_curvature_edges, "high curvature edges");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "high curvature edges");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 1.0f, 0.0f, "high curvature edges");
+
+ viewer.addPointCloud<pcl::PointXYZRGBA> (rgb_edges, "rgb edges");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "rgb edges");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 1.0f, "rgb edges");
+
+ while (!viewer.wasStopped ())
+ {
+ viewer.spinOnce ();
+ boost::this_thread::sleep (boost::posix_time::microseconds (100));
+ }
+
+ // Combine point clouds and edge labels
+ pcl::PCLPointCloud2 output_edges;
+ toPCLPointCloud2 (labels, output_edges);
+ concatenateFields (*input, output_edges, output);
+}
+
+int
+main (int argc, char** argv)
+{
+ print_info ("Detect 3D edges from organized point cloud data. For more information, use: %s -h\n", argv[0]);
+ bool help = false;
+ parse_argument (argc, argv, "-h", help);
+ if (argc < 3 || help)
+ {
+ printHelp (argc, argv);
+ return (-1);
+ }
+
+ // Parse the command line arguments for .pcd files
+ std::vector<int> p_file_indices;
+ p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
+ if (p_file_indices.size () != 2)
+ {
+ print_error ("Need one input PCD file and one output PCD file to continue.\n");
+ return (-1);
+ }
+
+ // Command line parsing
+ float th_dd = default_th_dd;
+ int max_search = default_max_search;
+
+ parse_argument (argc, argv, "-th_dd", th_dd);
+ parse_argument (argc, argv, "-max_search", max_search);
+
+ print_info ("th_dd: "); print_value ("%f\n", th_dd);
+ print_info ("max_search: "); print_value ("%d\n", max_search);
+
+ // Load the first file
+ pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2);
+ if (!loadCloud (argv[p_file_indices[0]], *cloud))
+ return (-1);
+
+ // Perform the feature estimation
+ pcl::PCLPointCloud2 output;
+
+ compute (cloud, output, th_dd, max_search);
+
+ // Save into the second file
+ saveCloud (argv[p_file_indices[1]], output);
+}
+
#include <vtkLoopSubdivisionFilter.h>
#include <vtkTriangle.h>
#include <vtkTransform.h>
-#if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>4)
+#if VTK_MAJOR_VERSION>=6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>4)
#include <vtkHardwareSelector.h>
#include <vtkSelectionNode.h>
#else
vtkSmartPointer<vtkLoopSubdivisionFilter> subdivide = vtkSmartPointer<vtkLoopSubdivisionFilter>::New ();
subdivide->SetNumberOfSubdivisions (tesselation_level_);
subdivide->SetInputConnection (ico->GetOutputPort ());
+#if VTK_MAJOR_VERSION>=6
+ subdivide->Update();
+#endif
// Get camera positions
vtkPolyData *sphere = subdivide->GetOutput ();
+#if VTK_MAJOR_VERSION<6
+ sphere->Update ();
+#endif
- std::vector<Eigen::Vector3f> cam_positions;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > cam_positions;
if (!use_vertices_)
{
vtkSmartPointer<vtkCellArray> cells_sphere = sphere->GetPolys ();
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/io/openni_grabber.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/visualization/image_viewer.h>
+#include <pcl/io/io.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/png_io.h>
+#include <pcl/io/pcd_grabber.h>
+#include <pcl/common/time.h>
+#include <pcl/common/distances.h>
+#include <pcl/common/intersections.h>
+#include <pcl/features/integral_image_normal.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/ModelCoefficients.h>
+#include <pcl/segmentation/planar_region.h>
+#include <pcl/segmentation/organized_multi_plane_segmentation.h>
+#include <pcl/segmentation/organized_connected_component_segmentation.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/sample_consensus/sac_model_plane.h>
+//#include <pcl/stereo/stereo_grabber.h>
+#include <pcl/stereo/stereo_matching.h>
+#include <pcl/segmentation/ground_plane_comparator.h>
+#include <pcl/segmentation/euclidean_cluster_comparator.h>
+
+
+
+typedef pcl::PointXYZRGB PointT;
+typedef pcl::PointCloud<PointT> Cloud;
+typedef Cloud::Ptr CloudPtr;
+typedef Cloud::ConstPtr CloudConstPtr;
+
+ /** \brief StereoGroundSegmentation is a demonstration application for using PCL's stereo tools and segmentation tools to detect smooth surfaces suitable for driving.
+ *
+ * \author Alex Trevor
+ */
+class HRCSSegmentation
+{
+ private:
+ boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+ boost::shared_ptr<pcl::visualization::ImageViewer> image_viewer;
+ pcl::PointCloud<PointT>::ConstPtr prev_cloud;
+ pcl::PointCloud<pcl::Normal>::ConstPtr prev_normal_cloud;
+ pcl::PointCloud<pcl::PointXYZ>::ConstPtr prev_ground_cloud;
+ pcl::PointCloud<PointT>::ConstPtr prev_ground_image;
+ pcl::PointCloud<PointT>::ConstPtr prev_label_image;
+ Eigen::Vector4f prev_ground_normal;
+ Eigen::Vector4f prev_ground_centroid;
+ boost::mutex cloud_mutex;
+
+ pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
+ pcl::GroundPlaneComparator<PointT, pcl::Normal>::Ptr road_comparator;
+ pcl::OrganizedConnectedComponentSegmentation<PointT, pcl::Label> road_segmentation;
+ std::vector<std::string> left_images;
+ std::vector<std::string> right_images;
+ int files_idx;
+ int images_idx;
+
+ pcl::AdaptiveCostSOStereoMatching stereo;
+ bool trigger;
+ bool continuous;
+ bool display_normals;
+ bool detect_obstacles;
+ int smooth_weak;
+ int smooth_strong;
+
+ public:
+ HRCSSegmentation (std::vector<std::string> left_images, std::vector<std::string> right_images) :
+ viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")),
+ image_viewer (new pcl::visualization::ImageViewer ("Image Viewer")),
+ prev_cloud (new pcl::PointCloud<PointT>),
+ prev_normal_cloud (new pcl::PointCloud<pcl::Normal>),
+ prev_ground_cloud (new pcl::PointCloud<pcl::PointXYZ>),
+ prev_ground_image (new pcl::PointCloud<PointT>),
+ prev_label_image (new pcl::PointCloud<PointT>),
+ road_comparator (new pcl::GroundPlaneComparator<PointT, pcl::Normal>),
+ road_segmentation (road_comparator)
+ {
+ trigger = true;
+ continuous = false;
+ display_normals = false;
+ detect_obstacles = false;
+
+ this->left_images = left_images;
+ this->right_images = right_images;
+ files_idx = 0;
+ images_idx = 0;
+
+ // Set up a 3D viewer
+ viewer->setBackgroundColor (0, 0, 0);
+ viewer->addCoordinateSystem (1.0, "global");
+ viewer->initCameraParameters ();
+ viewer->registerKeyboardCallback (&HRCSSegmentation::keyboardCallback, *this, 0);
+
+ // Set up the stereo matching
+ stereo.setMaxDisparity(60);
+ stereo.setXOffset(0);
+ stereo.setRadius(5);
+
+ smooth_weak = 20;
+ smooth_strong = 100;
+ stereo.setSmoothWeak(smooth_weak);
+ stereo.setSmoothStrong(smooth_strong);
+ stereo.setGammaC(25);
+ stereo.setGammaS(10);
+
+ stereo.setRatioFilter(20);
+ stereo.setPeakFilter(0);
+
+ stereo.setLeftRightCheck(true);
+ stereo.setLeftRightCheckThreshold(1);
+
+ stereo.setPreProcessing(true);
+
+ // Set up the normal estimation
+ ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
+ ne.setMaxDepthChangeFactor (0.03f);
+ ne.setNormalSmoothingSize (40.0f);//20.0f
+
+ // Set up the groundplane comparator
+ // If the camera was pointing straight out, the normal would be:
+ Eigen::Vector3f nominal_road_normal (0.0, -1.0, 0.0);
+ // Adjust for camera tilt:
+ Eigen::Vector3f tilt_road_normal = Eigen::AngleAxisf (pcl::deg2rad (5.0f), Eigen::Vector3f::UnitX ()) * nominal_road_normal;
+ road_comparator->setExpectedGroundNormal (tilt_road_normal);
+ road_comparator->setGroundAngularThreshold (pcl::deg2rad (10.0f));
+ road_comparator->setAngularThreshold (pcl::deg2rad (3.0f));
+ }
+
+ ~HRCSSegmentation ()
+ {
+ }
+
+ void
+ cloud_cb_ (const pcl::PointCloud<PointT>::ConstPtr& cloud)
+ {
+ if (!viewer->wasStopped ())
+ {
+ cloud_mutex.lock ();
+ prev_cloud = cloud;
+ cloud_mutex.unlock ();
+ }
+ }
+
+ void
+ keyboardCallback (const pcl::visualization::KeyboardEvent& event, void*)
+ {
+ if (event.keyUp ())
+ {
+ switch (event.getKeyCode ())
+ {
+ case ' ':
+ trigger = true;
+ break;
+ case '1':
+ smooth_strong -= 10;
+ PCL_INFO ("smooth_strong: %d\n", smooth_strong);
+ stereo.setSmoothStrong (smooth_strong);
+ break;
+ case '2':
+ smooth_strong += 10;
+ PCL_INFO ("smooth_strong: %d\n", smooth_strong);
+ stereo.setSmoothStrong (smooth_strong);
+ break;
+ case '3':
+ smooth_weak -= 10;
+ PCL_INFO ("smooth_weak: %d\n", smooth_weak);
+ stereo.setSmoothWeak (smooth_weak);
+ break;
+ case '4':
+ smooth_weak += 10;
+ PCL_INFO ("smooth_weak: %d\n", smooth_weak);
+ stereo.setSmoothWeak (smooth_weak);
+ break;
+ case 'c':
+ continuous = !continuous;
+ break;
+ case 'n':
+ display_normals = !display_normals;
+ break;
+ case 'o':
+ detect_obstacles = !detect_obstacles;
+ break;
+ }
+ }
+ }
+
+
+ void
+ processStereoPair (const pcl::PointCloud<pcl::RGB>::Ptr& left_image, const pcl::PointCloud<pcl::RGB>::Ptr& right_image, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& out_cloud)
+ {
+ stereo.compute (*left_image, *right_image);
+ stereo.medianFilter (4);
+ stereo.getPointCloud(318.112200f, 224.334900f, 368.534700f, 0.8387445f, out_cloud, left_image);
+ }
+
+
+ void
+ processCloud (const pcl::PointCloud<PointT>::ConstPtr& cloud)
+ {
+ // Compute the normals
+ pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
+ ne.setInputCloud (cloud);
+ ne.compute (*normal_cloud);
+
+ // Set up the groundplane comparator
+ road_comparator->setInputCloud (cloud);
+ road_comparator->setInputNormals (normal_cloud);
+
+ // Run segmentation
+ pcl::PointCloud<pcl::Label> labels;
+ std::vector<pcl::PointIndices> region_indices;
+ road_segmentation.setInputCloud (cloud);
+ road_segmentation.segment (labels, region_indices);
+
+ // Draw the segmentation result
+ pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr ground_image (new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr label_image (new pcl::PointCloud<pcl::PointXYZRGB>);
+ *ground_image = *cloud;
+ *label_image = *cloud;
+
+ Eigen::Vector4f clust_centroid = Eigen::Vector4f::Zero ();
+ Eigen::Vector4f vp = Eigen::Vector4f::Zero ();
+ Eigen::Matrix3f clust_cov;
+ pcl::ModelCoefficients model;
+ model.values.resize (4);
+
+ std::vector<pcl::ModelCoefficients> model_coefficients;
+ std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
+ std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
+ std::vector<pcl::PointIndices> inlier_indices;
+
+ for (size_t i = 0; i < region_indices.size (); i++)
+ {
+ if (region_indices[i].indices.size () > 1000)
+ {
+
+ for (size_t j = 0; j < region_indices[i].indices.size (); j++)
+ {
+ pcl::PointXYZ ground_pt (cloud->points[region_indices[i].indices[j]].x,
+ cloud->points[region_indices[i].indices[j]].y,
+ cloud->points[region_indices[i].indices[j]].z);
+ ground_cloud->points.push_back (ground_pt);
+ ground_image->points[region_indices[i].indices[j]].g = static_cast<pcl::uint8_t> ((cloud->points[region_indices[i].indices[j]].g + 255) / 2);
+ label_image->points[region_indices[i].indices[j]].r = 0;
+ label_image->points[region_indices[i].indices[j]].g = 255;
+ label_image->points[region_indices[i].indices[j]].b = 0;
+ }
+
+ // Compute plane info
+ pcl::computeMeanAndCovarianceMatrix (*cloud, region_indices[i].indices, clust_cov, clust_centroid);
+ Eigen::Vector4f plane_params;
+
+ EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
+ EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
+ pcl::eigen33 (clust_cov, eigen_value, eigen_vector);
+ plane_params[0] = eigen_vector[0];
+ plane_params[1] = eigen_vector[1];
+ plane_params[2] = eigen_vector[2];
+ plane_params[3] = 0;
+ plane_params[3] = -1 * plane_params.dot (clust_centroid);
+
+ vp -= clust_centroid;
+ float cos_theta = vp.dot (plane_params);
+ if (cos_theta < 0)
+ {
+ plane_params *= -1;
+ plane_params[3] = 0;
+ plane_params[3] = -1 * plane_params.dot (clust_centroid);
+ }
+
+ model.values[0] = plane_params[0];
+ model.values[1] = plane_params[1];
+ model.values[2] = plane_params[2];
+ model.values[3] = plane_params[3];
+ model_coefficients.push_back (model);
+ inlier_indices.push_back (region_indices[i]);
+ centroids.push_back (clust_centroid);
+ covariances.push_back (clust_cov);
+ }
+ }
+
+ //Refinement
+ std::vector<bool> grow_labels;
+ std::vector<int> label_to_model;
+ grow_labels.resize (region_indices.size (), false);
+ label_to_model.resize (region_indices.size (), 0);
+
+ for (size_t i = 0; i < model_coefficients.size (); i++)
+ {
+ int model_label = (labels)[inlier_indices[i].indices[0]].label;
+ label_to_model[model_label] = static_cast<int> (i);
+ grow_labels[model_label] = true;
+ }
+
+ boost::shared_ptr<pcl::PointCloud<pcl::Label> > labels_ptr (new pcl::PointCloud<pcl::Label>());
+ *labels_ptr = labels;
+ pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
+ pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr refinement_compare (new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>());
+ refinement_compare->setInputCloud (cloud);
+ refinement_compare->setDistanceThreshold (0.15f);
+ refinement_compare->setLabels (labels_ptr);
+ refinement_compare->setModelCoefficients (model_coefficients);
+ refinement_compare->setRefineLabels (grow_labels);
+ refinement_compare->setLabelToModel (label_to_model);
+ mps.setRefinementComparator (refinement_compare);
+ mps.setMinInliers (500);
+ mps.setAngularThreshold (pcl::deg2rad (3.0));
+ mps.setDistanceThreshold (0.02);
+ mps.setInputCloud (cloud);
+ mps.setInputNormals (normal_cloud);
+ mps.refine (model_coefficients,
+ inlier_indices,
+ centroids,
+ covariances,
+ labels_ptr,
+ region_indices);
+
+ //Note the regions that have been extended
+ pcl::PointCloud<PointT> extended_ground_cloud;
+ for (size_t i = 0; i < region_indices.size (); i++)
+ {
+ if (region_indices[i].indices.size () > 1000)
+ {
+ for (size_t j = 0; j < region_indices[i].indices.size (); j++)
+ {
+ // Check to see if it has already been labeled
+ if (ground_image->points[region_indices[i].indices[j]].g == ground_image->points[region_indices[i].indices[j]].b)
+ {
+ pcl::PointXYZ ground_pt (cloud->points[region_indices[i].indices[j]].x,
+ cloud->points[region_indices[i].indices[j]].y,
+ cloud->points[region_indices[i].indices[j]].z);
+ ground_cloud->points.push_back (ground_pt);
+ ground_image->points[region_indices[i].indices[j]].r = static_cast<pcl::uint8_t> ((cloud->points[region_indices[i].indices[j]].r + 255) / 2);
+ ground_image->points[region_indices[i].indices[j]].g = static_cast<pcl::uint8_t> ((cloud->points[region_indices[i].indices[j]].g + 255) / 2);
+ label_image->points[region_indices[i].indices[j]].r = 128;
+ label_image->points[region_indices[i].indices[j]].g = 128;
+ label_image->points[region_indices[i].indices[j]].b = 0;
+ }
+
+ }
+ }
+ }
+
+ // Segment Obstacles (Disabled by default)
+ Eigen::Vector4f ground_plane_params (1.0, 0.0, 0.0, 1.0);
+ Eigen::Vector4f ground_centroid (0.0, 0.0, 0.0, 0.0);
+
+ if (ground_cloud->points.size () > 0)
+ {
+ ground_centroid = centroids[0];
+ ground_plane_params = Eigen::Vector4f (model_coefficients[0].values[0], model_coefficients[0].values[1], model_coefficients[0].values[2], model_coefficients[0].values[3]);
+ }
+
+ if (detect_obstacles)
+ {
+ pcl::PointCloud<PointT>::CloudVectorType clusters;
+ if (ground_cloud->points.size () > 0)
+ {
+ std::vector<bool> plane_labels;
+ plane_labels.resize (region_indices.size (), false);
+ for (size_t i = 0; i < region_indices.size (); i++)
+ {
+ if (region_indices[i].indices.size () > mps.getMinInliers ())
+ {
+ plane_labels[i] = true;
+ }
+ }
+
+ pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr euclidean_cluster_comparator_ (new pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label> ());
+
+ euclidean_cluster_comparator_->setInputCloud (cloud);
+ euclidean_cluster_comparator_->setLabels (labels_ptr);
+ euclidean_cluster_comparator_->setExcludeLabels (plane_labels);
+ euclidean_cluster_comparator_->setDistanceThreshold (0.05f, false);
+
+ pcl::PointCloud<pcl::Label> euclidean_labels;
+ std::vector<pcl::PointIndices> euclidean_label_indices;
+ pcl::OrganizedConnectedComponentSegmentation<PointT,pcl::Label> euclidean_segmentation (euclidean_cluster_comparator_);
+ euclidean_segmentation.setInputCloud (cloud);
+ euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
+
+ for (size_t i = 0; i < euclidean_label_indices.size (); i++)
+ {
+ if ((euclidean_label_indices[i].indices.size () > 200))
+ {
+ pcl::PointCloud<PointT> cluster;
+ pcl::copyPointCloud (*cloud, euclidean_label_indices[i].indices, cluster);
+ clusters.push_back (cluster);
+
+ Eigen::Vector4f cluster_centroid;
+ Eigen::Matrix3f cluster_cov;
+ pcl::computeMeanAndCovarianceMatrix (*cloud, euclidean_label_indices[i].indices, cluster_cov, cluster_centroid);
+
+ pcl::PointXYZ centroid_pt (cluster_centroid[0], cluster_centroid[1], cluster_centroid[2]);
+ double ptp_dist = pcl::pointToPlaneDistanceSigned (centroid_pt, ground_plane_params[0], ground_plane_params[1], ground_plane_params[2], ground_plane_params[3]);
+
+ if ((ptp_dist > 0.5) && (ptp_dist < 3.0))
+ {
+
+ for (size_t j = 0; j < euclidean_label_indices[i].indices.size (); j++)
+ {
+ ground_image->points[euclidean_label_indices[i].indices[j]].r = 255;
+ label_image->points[euclidean_label_indices[i].indices[j]].r = 255;
+ label_image->points[euclidean_label_indices[i].indices[j]].g = 0;
+ label_image->points[euclidean_label_indices[i].indices[j]].b = 0;
+ }
+
+ }
+
+ }
+ }
+
+ }
+ }
+
+ // note the NAN points in the image as well
+ for (size_t i = 0; i < cloud->points.size (); i++)
+ {
+ if (!pcl::isFinite (cloud->points[i]))
+ {
+ ground_image->points[i].b = static_cast<pcl::uint8_t>((cloud->points[i].b + 255) / 2);
+ label_image->points[i].r = 0;
+ label_image->points[i].g = 0;
+ label_image->points[i].b = 255;
+ }
+ }
+
+ // Update info for the visualization thread
+ {
+ cloud_mutex.lock ();
+ prev_cloud = cloud;
+ prev_normal_cloud = normal_cloud;
+ prev_ground_cloud = ground_cloud;
+ prev_ground_image = ground_image;
+ prev_label_image = label_image;
+ prev_ground_normal = ground_plane_params;
+ prev_ground_centroid = ground_centroid;
+ cloud_mutex.unlock ();
+ }
+ }
+
+ void
+ run ()
+ {
+ while (!viewer->wasStopped ())
+ {
+ // Process a new image
+ if (trigger || continuous)
+ {
+ pcl::PointCloud<pcl::RGB>::Ptr left_cloud (new pcl::PointCloud<pcl::RGB>);
+ pcl::PointCloud<pcl::RGB>::Ptr right_cloud (new pcl::PointCloud<pcl::RGB>);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+
+ pcl::PCDReader pcd;
+ pcd.read (left_images[images_idx], *left_cloud);
+ pcd.read (right_images[images_idx], *right_cloud);
+ processStereoPair (left_cloud, right_cloud, out_cloud);
+ processCloud (out_cloud);
+ images_idx++;
+
+ trigger = false;
+ }
+
+ // Draw visualizations
+ if (cloud_mutex.try_lock ())
+ {
+ if (!viewer->updatePointCloud (prev_ground_image, "cloud"))
+ viewer->addPointCloud (prev_ground_image, "cloud");
+
+ if (prev_normal_cloud->points.size () > 1000 && display_normals)
+ {
+ viewer->removePointCloud ("normals");
+ viewer->addPointCloudNormals<PointT, pcl::Normal>(prev_ground_image, prev_normal_cloud, 10, 0.15f, "normals");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "normals");
+ }
+
+ if (prev_cloud->points.size () > 1000)
+ {
+ image_viewer->addRGBImage<PointT>(prev_ground_image, "rgb_image", 0.3);
+ }
+
+ // Show the groundplane normal
+ Eigen::Vector3f nominal_road_normal (0.0, -1.0, 0.0);
+ // Adjust for camera tilt
+ Eigen::Vector3f tilt_road_normal = Eigen::AngleAxisf (pcl::deg2rad (5.0f), Eigen::Vector3f::UnitX ()) * nominal_road_normal;
+
+ // Show the groundplane normal
+ pcl::PointXYZ np1 (prev_ground_centroid[0], prev_ground_centroid[1], prev_ground_centroid[2]);
+ pcl::PointXYZ np2 (prev_ground_centroid[0] + prev_ground_normal[0],
+ prev_ground_centroid[1] + prev_ground_normal[1],
+ prev_ground_centroid[2] + prev_ground_normal[2]);
+ pcl::PointXYZ np3 (prev_ground_centroid[0] + tilt_road_normal[0],
+ prev_ground_centroid[1] + tilt_road_normal[1],
+ prev_ground_centroid[2] + tilt_road_normal[2]);
+
+ viewer->removeShape ("ground_norm");
+ viewer->addArrow (np2, np1, 1.0, 0, 0, false, "ground_norm");
+ viewer->removeShape ("expected_ground_norm");
+ viewer->addArrow (np3, np1, 0.0, 1.0, 0, false, "expected_ground_norm");
+
+ cloud_mutex.unlock ();
+ }
+ viewer->spinOnce (100);
+ image_viewer->spinOnce (100);
+
+ }
+ }
+
+
+
+};
+
+int
+main (int argc, char** argv)
+{
+ if (argc < 3)
+ {
+ PCL_INFO ("usage: pcl_stereo_ground_segmentation left_image_directory right_image_directory\n");
+ PCL_INFO ("note: images must be in PCD format. See pcl_png2pcd\n");
+ }
+
+ // Get list of stereo files
+ std::vector<std::string> left_images;
+ boost::filesystem::directory_iterator end_itr;
+ for (boost::filesystem::directory_iterator itr (argv[1]); itr != end_itr; ++itr)
+ {
+ left_images.push_back (itr->path ().string ());
+ }
+ sort (left_images.begin (), left_images.end ());
+ std::vector<std::string> right_images;
+ for (boost::filesystem::directory_iterator itr (argv[2]); itr != end_itr; ++itr)
+ {
+ right_images.push_back (itr->path ().string ());
+ }
+ sort (right_images.begin (), right_images.end ());
+
+ PCL_INFO ("Press space to advance to the next frame, or 'c' to enable continuous mode\n");
+
+ // Process and display
+ HRCSSegmentation hrcs (left_images, right_images);
+ hrcs.run ();
+
+ return 0;
+}
--- /dev/null
+#
+# Compute target flags macros by Anatoly Baksheev
+#
+# Usage in CmakeLists.txt:
+# include(CudaComputeTargetFlags.cmake)
+# APPEND_TARGET_ARCH_FLAGS()
+
+#compute flags macros
+MACRO(CUDA_COMPUTE_TARGET_FLAGS arch_bin arch_ptx cuda_nvcc_target_flags)
+ string(REGEX REPLACE "\\." "" ARCH_BIN_WITHOUT_DOTS "${${arch_bin}}")
+ string(REGEX REPLACE "\\." "" ARCH_PTX_WITHOUT_DOTS "${${arch_ptx}}")
+
+ set(cuda_computer_target_flags_temp "")
+
+ # Tell NVCC to add binaries for the specified GPUs
+ string(REGEX MATCHALL "[0-9()]+" ARCH_LIST "${ARCH_BIN_WITHOUT_DOTS}")
+ foreach(ARCH IN LISTS ARCH_LIST)
+ if (ARCH MATCHES "([0-9]+)\\(([0-9]+)\\)")
+ # User explicitly specified PTX for the concrete BIN
+ set(cuda_computer_target_flags_temp ${cuda_computer_target_flags_temp} -gencode arch=compute_${CMAKE_MATCH_2},code=sm_${CMAKE_MATCH_1})
+ else()
+ # User didn't explicitly specify PTX for the concrete BIN, we assume PTX=BIN
+ set(cuda_computer_target_flags_temp ${cuda_computer_target_flags_temp} -gencode arch=compute_${ARCH},code=sm_${ARCH})
+ endif()
+ endforeach()
+
+ # Tell NVCC to add PTX intermediate code for the specified architectures
+ string(REGEX MATCHALL "[0-9]+" ARCH_LIST "${ARCH_PTX_WITHOUT_DOTS}")
+ foreach(ARCH IN LISTS ARCH_LIST)
+ set(cuda_computer_target_flags_temp ${cuda_computer_target_flags_temp} -gencode arch=compute_${ARCH},code=compute_${ARCH})
+ endforeach()
+
+ set(${cuda_nvcc_target_flags} ${cuda_computer_target_flags_temp})
+ENDMACRO()
+
+MACRO(APPEND_TARGET_ARCH_FLAGS)
+ set(cuda_nvcc_target_flags "")
+ CUDA_COMPUTE_TARGET_FLAGS(CUDA_ARCH_BIN CUDA_ARCH_PTX cuda_nvcc_target_flags)
+ if (cuda_nvcc_target_flags)
+ message(STATUS "CUDA NVCC target flags: ${cuda_nvcc_target_flags}")
+ list(APPEND CUDA_NVCC_FLAGS ${cuda_nvcc_target_flags})
+ endif()
+ENDMACRO()
\ No newline at end of file
--- /dev/null
+###############################################################################
+# Find SoftKinetic DepthSense SDK
+#
+# find_package(DSSDK)
+#
+# Variables defined by this module:
+#
+# DSSDK_FOUND True if DepthSense SDK was found
+# DSSDK_VERSION The version of DepthSense SDK
+# DSSDK_INCLUDE_DIRS The location(s) of DepthSense SDK headers
+# DSSDK_LIBRARIES Libraries needed to use DepthSense SDK
+
+find_path(DSSDK_DIR include/DepthSenseVersion.hxx
+ HINTS "$ENV{DEPTHSENSESDK32}"
+ "$ENV{DEPTHSENSESDK64}"
+ PATHS "$ENV{PROGRAMFILES}/SoftKinetic/DepthSenseSDK"
+ "$ENV{PROGRAMW6432}/SoftKinetic/DepthSenseSDK"
+ "C:/Program Files (x86)/SoftKinetic/DepthSenseSDK"
+ "C:/Program Files/SoftKinetic/DepthSenseSDK"
+ "/opt/softkinetic/DepthSenseSDK"
+ DOC "DepthSense SDK directory")
+
+if(DSSDK_DIR)
+
+ # Include directories
+ set(DSSDK_INCLUDE_DIRS ${DSSDK_DIR}/include)
+ mark_as_advanced(DSSDK_INCLUDE_DIRS)
+
+ # Libraries
+ if(MSVC)
+ set(DSSDK_LIBRARIES_NAMES DepthSense)
+ else()
+ set(DSSDK_LIBRARIES_NAMES DepthSense DepthSensePlugins turbojpeg)
+ endif()
+ foreach(LIB ${DSSDK_LIBRARIES_NAMES})
+ find_library(DSSDK_LIBRARY_${LIB}
+ NAMES "${LIB}"
+ PATHS "${DSSDK_DIR}/lib/" NO_DEFAULT_PATH)
+ list(APPEND DSSDK_LIBRARIES ${DSSDK_LIBRARY_${LIB}})
+ mark_as_advanced(DSSDK_LIBRARY_${LIB})
+ endforeach()
+
+ # Version
+ set(DSSDK_VERSION 0)
+ file(STRINGS "${DSSDK_INCLUDE_DIRS}/DepthSenseVersion.hxx" _dsversion_H_CONTENTS REGEX "#define DEPTHSENSE_FILE_VERSION_STRING.*")
+ set(_DSSDK_VERSION_REGEX "([0-9]+\\.[0-9]+\\.[0-9]+\\.[0-9]+)")
+ if("${_dsversion_H_CONTENTS}" MATCHES ".*#define DEPTHSENSE_FILE_VERSION_STRING .*${_DSSDK_VERSION_REGEX}.*")
+ set(DSSDK_VERSION "${CMAKE_MATCH_1}")
+ endif()
+ unset(_dsversion_H_CONTENTS)
+
+endif()
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(DSSDK
+ FOUND_VAR DSSDK_FOUND
+ REQUIRED_VARS DSSDK_LIBRARIES DSSDK_INCLUDE_DIRS
+ VERSION_VAR DSSDK_VERSION
+)
--- /dev/null
+###############################################################################
+# - Try to find Ensenso SDK (IDS-Imaging)
+# Once done this will define
+# ENSENSO_FOUND - System has Ensenso SDK
+# ENSENSO_INCLUDE_DIRS - The Ensenso SDK include directories
+# ENSENSO_LIBRARIES - The libraries needed to use Ensenso SDK
+# ENSENSO_DEFINITIONS - Compiler switches required for using Ensenso SDK
+# -----------------------
+
+find_path(ENSENSO_INCLUDE_DIR nxLib.h
+ HINTS ${ENSENSO_ABI_HINT}
+ /opt/ensenso/development/c
+ "$ENV{PROGRAMFILES}/Ensenso/development/c" "$ENV{PROGRAMW6432}/Ensenso/development/c"
+ PATH_SUFFIXES include/)
+
+find_library(ENSENSO_LIBRARY QUIET NAMES NxLib64 NxLib32 nxLib64 nxLib32
+ HINTS ${ENSENSO_ABI_HINT}
+ "$ENV{PROGRAMFILES}/Ensenso/development/c" "$ENV{PROGRAMW6432}/Ensenso/development/c"
+ PATH_SUFFIXES lib/)
+
+set(ENSENSO_LIBRARIES ${ENSENSO_LIBRARY})
+set(ENSENSO_INCLUDE_DIRS ${ENSENSO_INCLUDE_DIR})
+
+include(FindPackageHandleStandardArgs)
+# handle the QUIETLY and REQUIRED arguments and set ENSENSO_FOUND to TRUE
+# if all listed variables are TRUE
+find_package_handle_standard_args(ensenso DEFAULT_MSG
+ ENSENSO_LIBRARY ENSENSO_INCLUDE_DIR)
+
+mark_as_advanced(ENSENSO_INCLUDE_DIR ENSENSO_LIBRARY)
+
+if(ENSENSO_FOUND)
+ message(STATUS "Ensenso SDK found")
+endif(ENSENSO_FOUND)
+
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(FZAPI DEFAULT_MSG
- FZAPI_LIBS FZAPI_INCLUDE_DIR)
-
\ No newline at end of file
+ FZAPI_LIBS FZAPI_INCLUDE_DIRS)
+
SET(GLEW_GLEW_LIBRARY "-framework GLEW" CACHE STRING "GLEW library for OSX")
else (${CMAKE_SYSTEM_VERSION} VERSION_LESS "13.0.0")
find_package(PkgConfig)
- pkg_check_modules(glew GLEW)
+ pkg_check_modules(GLEW glew)
SET(GLEW_GLEW_LIBRARY ${GLEW_LIBRARIES} CACHE STRING "GLEW library for OSX")
+ SET(GLEW_INCLUDE_DIR ${GLEW_INCLUDEDIR} CACHE STRING "GLEW include dir for OSX")
endif (${CMAKE_SYSTEM_VERSION} VERSION_LESS "13.0.0")
SET(GLEW_cocoa_LIBRARY "-framework Cocoa" CACHE STRING "Cocoa framework for OSX")
ELSE (APPLE)
# This sets the following variables:
# GTEST_FOUND - True if GTest was found.
# GTEST_INCLUDE_DIRS - Directories containing the GTest include files.
-# GTEST_SRC - Directories containing the GTest source files.
+# GTEST_SRC_DIR - Directories containing the GTest source files.
if(CMAKE_SYSTEM_NAME STREQUAL Linux)
set(CMAKE_INCLUDE_PATH ${CMAKE_INCLUDE_PATH} /usr /usr/local)
endif(NOT WIN32)
if(${CMAKE_VERSION} VERSION_LESS 2.8.2)
- pkg_check_modules(PC_OPENNI openni-dev)
+ pkg_check_modules(PC_OPENNI libopenni)
else()
- pkg_check_modules(PC_OPENNI QUIET openni-dev)
+ pkg_check_modules(PC_OPENNI QUIET libopenni)
endif()
set(OPENNI_DEFINITIONS ${PC_OPENNI_CFLAGS_OTHER})
endif(NOT WIN32)
if(${CMAKE_VERSION} VERSION_LESS 2.8.2)
- pkg_check_modules(PC_OPENNI2 openni2-dev)
+ pkg_check_modules(PC_OPENNI2 libopenni2)
else()
- pkg_check_modules(PC_OPENNI2 QUIET openni2-dev)
+ pkg_check_modules(PC_OPENNI2 QUIET libopenni2)
endif()
set(OPENNI2_DEFINITIONS ${PC_OPENNI_CFLAGS_OTHER})
+++ /dev/null
-###############################################################################
-# Find Intel Perceptual Computing SDK PXCAPI
-#
-# This sets the following variables:
-# PXCAPI_FOUND - True if PXCAPI was found.
-# PXCAPI_INCLUDE_DIRS - Directories containing the PXCAPI include files.
-# PXCAPI_LIBRARIES - Libraries needed to use PXCAPI.
-
-find_path(PXCAPI_DIR include/pxcimage.h
- PATHS "${PXCAPI_DIR}" "C:/Program Files/Intel/PCSDK" "C:/Program Files (x86)/Intel/PCSDK"
- DOC "PXCAPI include directories")
-
-if(PXCAPI_DIR)
- set(PXCAPI_INCLUDE_DIRS ${PXCAPI_DIR}/include ${PXCAPI_DIR}/sample/common/include)
-
- find_library(PXCAPI_LIB libpxc.lib
- PATHS "${PXCAPI_DIR}/lib/" NO_DEFAULT_PATH
- PATH_SUFFIXES x64 Win32)
- find_library(PXCAPI_SAMPLE_LIB libpxcutils.lib
- PATHS "${PXCAPI_DIR}/sample/common/lib" NO_DEFAULT_PATH
- PATH_SUFFIXES x64/v100 Win32/v100)
- set(PXCAPI_LIBS ${PXCAPI_LIB} ${PXCAPI_SAMPLE_LIB})
-endif()
-
-include(FindPackageHandleStandardArgs)
-find_package_handle_standard_args(PXCAPI DEFAULT_MSG
- PXCAPI_LIBS PXCAPI_INCLUDE_DIRS)
-
-mark_as_advanced(PXCAPI_LIB PXCAPI_SAMPLE_LIB)
-
-if(MSVC)
- set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} /NODEFAULTLIB:LIBCMT")
-endif()
set (VTK_USE_QVTK ON)
endif(NOT QVTK_FOUND)
else (${VTK_MAJOR_VERSION} VERSION_LESS "6.0")
- list (FIND VTK_MODULES_ENABLED vtkGUISupportQt GUI_SUPPORT_QT_FOUND)
- list (FIND VTK_MODULES_ENABLED vtkRenderingQt RENDERING_QT_FOUND)
- if (GUI_SUPPORT_QT_FOUND AND RENDERING_QT_FOUND)
+ if (";${VTK_MODULES_ENABLED};" MATCHES ";vtkGUISupportQt;" AND ";${VTK_MODULES_ENABLED};" MATCHES ";vtkRenderingQt;")
set (VTK_USE_QVTK ON)
set (QVTK_LIBRARY vtkRenderingQt vtkGUISupportQt)
- else (GUI_SUPPORT_QT_FOUND AND RENDERING_QT_FOUND)
+ else ()
unset(QVTK_FOUND)
- endif (GUI_SUPPORT_QT_FOUND AND RENDERING_QT_FOUND)
+ endif ()
endif (${VTK_MAJOR_VERSION} VERSION_LESS "6.0")
--- /dev/null
+###############################################################################
+# Find Intel RealSense SDK
+#
+# find_package(RSSDK)
+#
+# Variables defined by this module:
+#
+# RSSDK_FOUND True if RealSense SDK was found
+# RSSDK_VERSION The version of RealSense SDK
+# RSSDK_INCLUDE_DIRS The location(s) of RealSense SDK headers
+# RSSDK_LIBRARIES Libraries needed to use RealSense SDK
+
+find_path(RSSDK_DIR include/pxcversion.h
+ PATHS "$ENV{RSSDK_DIR}"
+ "$ENV{PROGRAMFILES}/Intel/RSSDK"
+ "$ENV{PROGRAMW6432}/Intel/RSSDK"
+ "C:/Program Files (x86)/Intel/RSSDK"
+ "C:/Program Files/Intel/RSSDK"
+ DOC "RealSense SDK directory")
+
+if(RSSDK_DIR)
+
+ # Include directories
+ set(RSSDK_INCLUDE_DIRS ${RSSDK_DIR}/include)
+ mark_as_advanced(RSSDK_INCLUDE_DIRS)
+
+ # Libraries
+ set(RSSDK_RELEASE_NAME libpxc.lib)
+ set(RSSDK_DEBUG_NAME libpxc_d.lib)
+ find_library(RSSDK_LIBRARY
+ NAMES ${RSSDK_RELEASE_NAME}
+ PATHS "${RSSDK_DIR}/lib/" NO_DEFAULT_PATH
+ PATH_SUFFIXES x64 Win32)
+ find_library(RSSDK_LIBRARY_DEBUG
+ NAMES ${RSSDK_DEBUG_NAME} ${RSSDK_RELEASE_NAME}
+ PATHS "${RSSDK_DIR}/lib/" NO_DEFAULT_PATH
+ PATH_SUFFIXES x64 Win32)
+ if(NOT RSSDK_LIBRARY_DEBUG)
+ set(RSSDK_LIBRARY_DEBUG ${RSSDK_LIBRARY})
+ endif()
+ set(RSSDK_LIBRARIES optimized ${RSSDK_LIBRARY} debug ${RSSDK_LIBRARY_DEBUG})
+ mark_as_advanced(RSSDK_LIBRARY RSSDK_LIBRARY_DEBUG)
+
+ # Version
+ set(RSSDK_VERSION 0)
+ file(STRINGS "${RSSDK_INCLUDE_DIRS}/pxcversion.h" _pxcversion_H_CONTENTS REGEX "#define PXC_VERSION_.*")
+ set(_RSSDK_VERSION_REGEX "([0-9]+)")
+ foreach(v MAJOR MINOR BUILD REVISION)
+ if("${_pxcversion_H_CONTENTS}" MATCHES ".*#define PXC_VERSION_${v} *${_RSSDK_VERSION_REGEX}.*")
+ set(RSSDK_VERSION_${v} "${CMAKE_MATCH_1}")
+ endif()
+ endforeach()
+ unset(_pxcversion_H_CONTENTS)
+ set(RSSDK_VERSION "${RSSDK_VERSION_MAJOR}.${RSSDK_VERSION_MINOR}.${RSSDK_VERSION_BUILD}.${RSSDK_VERSION_REVISION}")
+
+endif()
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(RSSDK
+ FOUND_VAR RSSDK_FOUND
+ REQUIRED_VARS RSSDK_LIBRARIES RSSDK_INCLUDE_DIRS
+ VERSION_VAR RSSDK_VERSION
+)
+
+if(MSVC)
+ set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} /NODEFAULTLIB:LIBCMTD")
+endif()
--- /dev/null
+###############################################################################
+# - Try to find davidSDK (David Vision Systems)
+# Once done this will define
+# DAVIDSDK_FOUND - System has davidSDK
+# DAVIDSDK_INCLUDE_DIRS - The davidSDK include directories
+# DAVIDSDK_LIBRARIES - The libraries needed to use davidSDK
+# DAVIDSDK_DEFINITIONS - Compiler switches required for using davidSDK
+# -----------------------
+
+find_path(DAVIDSDK_INCLUDE_DIR david.h
+ HINTS ${DAVIDSDK_ABI_HINT}
+ /usr/local/include/davidSDK
+ "$ENV{PROGRAMFILES}/davidSDK" "$ENV{PROGRAMW6432}/davidSDK"
+ PATH_SUFFIXES include/)
+
+find_library(DAVIDSDK_LIBRARY QUIET NAMES davidSDK
+ HINTS ${DAVIDSDK_ABI_HINT}
+ "$ENV{PROGRAMFILES}/davidSDK" "$ENV{PROGRAMW6432}/davidSDK"
+ PATH_SUFFIXES lib/)
+
+set(DAVIDSDK_LIBRARIES ${DAVIDSDK_LIBRARY})
+set(DAVIDSDK_INCLUDE_DIRS ${DAVIDSDK_INCLUDE_DIR})
+
+include(FindPackageHandleStandardArgs)
+# handle the QUIETLY and REQUIRED arguments and set DAVIDSDK_FOUND to TRUE
+# if all listed variables are TRUE
+find_package_handle_standard_args(davidSDK DEFAULT_MSG
+ DAVIDSDK_LIBRARY DAVIDSDK_INCLUDE_DIR)
+
+mark_as_advanced(DAVIDSDK_INCLUDE_DIR DAVIDSDK_LIBRARY)
+
+if(DAVIDSDK_FOUND)
+ message(STATUS "davidSDK found")
+endif(DAVIDSDK_FOUND)
+
set(CPACK_NSIS_DISPLAY_NAME "@PROJECT_NAME@-@PCL_VERSION@")
set(CPACK_NSIS_MUI_ICON "@PROJECT_SOURCE_DIR@/cmake/images/pcl.ico")
set(CPACK_NSIS_MUI_UNIICON "@PROJECT_SOURCE_DIR@/cmake/images/pcl.ico")
- set(CPACK_NSIS_HELP_LINK "http:\\\\\\\\www.pointclouds.org")
- set(CPACK_NSIS_URL_INFO_ABOUT "http:\\\\\\\\www.pointclouds.org")
+ set(CPACK_NSIS_HELP_LINK "http://www.pointclouds.org")
+ set(CPACK_NSIS_URL_INFO_ABOUT "http://www.pointclouds.org")
set(CPACK_NSIS_MODIFY_PATH ON)
set(CPACK_PACKAGE_EXECUTABLES @PCL_EXECUTABLES@)
set(CPACK_NSIS_MENU_LINKS
if(BUILD_all_in_one_installer)
get_filename_component(BOOST_ROOT "${Boost_INCLUDE_DIR}" PATH)
+ get_filename_component(BOOST_ROOT "${BOOST_ROOT}" PATH)
get_filename_component(EIGEN_ROOT "${EIGEN_INCLUDE_DIRS}" PATH)
get_filename_component(QHULL_ROOT "${QHULL_INCLUDE_DIRS}" PATH)
get_filename_component(FLANN_ROOT "${FLANN_INCLUDE_DIRS}" PATH)
get_filename_component(VTK_ROOT "${VTK_DIR}" PATH)
get_filename_component(VTK_ROOT "${VTK_ROOT}" PATH)
+ if(${VTK_MAJOR_VERSION} GREATER 5)
+ get_filename_component(VTK_ROOT "${VTK_ROOT}" PATH)
+ endif()
set(PCL_3RDPARTY_COMPONENTS)
foreach(dep Eigen Boost Qhull FLANN VTK)
string(TOUPPER ${dep} DEP)
list(APPEND PCL_3RDPARTY_COMPONENTS ${dep})
endforeach(dep)
- if(CMAKE_CL_64)
- set(OPENNI_PACKAGE "OpenNI-Win64-1.5.4-Dev.msi")
- set(OPENNI_URL "http://www.pointclouds.org/assets/files/dependencies/${OPENNI_PACKAGE}")
- set(OPENNI_MD5 c8f9cbe8447a16d32572a4e2c2d00af0)
- set(OPENNI_SENSOR_PACKAGE "Sensor-Win-OpenSource64-5.1.0.msi")
- set(OPENNI_SENSOR_URL "http://www.pointclouds.org/assets/files/dependencies/${OPENNI_SENSOR_PACKAGE}")
- set(OPENNI_SENSOR_MD5 badb880116436870943b1b7c447dfa22)
- else(CMAKE_CL_64)
- set(OPENNI_PACKAGE "OpenNI-Win32-1.5.4-Dev.msi")
- set(OPENNI_URL "http://www.pointclouds.org/assets/files/dependencies/${OPENNI_PACKAGE}")
- set(OPENNI_MD5 0b7118a0581abef411b58530d4039cf0)
- set(OPENNI_SENSOR_PACKAGE "Sensor-Win-OpenSource32-5.1.0.msi")
- set(OPENNI_SENSOR_URL "http://www.pointclouds.org/assets/files/dependencies/${OPENNI_SENSOR_PACKAGE}")
- set(OPENNI_SENSOR_MD5 8bf14b2e813859f868fc316acb2d08fa)
- endif(CMAKE_CL_64)
+ if(WITH_OPENNI)
+ if(CMAKE_CL_64)
+ set(OPENNI_PACKAGE "OpenNI-Win64-1.5.4-Dev.msi")
+ set(OPENNI_URL "http://sourceforge.net/projects/pointclouds/files/dependencies/${OPENNI_PACKAGE}")
+ set(OPENNI_MD5 c8f9cbe8447a16d32572a4e2c2d00af0)
+ set(OPENNI_SENSOR_PACKAGE "Sensor-Win-OpenSource64-5.1.0.msi")
+ set(OPENNI_SENSOR_URL "http://sourceforge.net/projects/pointclouds/files/dependencies/${OPENNI_SENSOR_PACKAGE}")
+ set(OPENNI_SENSOR_MD5 badb880116436870943b1b7c447dfa22)
+ else(CMAKE_CL_64)
+ set(OPENNI_PACKAGE "OpenNI-Win32-1.5.4-Dev.msi")
+ set(OPENNI_URL "http://sourceforge.net/projects/pointclouds/files/dependencies/${OPENNI_PACKAGE}")
+ set(OPENNI_MD5 996d48f447b41a5501b7d22af27ab251)
+ set(OPENNI_SENSOR_PACKAGE "Sensor-Win-OpenSource32-5.1.0.msi")
+ set(OPENNI_SENSOR_URL "http://sourceforge.net/projects/pointclouds/files/dependencies/${OPENNI_SENSOR_PACKAGE}")
+ set(OPENNI_SENSOR_MD5 55da1f7541d7c9c98772bddf801c7e1c)
+ endif(CMAKE_CL_64)
- set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS " IntCmp $OpenNI_selected 0 noinstall_openni_packages\n")
+ set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS " IntCmp $OpenNI_selected 0 noinstall_openni_packages\n")
- file(DOWNLOAD ${OPENNI_URL} "${CMAKE_CURRENT_BINARY_DIR}/${OPENNI_PACKAGE}"
- STATUS _openni_download_status LOG _openni_download_log
- EXPECTED_MD5 ${OPENNI_MD5}
- )
- list(GET _openni_download_status 0 _error_code)
- list(GET _openni_download_status 1 _error_message)
- if(_error_code EQUAL 0)
- install(
- FILES "${CMAKE_CURRENT_BINARY_DIR}/${OPENNI_PACKAGE}"
- DESTINATION 3rdParty/OpenNI
- COMPONENT OpenNI
- )
- list(APPEND PCL_3RDPARTY_COMPONENTS OpenNI)
- set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS
- "${CPACK_NSIS_EXTRA_INSTALL_COMMANDS}\n ExecWait 'msiexec /i \\\"$INSTDIR\\\\3rdParty\\\\OpenNI\\\\${OPENNI_PACKAGE}\\\" '")
- else(_error_code EQUAL 0)
- message("WARNING : Could not download ${OPENNI_URL}, error code : ${_error_code}, error message : ${_error_message}")
- endif(_error_code EQUAL 0)
+ file(DOWNLOAD ${OPENNI_URL} "${CMAKE_CURRENT_BINARY_DIR}/${OPENNI_PACKAGE}"
+ STATUS _openni_download_status LOG _openni_download_log
+ EXPECTED_MD5 ${OPENNI_MD5}
+ )
+ list(GET _openni_download_status 0 _error_code)
+ list(GET _openni_download_status 1 _error_message)
+ if(_error_code EQUAL 0)
+ install(
+ FILES "${CMAKE_CURRENT_BINARY_DIR}/${OPENNI_PACKAGE}"
+ DESTINATION 3rdParty/OpenNI
+ COMPONENT OpenNI
+ )
+ list(APPEND PCL_3RDPARTY_COMPONENTS OpenNI)
+ set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS
+ "${CPACK_NSIS_EXTRA_INSTALL_COMMANDS}\n ExecWait 'msiexec /i \\\"$INSTDIR\\\\3rdParty\\\\OpenNI\\\\${OPENNI_PACKAGE}\\\" '")
+ else(_error_code EQUAL 0)
+ message("WARNING : Could not download ${OPENNI_URL}, error code : ${_error_code}, error message : ${_error_message}")
+ endif(_error_code EQUAL 0)
- file(DOWNLOAD ${OPENNI_SENSOR_URL} "${CMAKE_CURRENT_BINARY_DIR}/${OPENNI_SENSOR_PACKAGE}"
- STATUS _openni_download_status LOG _openni_download_log
- EXPECTED_MD5 ${OPENNI_SENSOR_MD5}
- )
- list(GET _openni_download_status 0 _error_code)
- list(GET _openni_download_status 1 _error_message)
- if(_error_code EQUAL 0)
- install(
- FILES "${CMAKE_CURRENT_BINARY_DIR}/${OPENNI_SENSOR_PACKAGE}"
- DESTINATION 3rdParty/OpenNI
- COMPONENT OpenNI
- )
- list(APPEND PCL_3RDPARTY_COMPONENTS OpenNI)
- set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS
- "${CPACK_NSIS_EXTRA_INSTALL_COMMANDS}\n ExecWait 'msiexec /i \\\"$INSTDIR\\\\3rdParty\\\\OpenNI\\\\${OPENNI_SENSOR_PACKAGE}\\\" '")
- else(_error_code EQUAL 0)
- message("WARNING : Could not download ${OPENNI_SENSOR_URL}, error code : ${_error_code}, error message : ${_error_message}")
- endif(_error_code EQUAL 0)
- list(REMOVE_DUPLICATES PCL_3RDPARTY_COMPONENTS)
- set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS "${CPACK_NSIS_EXTRA_INSTALL_COMMANDS}\n noinstall_openni_packages:\n")
+ file(DOWNLOAD ${OPENNI_SENSOR_URL} "${CMAKE_CURRENT_BINARY_DIR}/${OPENNI_SENSOR_PACKAGE}"
+ STATUS _openni_download_status LOG _openni_download_log
+ EXPECTED_MD5 ${OPENNI_SENSOR_MD5}
+ )
+ list(GET _openni_download_status 0 _error_code)
+ list(GET _openni_download_status 1 _error_message)
+ if(_error_code EQUAL 0)
+ install(
+ FILES "${CMAKE_CURRENT_BINARY_DIR}/${OPENNI_SENSOR_PACKAGE}"
+ DESTINATION 3rdParty/OpenNI
+ COMPONENT OpenNI
+ )
+ list(APPEND PCL_3RDPARTY_COMPONENTS OpenNI)
+ set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS
+ "${CPACK_NSIS_EXTRA_INSTALL_COMMANDS}\n ExecWait 'msiexec /i \\\"$INSTDIR\\\\3rdParty\\\\OpenNI\\\\${OPENNI_SENSOR_PACKAGE}\\\" '")
+ else(_error_code EQUAL 0)
+ message("WARNING : Could not download ${OPENNI_SENSOR_URL}, error code : ${_error_code}, error message : ${_error_message}")
+ endif(_error_code EQUAL 0)
+ list(REMOVE_DUPLICATES PCL_3RDPARTY_COMPONENTS)
+ set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS "${CPACK_NSIS_EXTRA_INSTALL_COMMANDS}\n noinstall_openni_packages:\n")
+ endif(WITH_OPENNI)
+
+ if(WITH_OPENNI2)
+ if(CMAKE_CL_64)
+ set(OPENNI2_PACKAGE "OpenNI-Windows-x64-2.2.msi")
+ set(OPENNI2_ZIP "OpenNI-Windows-x64-2.2.0.33.zip")
+ set(OPENNI2_URL "http://com.occipital.openni.s3.amazonaws.com/${OPENNI2_ZIP}")
+ set(OPENNI2_MD5 d187f1dd0b091e27cebd03216b1bfff5)
+ else(CMAKE_CL_64)
+ set(OPENNI2_PACKAGE "OpenNI-Windows-x86-2.2.msi")
+ set(OPENNI2_ZIP "OpenNI-Windows-x86-2.2.0.33.zip")
+ set(OPENNI2_URL "http://com.occipital.openni.s3.amazonaws.com/${OPENNI2_ZIP}")
+ set(OPENNI2_MD5 59b38e23d951d59917a35f7f89efaf22)
+ endif(CMAKE_CL_64)
+
+ set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS " IntCmp $OpenNI2_selected 0 noinstall_openni2_packages\n")
+
+ file(DOWNLOAD ${OPENNI2_URL} "${CMAKE_CURRENT_BINARY_DIR}/${OPENNI2_ZIP}"
+ STATUS _openni2_download_status LOG _openni2_download_log
+ EXPECTED_MD5 ${OPENNI2_MD5}
+ )
+ list(GET _openni2_download_status 0 _error_code)
+ list(GET _openni2_download_status 1 _error_message)
+ if(_error_code EQUAL 0)
+ execute_process(
+ COMMAND ${CMAKE_COMMAND} -E tar -xzf ${OPENNI2_ZIP}
+ WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+ RESULT_VARIABLE _error_code
+ ERROR_VARIABLE _error_message
+ )
+ if(_error_code EQUAL 0)
+ install(
+ FILES "${CMAKE_CURRENT_BINARY_DIR}/${OPENNI2_PACKAGE}"
+ DESTINATION 3rdParty/OpenNI2
+ COMPONENT OpenNI2
+ )
+ list(APPEND PCL_3RDPARTY_COMPONENTS OpenNI2)
+ set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS
+ "${CPACK_NSIS_EXTRA_INSTALL_COMMANDS}\n ExecWait 'msiexec /i \\\"$INSTDIR\\\\3rdParty\\\\OpenNI2\\\\${OPENNI2_PACKAGE}\\\" '")
+ else(_error_code EQUAL 0)
+ message("WARNING : Could not unzip ${OPENNI2_ZIP}, error code : ${_error_code}, error message : ${_error_message}")
+ endif(_error_code EQUAL 0)
+ else(_error_code EQUAL 0)
+ message("WARNING : Could not download ${OPENNI2_ZIP_URL}, error code : ${_error_code}, error message : ${_error_message}")
+ endif(_error_code EQUAL 0)
+ list(REMOVE_DUPLICATES PCL_3RDPARTY_COMPONENTS)
+ set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS "${CPACK_NSIS_EXTRA_INSTALL_COMMANDS}\n noinstall_openni2_packages:\n")
+ endif(WITH_OPENNI2)
endif(BUILD_all_in_one_installer)
endif(BUILD_all_in_one_installer)
if(MSVC10)
set(CPACK_NSIS_PACKAGE_NAME "${CPACK_NSIS_PACKAGE_NAME}-msvc2010-${win_system_name}")
- else(MSVC10)
+ elseif(MSVC11)
+ set(CPACK_NSIS_PACKAGE_NAME "${CPACK_NSIS_PACKAGE_NAME}-msvc2012-${win_system_name}")
+ elseif(MSVC12)
+ set(CPACK_NSIS_PACKAGE_NAME "${CPACK_NSIS_PACKAGE_NAME}-msvc2013-${win_system_name}")
+ elseif(MSVC14)
+ set(CPACK_NSIS_PACKAGE_NAME "${CPACK_NSIS_PACKAGE_NAME}-msvc2015-${win_system_name}")
+ else()
set(CPACK_NSIS_PACKAGE_NAME "${CPACK_NSIS_PACKAGE_NAME}-${win_system_name}")
- endif(MSVC10)
+ endif()
set(CPACK_PACKAGE_FILE_NAME ${CPACK_NSIS_PACKAGE_NAME})
# force CPACK_PACKAGE_INSTALL_REGISTRY_KEY because of a known limitation in cmake/cpack to be fixed in next releases
# http://public.kitware.com/Bug/view.php?id=9094
set(Boost_NO_BOOST_CMAKE ON)
# Optional boost modules
-find_package(Boost 1.47.0 QUIET COMPONENTS serialization mpi)
+find_package(Boost 1.40.0 QUIET COMPONENTS serialization mpi)
if(Boost_SERIALIZATION_FOUND)
set(BOOST_SERIALIZATION_FOUND TRUE)
endif(Boost_SERIALIZATION_FOUND)
# Required boost modules
+if(WITH_OPENNI2)
set(BOOST_REQUIRED_MODULES system filesystem thread date_time iostreams chrono)
-
find_package(Boost 1.47.0 REQUIRED COMPONENTS ${BOOST_REQUIRED_MODULES})
+else()
+set(BOOST_REQUIRED_MODULES system filesystem thread date_time iostreams)
+find_package(Boost 1.40.0 REQUIRED COMPONENTS ${BOOST_REQUIRED_MODULES})
+endif()
if(Boost_FOUND)
set(BOOST_FOUND TRUE)
--- /dev/null
+# Find CUDA
+
+
+
+if(MSVC)
+ # Setting this to true brakes Visual Studio builds.
+ set(CUDA_ATTACH_VS_BUILD_RULE_TO_CUDA_FILE OFF CACHE BOOL "CUDA_ATTACH_VS_BUILD_RULE_TO_CUDA_FILE")
+endif()
+
+set(CUDA_FIND_QUIETLY TRUE)
+find_package(CUDA 4)
+
+if(CUDA_FOUND)
+ message(STATUS "Found CUDA Toolkit v${CUDA_VERSION_STRING}")
+
+ if(${CUDA_VERSION_STRING} VERSION_LESS "7.5")
+ # Recent versions of cmake set CUDA_HOST_COMPILER to CMAKE_C_COMPILER which
+ # on OSX defaults to clang (/usr/bin/cc), but this is not a supported cuda
+ # compiler. So, here we will preemptively set CUDA_HOST_COMPILER to gcc if
+ # that compiler exists in /usr/bin. This will not override an existing cache
+ # value if the user has passed CUDA_HOST_COMPILER on the command line.
+ if (NOT DEFINED CUDA_HOST_COMPILER AND CMAKE_C_COMPILER_ID STREQUAL "Clang" AND EXISTS /usr/bin/gcc)
+ set(CUDA_HOST_COMPILER /usr/bin/gcc CACHE FILEPATH "Host side compiler used by NVCC")
+ message(STATUS "Setting CMAKE_HOST_COMPILER to /usr/bin/gcc instead of ${CMAKE_C_COMPILER}. See http://dev.pointclouds.org/issues/979")
+ endif()
+
+ # Send a warning if CUDA_HOST_COMPILER is set to a compiler that is known
+ # to be unsupported.
+ if (CUDA_HOST_COMPILER STREQUAL CMAKE_C_COMPILER AND CMAKE_C_COMPILER_ID STREQUAL "Clang")
+ message(WARNING "CUDA_HOST_COMPILER is set to an unsupported compiler: ${CMAKE_C_COMPILER}. See http://dev.pointclouds.org/issues/979")
+ endif()
+ endif()
+
+ # CUDA_ARCH_BIN is a space separated list of versions to include in output so-file. So you can set CUDA_ARCH_BIN = 10 11 12 13 20
+ # Also user can specify virtual arch in parenthesis to limit instructions set,
+ # for example CUDA_ARCH_BIN = 11(11) 12(11) 13(11) 20(11) 21(11) -> forces using only sm_11 instructions.
+ # The CMake scripts interpret XX as XX (XX). This allows user to omit parenthesis.
+ # Arch 21 is an exceptional case since it doesn't have own sm_21 instructions set.
+ # So 21 = 21(21) is an invalid configuration and user has to explicitly force previous sm_20 instruction set via 21(20).
+ # CUDA_ARCH_BIN adds support of only listed GPUs. As alternative CMake scripts also parse 'CUDA_ARCH_PTX' variable,
+ # which is a list of intermediate PTX codes to include in final so-file. The PTX code can/will be JIT compiled for any current or future GPU.
+ # To add support of older GPU for kinfu, I would embed PTX 11 and 12 into so-file. GPU with sm_13 will run PTX 12 code (no difference for kinfu)
+
+ # Find a complete list for CUDA compute capabilities at http://developer.nvidia.com/cuda-gpus
+
+ if(NOT ${CUDA_VERSION_STRING} VERSION_LESS "6.5")
+ set(__cuda_arch_bin "2.0 2.1(2.0) 3.0 3.5 5.0 5.2")
+ elseif(NOT ${CUDA_VERSION_STRING} VERSION_LESS "6.0")
+ set(__cuda_arch_bin "2.0 2.1(2.0) 3.0 3.5 5.0")
+ elseif(NOT ${CUDA_VERSION_STRING} VERSION_LESS "5.0")
+ set(__cuda_arch_bin "2.0 2.1(2.0) 3.0 3.5")
+ elseif(${CUDA_VERSION_STRING} VERSION_GREATER "4.1")
+ set(__cuda_arch_bin "2.0 2.1(2.0) 3.0")
+ else()
+ set(__cuda_arch_bin "2.0 2.1(2.0)")
+ endif()
+
+ set(CUDA_ARCH_BIN ${__cuda_arch_bin} CACHE STRING "Specify 'real' GPU architectures to build binaries for, BIN(PTX) format is supported")
+
+ set(CUDA_ARCH_PTX "" CACHE STRING "Specify 'virtual' PTX arch to build PTX intermediate code for. Example: 1.0 1.2 or 10 12")
+ #set(CUDA_ARCH_PTX "1.1 1.2" CACHE STRING "Specify 'virtual' PTX arch to build PTX intermediate code for. Example: 1.0 1.2 or 10 12")
+
+ # Guess this macros will be included in cmake distributive
+ include(${PCL_SOURCE_DIR}/cmake/CudaComputeTargetFlags.cmake)
+ APPEND_TARGET_ARCH_FLAGS()
+
+endif()
# set(CMAKE_AUTOMOC ON)
# set(CMAKE_INCLUDE_CURRENT_DIR ON)
+ set(QT_QTOPENGL_INCLUDE_DIR Qt5OpenGL_INCLUDE_DIRS)
+
# Replace qt4 macros.
macro(qt4_wrap_cpp)
qt5_wrap_cpp(${ARGN})
endmacro()
endif()
- set(QT_USE_FILE ${CMAKE_CURRENT_BINARY_DIR}/use-qt5.cmake CACHE PATH "")
+ set(QT_USE_FILE ${CMAKE_CURRENT_BINARY_DIR}/use-qt5.cmake CACHE PATH "" FORCE)
file(WRITE ${QT_USE_FILE} "#")
# Trick the remainder of the build system.
set(QT4_FOUND TRUE)
-endif()
\ No newline at end of file
+endif()
# Check for the presence of SSE and figure out the flags to use for it.
macro(PCL_CHECK_FOR_SSE)
set(SSE_FLAGS)
+ set(SSE_DEFINITIONS)
# Test CLANG
#if(CMAKE_COMPILER_IS_CLANG)
}"
HAVE_SSE4_1_EXTENSIONS)
+ if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
+ set(CMAKE_REQUIRED_FLAGS "-mssse3")
+ endif(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
+
+ check_cxx_source_runs("
+ #include <tmmintrin.h>
+ int main ()
+ {
+ __m128i a, b;
+ int vals[4] = {-1, -2, -3, -4};
+ a = _mm_loadu_si128 ((const __m128i*)vals);
+ b = _mm_abs_epi32 (a);
+ _mm_storeu_si128 ((__m128i*)vals, b);
+ return (0);
+ }"
+ HAVE_SSSE3_EXTENSIONS)
+
if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
set(CMAKE_REQUIRED_FLAGS "-msse3")
endif(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
SET(SSE_FLAGS "${SSE_FLAGS} -msse4.2 -mfpmath=sse")
elseif(HAVE_SSE4_1_EXTENSIONS)
SET(SSE_FLAGS "${SSE_FLAGS} -msse4.1 -mfpmath=sse")
+ elseif(HAVE_SSSE3_EXTENSIONS)
+ SET(SSE_FLAGS "${SSE_FLAGS} -mssse3 -mfpmath=sse")
elseif(HAVE_SSE3_EXTENSIONS)
SET(SSE_FLAGS "${SSE_FLAGS} -msse3 -mfpmath=sse")
elseif(HAVE_SSE2_EXTENSIONS)
SET(SSE_FLAGS "${SSE_FLAGS} /arch:SSE")
endif(HAVE_SSE2_EXTENSIONS)
endif()
-endmacro(PCL_CHECK_FOR_SSE)
-###############################################################################
-# Check for the presence of SSE 4.1
-macro(PCL_CHECK_FOR_SSE4_1)
- include(CheckCXXSourceRuns)
- set(CMAKE_REQUIRED_FLAGS)
-
- if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
- set(CMAKE_REQUIRED_FLAGS "-msse4.1")
- endif(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
-
- check_cxx_source_runs("
- #include <smmintrin.h>
- int main()
- {
- __m128 a, b;
- float vals[4] = {1, 2, 3, 4};
- const int mask = 123;
- a = _mm_loadu_ps(vals);
- b = a;
- b = _mm_dp_ps (a, a, mask);
- _mm_storeu_ps(vals,b);
- return 0;
- }"
- HAVE_SSE4_1_EXTENSIONS "CHECK_FOR_SSE4_1")
-endmacro(PCL_CHECK_FOR_SSE4_1)
-
-###############################################################################
-# Check for the presence of SSE 3
-macro(PCL_CHECK_FOR_SSE3)
- include(CheckCXXSourceRuns)
- set(CMAKE_REQUIRED_FLAGS)
-
- if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
- set(CMAKE_REQUIRED_FLAGS "-msse3")
- endif(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG)
-
- check_cxx_source_runs("
- #include <pmmintrin.h>
- int main ()
- {
- __m128d a, b;
- double vals[2] = {0};
- a = _mm_loadu_pd (vals);
- b = _mm_hadd_pd (a,a);
- _mm_storeu_pd (vals, b);
- return (0);
- }"
- HAVE_SSE3_EXTENSIONS)
-endmacro(PCL_CHECK_FOR_SSE3)
+ if(MSVC)
+ if(HAVE_SSSE3_EXTENSIONS)
+ SET(SSE_DEFINITIONS "${SSE_DEFINITIONS} -D__SSSE3__")
+ endif()
+ if(HAVE_SSE2_EXTENSIONS)
+ SET(SSE_DEFINITIONS "${SSE_DEFINITIONS} -D__SSE2__")
+ endif()
+ if(HAVE_SSE_EXTENSIONS)
+ SET(SSE_DEFINITIONS "${SSE_DEFINITIONS} -D__SSE__")
+ endif()
+ endif()
+endmacro(PCL_CHECK_FOR_SSE)
set(PCL_SUBSYSTEMS_MODULES ${PCL_SUBSYSTEMS})
-list(REMOVE_ITEM PCL_SUBSYSTEMS_MODULES tools global_tests proctor examples)
+list(REMOVE_ITEM PCL_SUBSYSTEMS_MODULES tools cuda_apps global_tests proctor examples)
set(PCLCONFIG_AVAILABLE_COMPONENTS)
set(PCLCONFIG_AVAILABLE_COMPONENTS_LIST)
"${PCL_BINARY_DIR}/PCLConfig.cmake"
"${PCL_BINARY_DIR}/PCLConfigVersion.cmake"
COMPONENT pclconfig
- DESTINATION ${PCLCONFIG_INSTALL_DIR})
+ DESTINATION ${PCLCONFIG_INSTALL_DIR})
\ No newline at end of file
if(SUBSYS_EXT_DEPS)
foreach(_dep ${SUBSYS_EXT_DEPS})
string(TOUPPER "${_dep}_found" EXT_DEP_FOUND)
- if(NOT ${EXT_DEP_FOUND} OR (NOT ("${EXT_DEP_FOUND}" STREQUAL "TRUE")))
+ if(NOT ${EXT_DEP_FOUND} OR (NOT (${EXT_DEP_FOUND} STREQUAL "TRUE")))
set(${_var} FALSE)
PCL_SET_SUBSYS_STATUS(${_name} FALSE "Requires external library ${_dep}.")
- endif(NOT ${EXT_DEP_FOUND} OR (NOT ("${EXT_DEP_FOUND}" STREQUAL "TRUE")))
+ endif(NOT ${EXT_DEP_FOUND} OR (NOT (${EXT_DEP_FOUND} STREQUAL "TRUE")))
endforeach(_dep)
endif(SUBSYS_EXT_DEPS)
endif(${_var} AND (NOT ("${subsys_status}" STREQUAL "AUTO_OFF")))
if(MSVC90 OR MSVC10)
target_link_libraries(${_name} delayimp.lib) # because delay load is enabled for openmp.dll
endif()
- #
- # Only link if needed
- if(WIN32 AND MSVC)
- set_target_properties(${_name} PROPERTIES LINK_FLAGS_RELEASE /OPT:REF)
- elseif(CMAKE_SYSTEM_NAME STREQUAL "Darwin")
- if(NOT CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
- set_target_properties(${_name} PROPERTIES LINK_FLAGS -Wl)
- endif()
- elseif(__COMPILER_PATHSCALE)
- set_target_properties(${_name} PROPERTIES LINK_FLAGS -mp)
- elseif(CMAKE_COMPILER_IS_GNUCXX AND MINGW)
- set_target_properties(${_name} PROPERTIES LINK_FLAGS "-Wl,--allow-multiple-definition -Wl,--as-needed")
- else()
- set_target_properties(${_name} PROPERTIES LINK_FLAGS -Wl,--as-needed)
- endif()
- #
+
set_target_properties(${_name} PROPERTIES
VERSION ${PCL_VERSION}
SOVERSION ${PCL_MAJOR_VERSION}.${PCL_MINOR_VERSION}
endmacro(PCL_ADD_LIBRARY)
+###############################################################################
+# Add a cuda library target.
+# _name The library name.
+# _component The part of PCL that this library belongs to.
+# ARGN The source files for the library.
+macro(PCL_CUDA_ADD_LIBRARY _name _component)
+ REMOVE_VTK_DEFINITIONS()
+ if(PCL_SHARED_LIBS)
+ # to overcome a limitation in cuda_add_library, we add manually PCLAPI_EXPORTS macro
+ cuda_add_library(${_name} ${PCL_LIB_TYPE} ${ARGN} OPTIONS -DPCLAPI_EXPORTS)
+ else(PCL_SHARED_LIBS)
+ cuda_add_library(${_name} ${PCL_LIB_TYPE} ${ARGN})
+ endif(PCL_SHARED_LIBS)
+
+ # must link explicitly against boost.
+ target_link_libraries(${_name} ${Boost_LIBRARIES})
+
+ set_target_properties(${_name} PROPERTIES
+ VERSION ${PCL_VERSION}
+ SOVERSION ${PCL_MAJOR_VERSION}
+ DEFINE_SYMBOL "PCLAPI_EXPORTS")
+ if(USE_PROJECT_FOLDERS)
+ set_target_properties(${_name} PROPERTIES FOLDER "Libraries")
+ endif(USE_PROJECT_FOLDERS)
+
+ install(TARGETS ${_name}
+ RUNTIME DESTINATION ${BIN_INSTALL_DIR} COMPONENT pcl_${_component}
+ LIBRARY DESTINATION ${LIB_INSTALL_DIR} COMPONENT pcl_${_component}
+ ARCHIVE DESTINATION ${LIB_INSTALL_DIR} COMPONENT pcl_${_component})
+endmacro(PCL_CUDA_ADD_LIBRARY)
+
+
###############################################################################
# Add an executable target.
# _name The executable name.
else()
target_link_libraries(${_name} ${Boost_LIBRARIES})
endif()
- #
- # Only link if needed
+
if(WIN32 AND MSVC)
- set_target_properties(${_name} PROPERTIES LINK_FLAGS_RELEASE /OPT:REF
- DEBUG_OUTPUT_NAME ${_name}${CMAKE_DEBUG_POSTFIX}
+ set_target_properties(${_name} PROPERTIES DEBUG_OUTPUT_NAME ${_name}${CMAKE_DEBUG_POSTFIX}
RELEASE_OUTPUT_NAME ${_name}${CMAKE_RELEASE_POSTFIX})
- elseif(CMAKE_SYSTEM_NAME STREQUAL "Darwin")
- if(NOT CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
- set_target_properties(${_name} PROPERTIES LINK_FLAGS -Wl)
- endif()
- elseif(__COMPILER_PATHSCALE)
- set_target_properties(${_name} PROPERTIES LINK_FLAGS -mp)
- elseif(CMAKE_COMPILER_IS_GNUCXX AND MINGW)
- set_target_properties(${_name} PROPERTIES LINK_FLAGS "-Wl,--allow-multiple-definition -Wl,--as-needed")
- else()
- set_target_properties(${_name} PROPERTIES LINK_FLAGS -Wl,--as-needed)
endif()
- #
+
if(USE_PROJECT_FOLDERS)
set_target_properties(${_name} PROPERTIES FOLDER "Tools and demos")
endif(USE_PROJECT_FOLDERS)
else()
target_link_libraries(${_name} ${Boost_LIBRARIES})
endif()
- #
- # Only link if needed
+
if(WIN32 AND MSVC)
- set_target_properties(${_name} PROPERTIES LINK_FLAGS_RELEASE /OPT:REF
- DEBUG_OUTPUT_NAME ${_name}${CMAKE_DEBUG_POSTFIX}
+ set_target_properties(${_name} PROPERTIES DEBUG_OUTPUT_NAME ${_name}${CMAKE_DEBUG_POSTFIX}
RELEASE_OUTPUT_NAME ${_name}${CMAKE_RELEASE_POSTFIX})
- elseif(CMAKE_SYSTEM_NAME STREQUAL "Darwin")
- if(NOT CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
- set_target_properties(${_name} PROPERTIES LINK_FLAGS -Wl)
- endif()
- elseif(__COMPILER_PATHSCALE)
- set_target_properties(${_name} PROPERTIES LINK_FLAGS -mp)
- elseif(CMAKE_COMPILER_IS_GNUCXX AND MINGW)
- set_target_properties(${_name} PROPERTIES LINK_FLAGS "-Wl,--allow-multiple-definition -Wl,--as-needed")
- else()
- set_target_properties(${_name} PROPERTIES LINK_FLAGS -Wl,--as-needed)
endif()
- #
+
if(USE_PROJECT_FOLDERS)
set_target_properties(${_name} PROPERTIES FOLDER "Tools and demos")
endif(USE_PROJECT_FOLDERS)
endmacro(PCL_ADD_EXECUTABLE_OPT_BUNDLE)
+###############################################################################
+# Add an executable target.
+# _name The executable name.
+# _component The part of PCL that this library belongs to.
+# ARGN the source files for the library.
+macro(PCL_CUDA_ADD_EXECUTABLE _name _component)
+ REMOVE_VTK_DEFINITIONS()
+ cuda_add_executable(${_name} ${ARGN})
+ # must link explicitly against boost.
+ target_link_libraries(${_name} ${Boost_LIBRARIES})
+
+ if(WIN32 AND MSVC)
+ set_target_properties(${_name} PROPERTIES DEBUG_OUTPUT_NAME ${_name}${CMAKE_DEBUG_POSTFIX}
+ RELEASE_OUTPUT_NAME ${_name}${CMAKE_RELEASE_POSTFIX})
+ endif()
+
+ if(USE_PROJECT_FOLDERS)
+ set_target_properties(${_name} PROPERTIES FOLDER "Tools and demos")
+ endif(USE_PROJECT_FOLDERS)
+
+ set(PCL_EXECUTABLES ${PCL_EXECUTABLES} ${_name})
+ install(TARGETS ${_name} RUNTIME DESTINATION ${BIN_INSTALL_DIR}
+ COMPONENT pcl_${_component})
+endmacro(PCL_CUDA_ADD_EXECUTABLE)
+
###############################################################################
# Add a test target.
# _name The test name.
endif(NOT WIN32)
#target_link_libraries(${_exename} ${GTEST_BOTH_LIBRARIES} ${PCL_ADD_TEST_LINK_WITH})
target_link_libraries(${_exename} ${PCL_ADD_TEST_LINK_WITH} ${CLANG_LIBRARIES})
- #
- # Only link if needed
+
if(CMAKE_SYSTEM_NAME STREQUAL "Darwin")
- if(NOT CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
- set_target_properties(${_exename} PROPERTIES LINK_FLAGS -Wl)
- endif()
target_link_libraries(${_exename} pthread)
elseif(UNIX AND NOT ANDROID)
- set_target_properties(${_exename} PROPERTIES LINK_FLAGS -Wl,--as-needed)
# GTest >= 1.5 requires pthread and CMake's 2.8.4 FindGTest is broken
target_link_libraries(${_exename} pthread)
- elseif(CMAKE_COMPILER_IS_GNUCXX AND MINGW)
- set_target_properties(${_exename} PROPERTIES LINK_FLAGS "-Wl,--allow-multiple-definition -Wl,--as-needed")
- elseif(WIN32)
- set_target_properties(${_exename} PROPERTIES LINK_FLAGS_RELEASE /OPT:REF)
endif()
- #
+
# must link explicitly against boost only on Windows
target_link_libraries(${_exename} ${Boost_LIBRARIES})
#
endif(USE_PROJECT_FOLDERS)
endif(DOXYGEN_FOUND)
endmacro(PCL_ADD_DOC)
+
+###############################################################################
+# Add a dependency for a grabber
+# _name The dependency name.
+# _description The description text to display when dependency is not found.
+# This macro adds on option named "WITH_NAME", where NAME is the capitalized
+# dependency name. The user may use this option to control whether the
+# corresponding grabber should be built or not. Also an attempt to find a
+# package with the given name is made. If it is not successfull, then the
+# "WITH_NAME" option is coerced to FALSE.
+macro(PCL_ADD_GRABBER_DEPENDENCY _name _description)
+ string(TOUPPER ${_name} _name_capitalized)
+ option(WITH_${_name_capitalized} "${_description}" TRUE)
+ if(WITH_${_name_capitalized})
+ find_package(${_name})
+ if (NOT ${_name_capitalized}_FOUND)
+ set(WITH_${_name_capitalized} FALSE CACHE BOOL "${_description}" FORCE)
+ message(WARNING "${_description}: not building because ${_name} not found")
+ else()
+ set(HAVE_${_name_capitalized} TRUE)
+ include_directories(SYSTEM "${${_name_capitalized}_INCLUDE_DIRS}")
+ endif()
+ endif()
+endmacro(PCL_ADD_GRABBER_DEPENDENCY)
src/projection_matrix.cpp
src/time_trigger.cpp
src/gaussian.cpp
+ src/colors.cpp
+ src/feature_histogram.cpp
${range_image_srcs}
)
include/pcl/pcl_tests.h
include/pcl/cloud_iterator.h
include/pcl/TextureMesh.h
- include/pcl/PCLPointField.h
- include/pcl/PCLPointCloud2.h
- include/pcl/PCLImage.h
- include/pcl/PCLHeader.h
- include/pcl/ModelCoefficients.h
- include/pcl/PolygonMesh.h
- include/pcl/Vertices.h
- include/pcl/PointIndices.h
include/pcl/sse.h
include/pcl/PCLPointField.h
include/pcl/PCLPointCloud2.h
include/pcl/common/random.h
include/pcl/common/generate.h
include/pcl/common/projection_matrix.h
- include/pcl/register_point_struct.h
- include/pcl/conversions.h
+ include/pcl/common/colors.h
+ include/pcl/common/feature_histogram.h
)
set(common_incs_impl
return (out);
}
+ inline bool operator== (const PCLHeader &lhs, const PCLHeader &rhs)
+ {
+ return (&lhs == &rhs) ||
+ (lhs.seq == rhs.seq && lhs.stamp == rhs.stamp && lhs.frame_id == rhs.frame_id);
+ }
+
} // namespace pcl
#endif // PCL_ROSLIB_MESSAGE_HEADER_H
std::vector<std::vector<pcl::Vertices> > tex_polygons; // polygon which is mapped with specific texture defined in TexMaterial
- std::vector<std::vector<Eigen::Vector2f> > tex_coordinates; // UV coordinates
+ std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > tex_coordinates; // UV coordinates
std::vector<pcl::TexMaterial> tex_materials; // define texture material
public:
#pragma GCC system_header
#endif
+#ifndef Q_MOC_RUN
// Marking all Boost headers as system headers to remove warnings
#include <boost/fusion/sequence/intrinsic/at_key.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/signals2.hpp>
#include <boost/signals2/slot.hpp>
#include <boost/algorithm/string.hpp>
+#endif
#endif // PCL_COMMON_BOOST_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_COMMON_COLORS_H
+#define PCL_COMMON_COLORS_H
+
+#include <pcl/pcl_macros.h>
+
+namespace pcl
+{
+
+ struct RGB;
+
+ PCL_EXPORTS RGB
+ getRandomColor (double min = 0.2, double max = 2.8);
+
+ /** Color lookup table consisting of 256 colors structured in a maximally
+ * discontinuous manner. Generated using the method of Glasbey et al.
+ * (see https://github.com/taketwo/glasbey) */
+ class PCL_EXPORTS GlasbeyLUT
+ {
+
+ public:
+
+ /** Get a color from the lookup table with a given id.
+ *
+ * The id should be less than the size of the LUT (see size()). */
+ static RGB at (unsigned int color_id);
+
+ /** Get the number of colors in the lookup table.
+ *
+ * Note: the number of colors is different from the number of elements
+ * in the lookup table (each color is defined by three bytes). */
+ static size_t size ();
+
+ /** Get a raw pointer to the lookup table. */
+ static const unsigned char* data ();
+
+ };
+
+}
+
+#endif /* PCL_COMMON_COLORS_H */
+
/*@{*/
namespace pcl
{
- /** \brief Compute the smallest angle between two vectors in the [ 0, PI ) interval in 3D.
+ /** \brief Compute the smallest angle between two 3D vectors in radians (default) or degree.
* \param v1 the first 3D vector (represented as a \a Eigen::Vector4f)
* \param v2 the second 3D vector (represented as a \a Eigen::Vector4f)
- * \return the angle between v1 and v2
+ * \return the angle between v1 and v2 in radians or degrees
+ * \note Handles rounding error for parallel and anti-parallel vectors
* \ingroup common
*/
inline double
- getAngle3D (const Eigen::Vector4f &v1, const Eigen::Vector4f &v2);
+ getAngle3D (const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree = false);
+
+ /** \brief Compute the smallest angle between two 3D vectors in radians (default) or degree.
+ * \param v1 the first 3D vector (represented as a \a Eigen::Vector3f)
+ * \param v2 the second 3D vector (represented as a \a Eigen::Vector3f)
+ * \param in_degree determine if angle should be in radians or degrees
+ * \return the angle between v1 and v2 in radians or degrees
+ * \ingroup common
+ */
+ inline double
+ getAngle3D (const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const bool in_degree = false);
+
/** \brief Compute both the mean and the standard deviation of an array of values
* \param values the array of values
/** \brief determines the eigenvalues and corresponding eigenvectors of the symmetric positive semi definite input matrix
* \param[in] mat symmetric positive semi definite input matrix
- * \param[out] evecs resulting eigenvalues in ascending order
- * \param[out] evals corresponding eigenvectors in correct order according to eigenvalues
+ * \param[out] evecs corresponding eigenvectors in correct order according to eigenvalues
+ * \param[out] evals resulting eigenvalues in ascending order
* \ingroup common
*/
template <typename Matrix, typename Vector> void
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef PCL_FEATURE_HISTOGRAM_H_
+#define PCL_FEATURE_HISTOGRAM_H_
+
+#include <vector>
+
+#include <pcl/pcl_macros.h>
+
+namespace pcl
+{
+ /** \brief Type for histograms for computing mean and variance of some floats.
+ *
+ * \author Timur Ibadov (ibadov.timur@gmail.com)
+ * \ingroup common
+ */
+ class PCL_EXPORTS FeatureHistogram
+ {
+ public:
+ /** \brief Public constructor.
+ * \param[in] number_of_bins number of bins in the histogram.
+ * \param[in] min lower threshold.
+ * \param[in] max upper threshold.
+ */
+ FeatureHistogram (const size_t number_of_bins, const float min,
+ const float max);
+
+ /** \brief Public destructor. */
+ virtual ~FeatureHistogram ();
+
+ /** \brief Get the lower threshold.
+ * \return lower threshold.
+ */
+ float
+ getThresholdMin () const;
+
+ /** \brief Get the upper threshold.
+ * \return upper threshold.
+ */
+ float
+ getThresholdMax () const;
+
+ /** \brief Get the number of elements was added to the histogram.
+ * \return number of elements in the histogram.
+ */
+ size_t
+ getNumberOfElements () const;
+
+ /** \brief Get number of bins in the histogram.
+ * \return number of bins in the histogram.
+ */
+ size_t
+ getNumberOfBins () const;
+
+ /** \brief Increase a bin, that corresponds the value.
+ * \param[in] value new value.
+ */
+ void
+ addValue (float value);
+
+ /** \brief Get value, corresponds to the greatest bin.
+ * \return mean value of the greatest bin.
+ */
+ float
+ getMeanValue ();
+
+ /** \brief Get variance of the value.
+ * \return variance of the greatest bin.
+ */
+ float
+ getVariance (float mean);
+
+ protected:
+ /** \brief Vector, that contain the histogram. */
+ std::vector <unsigned> histogram_;
+
+ /** \brief Min threshold. */
+ float threshold_min_;
+ /** \brief Max threshold. */
+ float threshold_max_;
+ /** \brief "Width" of a bin. */
+ float step_;
+
+ /** \brief Number of values was added to the histogram. */
+ size_t number_of_elements_;
+
+ /** \brief Number of bins. */
+ size_t number_of_bins_;
+ };
+}
+#endif // PCL_FEATURE_HISTOGRAM_H_
\ No newline at end of file
template <typename PointT> inline float
distance (const PointT& p1, const PointT& p2)
{
- Eigen::Vector3f diff = p1 -p2;
+ Eigen::Vector3f diff = p1.getVector3fMap () - p2.getVector3fMap ();
return (diff.norm ());
}
template<typename PointT> inline float
squaredDistance (const PointT& p1, const PointT& p2)
{
- Eigen::Vector3f diff = p1 -p2;
+ Eigen::Vector3f diff = p1.getVector3fMap () - p2.getVector3fMap ();
return (diff.squaredNorm ());
}
//////////////////////////////////////////////////////////////////////////////////////////////
inline double
-pcl::getAngle3D (const Eigen::Vector4f &v1, const Eigen::Vector4f &v2)
+pcl::getAngle3D (const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree)
{
// Compute the actual angle
- double rad = v1.dot (v2) / sqrt (v1.squaredNorm () * v2.squaredNorm ());
- if (rad < -1.0) rad = -1.0;
- if (rad > 1.0) rad = 1.0;
- return (acos (rad));
+ double rad = v1.normalized ().dot (v2.normalized ());
+ if (rad < -1.0)
+ rad = -1.0;
+ else if (rad > 1.0)
+ rad = 1.0;
+ return (in_degree ? acos (rad) * 180.0 / M_PI : acos (rad));
+}
+
+inline double
+pcl::getAngle3D (const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const bool in_degree)
+{
+ // Compute the actual angle
+ double rad = v1.normalized ().dot (v2.normalized ());
+ if (rad < -1.0)
+ rad = -1.0;
+ else if (rad > 1.0)
+ rad = 1.0;
+ return (in_degree ? acos (rad) * 180.0 / M_PI : acos (rad));
}
//////////////////////////////////////////////////////////////////////////////////////////////
float max_dist = -FLT_MAX;
int max_idx = -1;
float dist;
+ const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> ();
// If the data is dense, we don't need to check for NaN
if (cloud.is_dense)
{
for (size_t i = 0; i < cloud.points.size (); ++i)
{
- pcl::Vector4fMapConst pt = cloud.points[i].getVector4fMap ();
- dist = (pivot_pt - pt).norm ();
+ pcl::Vector3fMapConst pt = cloud.points[i].getVector3fMap ();
+ dist = (pivot_pt3 - pt).norm ();
if (dist > max_dist)
{
max_idx = int (i);
// Check if the point is invalid
if (!pcl_isfinite (cloud.points[i].x) || !pcl_isfinite (cloud.points[i].y) || !pcl_isfinite (cloud.points[i].z))
continue;
- pcl::Vector4fMapConst pt = cloud.points[i].getVector4fMap ();
- dist = (pivot_pt - pt).norm ();
+ pcl::Vector3fMapConst pt = cloud.points[i].getVector3fMap ();
+ dist = (pivot_pt3 - pt).norm ();
if (dist > max_dist)
{
max_idx = int (i);
float max_dist = -FLT_MAX;
int max_idx = -1;
float dist;
+ const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> ();
// If the data is dense, we don't need to check for NaN
if (cloud.is_dense)
{
for (size_t i = 0; i < indices.size (); ++i)
{
- pcl::Vector4fMapConst pt = cloud.points[indices[i]].getVector4fMap ();
- dist = (pivot_pt - pt).norm ();
+ pcl::Vector3fMapConst pt = cloud.points[indices[i]].getVector3fMap ();
+ dist = (pivot_pt3 - pt).norm ();
if (dist > max_dist)
{
max_idx = static_cast<int> (i);
!pcl_isfinite (cloud.points[indices[i]].z))
continue;
- pcl::Vector4fMapConst pt = cloud.points[indices[i]].getVector4fMap ();
- dist = (pivot_pt - pt).norm ();
+ pcl::Vector3fMapConst pt = cloud.points[indices[i]].getVector3fMap ();
+ dist = (pivot_pt3 - pt).norm ();
if (dist > max_dist)
{
max_idx = static_cast<int> (i);
}
if(max_idx != -1)
- max_pt = cloud.points[max_idx].getVector4fMap ();
+ max_pt = cloud.points[indices[max_idx]].getVector4fMap ();
else
max_pt = Eigen::Vector4f(std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN());
}
set (p, intensity);
}
};
+
+ template<>
+ struct IntensityFieldAccessor<pcl::PointXYZHSV>
+ {
+ inline float
+ operator () (const pcl::PointXYZHSV &p) const
+ {
+ return (p.v);
+ }
+
+ inline void
+ get (const pcl::PointXYZHSV &p, float &intensity) const
+ {
+ intensity = p.v;
+ }
+
+ inline void
+ set (pcl::PointXYZHSV &p, float intensity) const
+ {
+ p.v = intensity;
+ p.s = 0.0f;
+ }
+
+ inline void
+ demean (pcl::PointXYZHSV& p, float value) const
+ {
+ p.v -= value;
+ }
+
+ inline void
+ add (pcl::PointXYZHSV& p, float value) const
+ {
+ p.v += value;
+ }
+ };
+
+ template<>
+ struct IntensityFieldAccessor<pcl::PointXYZL>
+ {
+ inline float
+ operator () (const pcl::PointXYZL &p) const
+ {
+ return (static_cast<float>(p.label));
+ }
+
+ inline void
+ get (const pcl::PointXYZL &p, float &intensity) const
+ {
+ intensity = static_cast<float>(p.label);
+ }
+
+ inline void
+ set (pcl::PointXYZL &p, float intensity) const
+ {
+ p.label = static_cast<uint32_t>(intensity);
+
+ }
+
+ inline void
+ demean (pcl::PointXYZL& p, float value) const
+ {
+ p.label -= static_cast<uint32_t>(value);
+ }
+
+ inline void
+ add (pcl::PointXYZL& p, float value) const
+ {
+ p.label += static_cast<uint32_t>(value);
+ }
+ };
+
+ template<>
+ struct IntensityFieldAccessor<pcl::PointXYZLNormal>
+ {
+ inline float
+ operator () (const pcl::PointXYZLNormal &p) const
+ {
+ return (static_cast<float>(p.label));
+ }
+
+ inline void
+ get (const pcl::PointXYZLNormal &p, float &intensity) const
+ {
+ intensity = static_cast<float>(p.label);
+ }
+
+ inline void
+ set (pcl::PointXYZLNormal &p, float intensity) const
+ {
+ p.label = static_cast<uint32_t>(intensity);
+
+ }
+
+ inline void
+ demean (pcl::PointXYZLNormal& p, float value) const
+ {
+ p.label -= static_cast<uint32_t>(value);
+ }
+
+ inline void
+ add (pcl::PointXYZLNormal& p, float value) const
+ {
+ p.label += static_cast<uint32_t>(value);
+ }
+ };
+
+ template<>
+ struct IntensityFieldAccessor<pcl::InterestPoint>
+ {
+ inline float
+ operator () (const pcl::InterestPoint &p) const
+ {
+ return (p.strength);
+ }
+
+ inline void
+ get (const pcl::InterestPoint &p, float &intensity) const
+ {
+ intensity = p.strength;
+ }
+
+ inline void
+ set (pcl::InterestPoint &p, float intensity) const
+ {
+ p.strength = intensity;
+ }
+
+ inline void
+ demean (pcl::InterestPoint& p, float value) const
+ {
+ p.strength -= value;
+ }
+
+ inline void
+ add (pcl::InterestPoint& p, float value) const
+ {
+ p.strength += value;
+ }
+ };
+
+ template<>
+ struct IntensityFieldAccessor<pcl::PointWithRange>
+ {
+ inline float
+ operator () (const pcl::PointWithRange &p) const
+ {
+ return (p.range);
+ }
+
+ inline void
+ get (const pcl::PointWithRange &p, float &intensity) const
+ {
+ intensity = p.range;
+ }
+
+ inline void
+ set (pcl::PointWithRange &p, float intensity) const
+ {
+ p.range = intensity;
+ }
+
+ inline void
+ demean (pcl::PointWithRange& p, float value) const
+ {
+ p.range -= value;
+ }
+
+ inline void
+ add (pcl::PointWithRange& p, float value) const
+ {
+ p.range += value;
+ }
+ };
+
+ template<>
+ struct IntensityFieldAccessor<pcl::PointWithScale>
+ {
+ inline float
+ operator () (const pcl::PointWithScale &p) const
+ {
+ return (p.scale);
+ }
+
+ inline void
+ get (const pcl::PointWithScale &p, float &intensity) const
+ {
+ intensity = p.scale;
+ }
+
+ inline void
+ set (pcl::PointWithScale &p, float intensity) const
+ {
+ p.scale = intensity;
+ }
+
+ inline void
+ demean (pcl::PointWithScale& p, float value) const
+ {
+ p.scale -= value;
+ }
+
+ inline void
+ add (pcl::PointWithScale& p, float value) const
+ {
+ p.scale += value;
+ }
+ };
+
+ template<>
+ struct IntensityFieldAccessor<pcl::PointWithViewpoint>
+ {
+ inline float
+ operator () (const pcl::PointWithViewpoint &p) const
+ {
+ return (p.z);
+ }
+
+ inline void
+ get (const pcl::PointWithViewpoint &p, float &intensity) const
+ {
+ intensity = p.z;
+ }
+
+ inline void
+ set (pcl::PointWithViewpoint &p, float intensity) const
+ {
+ p.z = intensity;
+ }
+
+ inline void
+ demean (pcl::PointWithViewpoint& p, float value) const
+ {
+ p.z -= value;
+ }
+
+ inline void
+ add (pcl::PointWithViewpoint& p, float value) const
+ {
+ p.z += value;
+ }
+ };
+
+ template<>
+ struct IntensityFieldAccessor<pcl::PointSurfel>
+ {
+ inline float
+ operator () (const pcl::PointSurfel &p) const
+ {
+ return (p.curvature);
+ }
+
+ inline void
+ get (const pcl::PointSurfel &p, float &intensity) const
+ {
+ intensity = p.curvature;
+ }
+
+ inline void
+ set (pcl::PointSurfel &p, float intensity) const
+ {
+ p.curvature = intensity;
+ }
+
+ inline void
+ demean (pcl::PointSurfel& p, float value) const
+ {
+ p.curvature -= value;
+ }
+
+ inline void
+ add (pcl::PointSurfel& p, float value) const
+ {
+ p.curvature += value;
+ }
+ };
}
}
cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
cloud_out.points.resize (cloud_in.points.size ());
+ if (cloud_in.points.size () == 0)
+ return;
+
if (isSamePointType<PointInT, PointOutT> ())
// Copy the whole memory block
memcpy (&cloud_out.points[0], &cloud_in.points[0], cloud_in.points.size () * sizeof (PointInT));
template<typename real>
inline pcl::BivariatePolynomialT<real>
pcl::PolynomialCalculationsT<real>::bivariatePolynomialApproximation (
- std::vector<Eigen::Matrix<real, 3, 1> >& samplePoints, unsigned int polynomial_degree, bool& error) const
+ std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints, unsigned int polynomial_degree, bool& error) const
{
pcl::BivariatePolynomialT<real> ret;
error = bivariatePolynomialApproximation (samplePoints, polynomial_degree, ret);
template<typename real>
inline bool
pcl::PolynomialCalculationsT<real>::bivariatePolynomialApproximation (
- std::vector<Eigen::Matrix<real, 3, 1> >& samplePoints, unsigned int polynomial_degree,
+ std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints, unsigned int polynomial_degree,
pcl::BivariatePolynomialT<real>& ret) const
{
//MEASURE_FUNCTION_TIME;
real tmpX, tmpY;
real *tmpC = new real[parameters_size];
real* tmpCEndPtr = &tmpC[parameters_size-1];
- for (typename std::vector<Eigen::Matrix<real, 3, 1> >::const_iterator it=samplePoints.begin ();
+ for (typename std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >::const_iterator it=samplePoints.begin ();
it!=samplePoints.end (); ++it)
{
currentX= (*it)[0]; currentY= (*it)[1]; currentZ= (*it)[2];
//unsigned int posInC;
//real tmpX, tmpY;
//DVector<real> tmpC (parameters_size);
- //for (typename std::vector<Eigen::Matrix<real, 3, 1> >::const_iterator it=samplePoints.begin ();
+ //for (typename std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >::const_iterator it=samplePoints.begin ();
// it!=samplePoints.end (); ++it)
//{
//currentX= (*it)[0]; currentY= (*it)[1]; currentZ= (*it)[2];
template <typename PointT, typename Scalar> void
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
+ bool copy_all_fields)
{
if (&cloud_in != &cloud_out)
{
- // Note: could be replaced by cloud_out = cloud_in
cloud_out.header = cloud_in.header;
cloud_out.is_dense = cloud_in.is_dense;
cloud_out.width = cloud_in.width;
cloud_out.height = cloud_in.height;
- cloud_out.points.reserve (cloud_out.points.size ());
- cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
+ cloud_out.points.reserve (cloud_in.points.size ());
+ if (copy_all_fields)
+ cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
+ else
+ cloud_out.points.resize (cloud_in.points.size ());
cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
}
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const std::vector<int> &indices,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
+ bool copy_all_fields)
{
size_t npts = indices.size ();
// In order to transform the data, we need to remove NaNs
for (size_t i = 0; i < npts; ++i)
{
// Copy fields first, then transform xyz data
- //cloud_out.points[i] = cloud_in.points[indices[i]];
+ if (copy_all_fields)
+ cloud_out.points[i] = cloud_in.points[indices[i]];
//cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
// otherwise we get errors during the multiplication (?)
for (size_t i = 0; i < npts; ++i)
{
+ if (copy_all_fields)
+ cloud_out.points[i] = cloud_in.points[indices[i]];
if (!pcl_isfinite (cloud_in.points[indices[i]].x) ||
!pcl_isfinite (cloud_in.points[indices[i]].y) ||
!pcl_isfinite (cloud_in.points[indices[i]].z))
continue;
- //cloud_out.points[i] = cloud_in.points[indices[i]];
//cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
template <typename PointT, typename Scalar> void
pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
+ bool copy_all_fields)
{
if (&cloud_in != &cloud_out)
{
cloud_out.height = cloud_in.height;
cloud_out.is_dense = cloud_in.is_dense;
cloud_out.points.reserve (cloud_out.points.size ());
- cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
+ if (copy_all_fields)
+ cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
+ else
+ cloud_out.points.resize (cloud_in.points.size ());
cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
}
pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
const std::vector<int> &indices,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
+ bool copy_all_fields)
{
size_t npts = indices.size ();
// In order to transform the data, we need to remove NaNs
{
for (size_t i = 0; i < cloud_out.points.size (); ++i)
{
+ // Copy fields first, then transform
+ if (copy_all_fields)
+ cloud_out.points[i] = cloud_in.points[indices[i]];
//cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
{
for (size_t i = 0; i < cloud_out.points.size (); ++i)
{
+ // Copy fields first, then transform
+ if (copy_all_fields)
+ cloud_out.points[i] = cloud_in.points[indices[i]];
+
if (!pcl_isfinite (cloud_in.points[indices[i]].x) ||
!pcl_isfinite (cloud_in.points[indices[i]].y) ||
!pcl_isfinite (cloud_in.points[indices[i]].z))
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Matrix<Scalar, 3, 1> &offset,
- const Eigen::Quaternion<Scalar> &rotation)
+ const Eigen::Quaternion<Scalar> &rotation,
+ bool copy_all_fields)
{
Eigen::Translation<Scalar, 3> translation (offset);
// Assemble an Eigen Transform
Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
- transformPointCloud (cloud_in, cloud_out, t);
+ transformPointCloud (cloud_in, cloud_out, t, copy_all_fields);
}
///////////////////////////////////////////////////////////////////////////////////////////
pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Matrix<Scalar, 3, 1> &offset,
- const Eigen::Quaternion<Scalar> &rotation)
+ const Eigen::Quaternion<Scalar> &rotation,
+ bool copy_all_fields)
{
Eigen::Translation<Scalar, 3> translation (offset);
// Assemble an Eigen Transform
Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
- transformPointCloudWithNormals (cloud_in, cloud_out, t);
+ transformPointCloudWithNormals (cloud_in, cloud_out, t, copy_all_fields);
}
///////////////////////////////////////////////////////////////////////////////////////////
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
PointT ret = point;
- //ret.getVector3fMap () = transform * point.getVector3fMap ();
- ret.x = static_cast<float> (transform (0, 0) * point.x + transform (0, 1) * point.y + transform (0, 2) * point.z + transform (0, 3));
- ret.y = static_cast<float> (transform (1, 0) * point.x + transform (1, 1) * point.y + transform (1, 2) * point.z + transform (1, 3));
- ret.z = static_cast<float> (transform (2, 0) * point.x + transform (2, 1) * point.y + transform (2, 2) * point.z + transform (2, 3));
+ ret.getVector3fMap () = transform * point.getVector3fMap ();
+
+ return (ret);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT, typename Scalar> inline PointT
+pcl::transformPointWithNormal (const PointT &point,
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
+{
+ PointT ret = point;
+ ret.getVector3fMap () = transform * point.getVector3fMap ();
+ ret.getNormalVector3fMap () = transform.rotation () * point.getNormalVector3fMap ();
return (ret);
}
inline PCA&
operator= (PCA const & pca)
{
- eigenvectors_ = pca.eigenvectors;
- coefficients_ = pca.coefficients;
- eigenvalues_ = pca.eigenvalues;
- mean_ = pca.mean;
+ eigenvectors_ = pca.eigenvectors_;
+ coefficients_ = pca.coefficients_;
+ eigenvalues_ = pca.eigenvalues_;
+ mean_ = pca.mean_;
return (*this);
}
compute_done_ = false;
}
+ /** \brief Provide a pointer to the vector of indices that represents the input data.
+ * \param[in] indices a pointer to the indices that represent the input data.
+ */
+ virtual void
+ setIndices (const IndicesPtr &indices)
+ {
+ Base::setIndices (indices);
+ compute_done_ = false;
+ }
+
+ /** \brief Provide a pointer to the vector of indices that represents the input data.
+ * \param[in] indices a pointer to the indices that represent the input data.
+ */
+ virtual void
+ setIndices (const IndicesConstPtr &indices)
+ {
+ Base::setIndices (indices);
+ compute_done_ = false;
+ }
+
+ /** \brief Provide a pointer to the vector of indices that represents the input data.
+ * \param[in] indices a pointer to the indices that represent the input data.
+ */
+ virtual void
+ setIndices (const PointIndicesConstPtr &indices)
+ {
+ Base::setIndices (indices);
+ compute_done_ = false;
+ }
+
+ /** \brief Set the indices for the points laying within an interest region of
+ * the point cloud.
+ * \note you shouldn't call this method on unorganized point clouds!
+ * \param[in] row_start the offset on rows
+ * \param[in] col_start the offset on columns
+ * \param[in] nb_rows the number of rows to be considered row_start included
+ * \param[in] nb_cols the number of columns to be considered col_start included
+ */
+ virtual void
+ setIndices (size_t row_start, size_t col_start, size_t nb_rows, size_t nb_cols)
+ {
+ Base::setIndices (row_start, col_start, nb_rows, nb_cols);
+ compute_done_ = false;
+ }
+
/** \brief Mean accessor
* \throw InitFailedException
*/
{
/** Tests if the 3D components of a point are all finite
* param[in] pt point to be tested
+ * return true if finite, false otherwise
*/
template <typename PointT> inline bool
isFinite (const PointT &pt)
template<> inline bool isFinite<pcl::SHOT1344> (const pcl::SHOT1344&) { return (true); }
template<> inline bool isFinite<pcl::ReferenceFrame> (const pcl::ReferenceFrame&) { return (true); }
template<> inline bool isFinite<pcl::ShapeContext1980> (const pcl::ShapeContext1980&) { return (true); }
+ template<> inline bool isFinite<pcl::UniqueShapeContext1960> (const pcl::UniqueShapeContext1960&) { return (true); }
template<> inline bool isFinite<pcl::PFHSignature125> (const pcl::PFHSignature125&) { return (true); }
template<> inline bool isFinite<pcl::PFHRGBSignature250> (const pcl::PFHRGBSignature250&) { return (true); }
template<> inline bool isFinite<pcl::PPFSignature> (const pcl::PPFSignature&) { return (true); }
template<> inline bool isFinite<pcl::VFHSignature308> (const pcl::VFHSignature308&) { return (true); }
template<> inline bool isFinite<pcl::ESFSignature640> (const pcl::ESFSignature640&) { return (true); }
template<> inline bool isFinite<pcl::IntensityGradient> (const pcl::IntensityGradient&) { return (true); }
+ template<> inline bool isFinite<pcl::BRISKSignature512> (const pcl::BRISKSignature512&) { return (true); }
// specification for pcl::PointXY
template <> inline bool
* error is set to true if the approximation did not work for any reason
* (not enough points, matrix not invertible, etc.) */
inline BivariatePolynomialT<real>
- bivariatePolynomialApproximation (std::vector<Eigen::Matrix<real, 3, 1> >& samplePoints,
+ bivariatePolynomialApproximation (std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints,
unsigned int polynomial_degree, bool& error) const;
//! Same as above, using a reference for the return value
inline bool
- bivariatePolynomialApproximation (std::vector<Eigen::Matrix<real, 3, 1> >& samplePoints,
+ bivariatePolynomialApproximation (std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints,
unsigned int polynomial_degree, BivariatePolynomialT<real>& ret) const;
//! Set the minimum value under which values are considered zero
#endif
#include <cmath>
+#include <queue>
#include <string>
+#ifndef Q_MOC_RUN
#include <boost/date_time/posix_time/posix_time.hpp>
+#endif
/**
* \file pcl/common/time.h
std::string title_;
};
+ /** \brief A helper class to measure frequency of a certain event.
+ *
+ * To use this class create an instance and call event() function every time
+ * the event in question occurs. The estimated frequency can be retrieved
+ * with getFrequency() function.
+ *
+ * \author Sergey Alexandrov
+ * \ingroup common
+ */
+ class EventFrequency
+ {
+
+ public:
+
+ /** \brief Constructor.
+ *
+ * \param[in] window_size number of most recent events that are
+ * considered in frequency estimation (default: 30) */
+ EventFrequency (size_t window_size = 30)
+ : window_size_ (window_size)
+ {
+ stop_watch_.reset ();
+ }
+
+ /** \brief Notifies the class that the event occured. */
+ void event ()
+ {
+ event_time_queue_.push (stop_watch_.getTimeSeconds ());
+ if (event_time_queue_.size () > window_size_)
+ event_time_queue_.pop ();
+ }
+
+ /** \brief Retrieve the estimated frequency. */
+ double
+ getFrequency () const
+ {
+ if (event_time_queue_.size () < 2)
+ return (0.0);
+ return ((event_time_queue_.size () - 1) /
+ (event_time_queue_.back () - event_time_queue_.front ()));
+ }
+
+ /** \brief Reset frequency computation. */
+ void reset ()
+ {
+ stop_watch_.reset ();
+ event_time_queue_ = std::queue<double> ();
+ }
+
+ private:
+
+ pcl::StopWatch stop_watch_;
+ std::queue<double> event_time_queue_;
+ const size_t window_size_;
+
+ };
#ifndef MEASURE_FUNCTION_TIME
#define MEASURE_FUNCTION_TIME \
#define PCL_COMMON_TIME_TRIGGER_H_
#include <pcl/pcl_macros.h>
+#ifndef Q_MOC_RUN
#include <boost/function.hpp>
#include <boost/thread.hpp>
#include <boost/signals2.hpp>
+#endif
namespace pcl
{
* \param[in] cloud_in the input point cloud
* \param[out] cloud_out the resultant output point cloud
* \param[in] transform an affine transformation (typically a rigid transformation)
+ * \param[in] copy_all_fields flag that controls whether the contents of the fields
+ * (other than x, y, z) should be copied into the new transformed cloud
* \note Can be used with cloud_in equal to cloud_out
* \ingroup common
*/
template <typename PointT, typename Scalar> void
transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform);
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
+ bool copy_all_fields = true);
template <typename PointT> void
transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Affine3f &transform)
+ const Eigen::Affine3f &transform,
+ bool copy_all_fields = true)
{
- return (transformPointCloud<PointT, float> (cloud_in, cloud_out, transform));
+ return (transformPointCloud<PointT, float> (cloud_in, cloud_out, transform, copy_all_fields));
}
/** \brief Apply an affine transform defined by an Eigen Transform
* \param[in] indices the set of point indices to use from the input point cloud
* \param[out] cloud_out the resultant output point cloud
* \param[in] transform an affine transformation (typically a rigid transformation)
+ * \param[in] copy_all_fields flag that controls whether the contents of the fields
+ * (other than x, y, z) should be copied into the new transformed cloud
* \ingroup common
*/
template <typename PointT, typename Scalar> void
transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const std::vector<int> &indices,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform);
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
+ bool copy_all_fields = true);
template <typename PointT> void
transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const std::vector<int> &indices,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Affine3f &transform)
+ const Eigen::Affine3f &transform,
+ bool copy_all_fields = true)
{
- return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform));
+ return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform, copy_all_fields));
}
/** \brief Apply an affine transform defined by an Eigen Transform
* \param[in] indices the set of point indices to use from the input point cloud
* \param[out] cloud_out the resultant output point cloud
* \param[in] transform an affine transformation (typically a rigid transformation)
+ * \param[in] copy_all_fields flag that controls whether the contents of the fields
+ * (other than x, y, z) should be copied into the new transformed cloud
* \ingroup common
*/
template <typename PointT, typename Scalar> void
transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const pcl::PointIndices &indices,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
+ bool copy_all_fields = true)
{
- return (transformPointCloud<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform));
+ return (transformPointCloud<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform, copy_all_fields));
}
template <typename PointT> void
transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const pcl::PointIndices &indices,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Affine3f &transform)
+ const Eigen::Affine3f &transform,
+ bool copy_all_fields = true)
{
- return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform));
+ return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform, copy_all_fields));
}
/** \brief Transform a point cloud and rotate its normals using an Eigen transform.
* \param[in] cloud_in the input point cloud
* \param[out] cloud_out the resultant output point cloud
* \param[in] transform an affine transformation (typically a rigid transformation)
+ * \param[in] copy_all_fields flag that controls whether the contents of the fields
+ * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new
+ * transformed cloud
* \note Can be used with cloud_in equal to cloud_out
*/
template <typename PointT, typename Scalar> void
transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform);
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
+ bool copy_all_fields = true);
template <typename PointT> void
transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Affine3f &transform)
+ const Eigen::Affine3f &transform,
+ bool copy_all_fields = true)
{
- return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, transform));
+ return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, transform, copy_all_fields));
}
/** \brief Transform a point cloud and rotate its normals using an Eigen transform.
* \param[in] indices the set of point indices to use from the input point cloud
* \param[out] cloud_out the resultant output point cloud
* \param[in] transform an affine transformation (typically a rigid transformation)
+ * \param[in] copy_all_fields flag that controls whether the contents of the fields
+ * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new
+ * transformed cloud
*/
template <typename PointT, typename Scalar> void
transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
const std::vector<int> &indices,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform);
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
+ bool copy_all_fields = true);
template <typename PointT> void
transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
const std::vector<int> &indices,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Affine3f &transform)
+ const Eigen::Affine3f &transform,
+ bool copy_all_fields = true)
{
- return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform));
+ return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform, copy_all_fields));
}
/** \brief Transform a point cloud and rotate its normals using an Eigen transform.
* \param[in] indices the set of point indices to use from the input point cloud
* \param[out] cloud_out the resultant output point cloud
* \param[in] transform an affine transformation (typically a rigid transformation)
+ * \param[in] copy_all_fields flag that controls whether the contents of the fields
+ * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new
+ * transformed cloud
*/
template <typename PointT, typename Scalar> void
transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
const pcl::PointIndices &indices,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
+ bool copy_all_fields = true)
{
- return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform));
+ return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform, copy_all_fields));
}
transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
const pcl::PointIndices &indices,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Affine3f &transform)
+ const Eigen::Affine3f &transform,
+ bool copy_all_fields = true)
{
- return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform));
+ return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform, copy_all_fields));
}
/** \brief Apply a rigid transform defined by a 4x4 matrix
* \param[in] cloud_in the input point cloud
* \param[out] cloud_out the resultant output point cloud
* \param[in] transform a rigid transformation
+ * \param[in] copy_all_fields flag that controls whether the contents of the fields
+ * (other than x, y, z) should be copied into the new transformed cloud
* \note Can be used with cloud_in equal to cloud_out
* \ingroup common
*/
template <typename PointT, typename Scalar> void
transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Matrix<Scalar, 4, 4> &transform)
+ const Eigen::Matrix<Scalar, 4, 4> &transform,
+ bool copy_all_fields = true)
{
Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
- return (transformPointCloud<PointT, Scalar> (cloud_in, cloud_out, t));
+ return (transformPointCloud<PointT, Scalar> (cloud_in, cloud_out, t, copy_all_fields));
}
template <typename PointT> void
transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Matrix4f &transform)
+ const Eigen::Matrix4f &transform,
+ bool copy_all_fields = true)
{
- return (transformPointCloud<PointT, float> (cloud_in, cloud_out, transform));
+ return (transformPointCloud<PointT, float> (cloud_in, cloud_out, transform, copy_all_fields));
}
/** \brief Apply a rigid transform defined by a 4x4 matrix
* \param[in] indices the set of point indices to use from the input point cloud
* \param[out] cloud_out the resultant output point cloud
* \param[in] transform a rigid transformation
+ * \param[in] copy_all_fields flag that controls whether the contents of the fields
+ * (other than x, y, z) should be copied into the new transformed cloud
* \ingroup common
*/
template <typename PointT, typename Scalar> void
transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const std::vector<int> &indices,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Matrix<Scalar, 4, 4> &transform)
+ const Eigen::Matrix<Scalar, 4, 4> &transform,
+ bool copy_all_fields = true)
{
Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
- return (transformPointCloud<PointT, Scalar> (cloud_in, indices, cloud_out, t));
+ return (transformPointCloud<PointT, Scalar> (cloud_in, indices, cloud_out, t, copy_all_fields));
}
template <typename PointT> void
transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const std::vector<int> &indices,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Matrix4f &transform)
+ const Eigen::Matrix4f &transform,
+ bool copy_all_fields = true)
{
- return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform));
+ return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform, copy_all_fields));
}
/** \brief Apply a rigid transform defined by a 4x4 matrix
* \param[in] indices the set of point indices to use from the input point cloud
* \param[out] cloud_out the resultant output point cloud
* \param[in] transform a rigid transformation
+ * \param[in] copy_all_fields flag that controls whether the contents of the fields
+ * (other than x, y, z) should be copied into the new transformed cloud
* \ingroup common
*/
template <typename PointT, typename Scalar> void
transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const pcl::PointIndices &indices,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Matrix<Scalar, 4, 4> &transform)
+ const Eigen::Matrix<Scalar, 4, 4> &transform,
+ bool copy_all_fields = true)
{
- return (transformPointCloud<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform));
+ return (transformPointCloud<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform, copy_all_fields));
}
template <typename PointT> void
transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const pcl::PointIndices &indices,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Matrix4f &transform)
+ const Eigen::Matrix4f &transform,
+ bool copy_all_fields = true)
{
- return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform));
+ return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform, copy_all_fields));
}
/** \brief Transform a point cloud and rotate its normals using an Eigen transform.
* \param[in] cloud_in the input point cloud
* \param[out] cloud_out the resultant output point cloud
* \param[in] transform an affine transformation (typically a rigid transformation)
+ * \param[in] copy_all_fields flag that controls whether the contents of the fields
+ * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new
+ * transformed cloud
* \note Can be used with cloud_in equal to cloud_out
* \ingroup common
*/
template <typename PointT, typename Scalar> void
transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Matrix<Scalar, 4, 4> &transform)
+ const Eigen::Matrix<Scalar, 4, 4> &transform,
+ bool copy_all_fields = true)
{
Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
- return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, cloud_out, t));
+ return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, cloud_out, t, copy_all_fields));
}
template <typename PointT> void
transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Matrix4f &transform)
+ const Eigen::Matrix4f &transform,
+ bool copy_all_fields = true)
{
- return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, transform));
+ return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, transform, copy_all_fields));
}
/** \brief Transform a point cloud and rotate its normals using an Eigen transform.
* \param[in] indices the set of point indices to use from the input point cloud
* \param[out] cloud_out the resultant output point cloud
* \param[in] transform an affine transformation (typically a rigid transformation)
+ * \param[in] copy_all_fields flag that controls whether the contents of the fields
+ * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new
+ * transformed cloud
* \note Can be used with cloud_in equal to cloud_out
* \ingroup common
*/
transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
const std::vector<int> &indices,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Matrix<Scalar, 4, 4> &transform)
+ const Eigen::Matrix<Scalar, 4, 4> &transform,
+ bool copy_all_fields = true)
{
Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
- return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices, cloud_out, t));
+ return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices, cloud_out, t, copy_all_fields));
}
transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
const std::vector<int> &indices,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Matrix4f &transform)
+ const Eigen::Matrix4f &transform,
+ bool copy_all_fields = true)
{
- return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform));
+ return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform, copy_all_fields));
}
/** \brief Transform a point cloud and rotate its normals using an Eigen transform.
* \param[in] indices the set of point indices to use from the input point cloud
* \param[out] cloud_out the resultant output point cloud
* \param[in] transform an affine transformation (typically a rigid transformation)
+ * \param[in] copy_all_fields flag that controls whether the contents of the fields
+ * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new
+ * transformed cloud
* \note Can be used with cloud_in equal to cloud_out
* \ingroup common
*/
transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
const pcl::PointIndices &indices,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Matrix<Scalar, 4, 4> &transform)
+ const Eigen::Matrix<Scalar, 4, 4> &transform,
+ bool copy_all_fields = true)
{
Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
- return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices, cloud_out, t));
+ return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices, cloud_out, t, copy_all_fields));
}
transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
const pcl::PointIndices &indices,
pcl::PointCloud<PointT> &cloud_out,
- const Eigen::Matrix4f &transform)
+ const Eigen::Matrix4f &transform,
+ bool copy_all_fields = true)
{
- return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform));
+ return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform, copy_all_fields));
}
/** \brief Apply a rigid transform defined by a 3D offset and a quaternion
* \param[out] cloud_out the resultant output point cloud
* \param[in] offset the translation component of the rigid transformation
* \param[in] rotation the rotation component of the rigid transformation
+ * \param[in] copy_all_fields flag that controls whether the contents of the fields
+ * (other than x, y, z) should be copied into the new transformed cloud
* \ingroup common
*/
template <typename PointT, typename Scalar> inline void
transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Matrix<Scalar, 3, 1> &offset,
- const Eigen::Quaternion<Scalar> &rotation);
+ const Eigen::Quaternion<Scalar> &rotation,
+ bool copy_all_fields = true);
template <typename PointT> inline void
transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Vector3f &offset,
- const Eigen::Quaternionf &rotation)
+ const Eigen::Quaternionf &rotation,
+ bool copy_all_fields = true)
{
- return (transformPointCloud<PointT, float> (cloud_in, cloud_out, offset, rotation));
+ return (transformPointCloud<PointT, float> (cloud_in, cloud_out, offset, rotation, copy_all_fields));
}
/** \brief Transform a point cloud and rotate its normals using an Eigen transform.
* \param[out] cloud_out the resultant output point cloud
* \param[in] offset the translation component of the rigid transformation
* \param[in] rotation the rotation component of the rigid transformation
+ * \param[in] copy_all_fields flag that controls whether the contents of the fields
+ * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new
+ * transformed cloud
* \ingroup common
*/
template <typename PointT, typename Scalar> inline void
transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Matrix<Scalar, 3, 1> &offset,
- const Eigen::Quaternion<Scalar> &rotation);
+ const Eigen::Quaternion<Scalar> &rotation,
+ bool copy_all_fields = true);
template <typename PointT> void
transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Vector3f &offset,
- const Eigen::Quaternionf &rotation)
+ const Eigen::Quaternionf &rotation,
+ bool copy_all_fields = true)
{
- return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, offset, rotation));
+ return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, offset, rotation, copy_all_fields));
}
/** \brief Transform a point with members x,y,z
return (transformPoint<PointT, float> (point, transform));
}
+ /** \brief Transform a point with members x,y,z,normal_x,normal_y,normal_z
+ * \param[in] point the point to transform
+ * \param[out] transform the transformation to apply
+ * \return the transformed point
+ * \ingroup common
+ */
+ template <typename PointT, typename Scalar> inline PointT
+ transformPointWithNormal (const PointT &point,
+ const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform);
+
+ template <typename PointT> inline PointT
+ transformPointWithNormal (const PointT &point,
+ const Eigen::Affine3f &transform)
+ {
+ return (transformPointWithNormal<PointT, float> (point, transform));
+ }
+
/** \brief Calculates the principal (PCA-based) alignment of the point cloud
* \param[in] cloud the input point cloud
* \param[out] transform the resultant transform
#pragma GCC system_header
#endif
+#ifndef Q_MOC_RUN
#include <boost/date_time/posix_time/posix_time.hpp>
+#endif
#include <pcl/console/print.h>
namespace pcl
#include <pcl/for_each_type.h>
#include <pcl/exceptions.h>
#include <pcl/console/print.h>
+#ifndef Q_MOC_RUN
#include <boost/foreach.hpp>
+#endif
namespace pcl
{
{ \
std::ostringstream s; \
s << message; \
- s.flush (); \
throw ExceptionName(s.str(), __FILE__, BOOST_CURRENT_FUNCTION, __LINE__); \
}
public:
PCLException (const std::string& error_description,
- const std::string& file_name = "",
- const std::string& function_name = "" ,
- unsigned line_number = 0) throw ()
- : std::runtime_error (error_description)
+ const char* file_name = NULL,
+ const char* function_name = NULL,
+ unsigned line_number = 0)
+ : std::runtime_error (createDetailedMessage (error_description,
+ file_name,
+ function_name,
+ line_number))
, file_name_ (file_name)
, function_name_ (function_name)
- , message_ (error_description)
- , line_number_ (line_number)
- {
- message_ = detailedMessage ();
- }
-
- virtual ~PCLException () throw ()
+ , line_number_ (line_number)
{}
- const std::string&
+ const char*
getFileName () const throw ()
{
return (file_name_);
}
- const std::string&
+ const char*
getFunctionName () const throw ()
{
return (function_name_);
return (line_number_);
}
- std::string
+ const char*
detailedMessage () const throw ()
{
- std::stringstream sstream;
- if (function_name_ != "")
- sstream << function_name_ << " ";
+ return (what ());
+ }
+
+
+ protected:
+ static std::string
+ createDetailedMessage (const std::string& error_description,
+ const char* file_name,
+ const char* function_name,
+ unsigned line_number)
+ {
+ std::ostringstream sstream;
+ if (function_name != NULL)
+ sstream << function_name << " ";
- if (file_name_ != "")
+ if (file_name != NULL)
{
- sstream << "in " << file_name_ << " ";
- if (line_number_ != 0)
- sstream << "@ " << line_number_ << " ";
+ sstream << "in " << file_name << " ";
+ if (line_number != 0)
+ sstream << "@ " << line_number << " ";
}
- sstream << ": " << what ();
+ sstream << ": " << error_description;
return (sstream.str ());
}
-
- char const*
- what () const throw ()
- {
- return (message_.c_str ());
- }
-
- protected:
- std::string file_name_;
- std::string function_name_;
- std::string message_;
+
+ const char* file_name_;
+ const char* function_name_;
unsigned line_number_;
} ;
public:
InvalidConversionException (const std::string& error_description,
- const std::string& file_name = "",
- const std::string& function_name = "" ,
- unsigned line_number = 0) throw ()
+ const char* file_name = NULL,
+ const char* function_name = NULL,
+ unsigned line_number = 0)
: pcl::PCLException (error_description, file_name, function_name, line_number) { }
} ;
public:
IsNotDenseException (const std::string& error_description,
- const std::string& file_name = "",
- const std::string& function_name = "" ,
- unsigned line_number = 0) throw ()
+ const char* file_name = NULL,
+ const char* function_name = NULL,
+ unsigned line_number = 0)
: pcl::PCLException (error_description, file_name, function_name, line_number) { }
} ;
public:
InvalidSACModelTypeException (const std::string& error_description,
- const std::string& file_name = "",
- const std::string& function_name = "" ,
- unsigned line_number = 0) throw ()
+ const char* file_name = NULL,
+ const char* function_name = NULL,
+ unsigned line_number = 0)
: pcl::PCLException (error_description, file_name, function_name, line_number) { }
} ;
public:
IOException (const std::string& error_description,
- const std::string& file_name = "",
- const std::string& function_name = "" ,
- unsigned line_number = 0) throw ()
+ const char* file_name = NULL,
+ const char* function_name = NULL,
+ unsigned line_number = 0)
: pcl::PCLException (error_description, file_name, function_name, line_number) { }
} ;
{
public:
InitFailedException (const std::string& error_description = "",
- const std::string& file_name = "",
- const std::string& function_name = "" ,
- unsigned line_number = 0) throw ()
+ const char* file_name = NULL,
+ const char* function_name = NULL,
+ unsigned line_number = 0)
: pcl::PCLException (error_description, file_name, function_name, line_number) { }
} ;
public:
UnorganizedPointCloudException (const std::string& error_description,
- const std::string& file_name = "",
- const std::string& function_name = "" ,
- unsigned line_number = 0) throw ()
+ const char* file_name = NULL,
+ const char* function_name = NULL,
+ unsigned line_number = 0)
: pcl::PCLException (error_description, file_name, function_name, line_number) { }
} ;
public:
KernelWidthTooSmallException (const std::string& error_description,
- const std::string& file_name = "",
- const std::string& function_name = "" ,
- unsigned line_number = 0) throw ()
+ const char* file_name = NULL,
+ const char* function_name = NULL,
+ unsigned line_number = 0)
: pcl::PCLException (error_description, file_name, function_name, line_number) { }
} ;
{
public:
UnhandledPointTypeException (const std::string& error_description,
- const std::string& file_name = "",
- const std::string& function_name = "" ,
- unsigned line_number = 0) throw ()
+ const char* file_name = NULL,
+ const char* function_name = NULL,
+ unsigned line_number = 0)
: pcl::PCLException (error_description, file_name, function_name, line_number) { }
};
{
public:
ComputeFailedException (const std::string& error_description,
- const std::string& file_name = "",
- const std::string& function_name = "" ,
- unsigned line_number = 0) throw ()
+ const char* file_name = NULL,
+ const char* function_name = NULL,
+ unsigned line_number = 0)
: pcl::PCLException (error_description, file_name, function_name, line_number) { }
};
{
public:
BadArgumentException (const std::string& error_description,
- const std::string& file_name = "",
- const std::string& function_name = "" ,
- unsigned line_number = 0) throw ()
+ const char* file_name = NULL,
+ const char* function_name = NULL,
+ unsigned line_number = 0)
: pcl::PCLException (error_description, file_name, function_name, line_number) { }
};
}
#pragma GCC system_header
#endif
+#ifndef Q_MOC_RUN
#include <boost/mpl/is_sequence.hpp>
#include <boost/mpl/begin_end.hpp>
#include <boost/mpl/next_prior.hpp>
#include <boost/mpl/not.hpp>
#include <boost/mpl/aux_/unwrap.hpp>
#include <boost/type_traits/is_same.hpp>
+#endif
namespace pcl
{
{
indices_->resize (input_->points.size ());
}
- catch (std::bad_alloc)
+ catch (const std::bad_alloc&)
{
PCL_ERROR ("[initCompute] Failed to allocate %lu indices.\n", input_->points.size ());
}
(pcl::PointNormal) \
(pcl::PointXYZRGBNormal) \
(pcl::PointXYZINormal) \
+ (pcl::PointXYZLNormal) \
(pcl::PointWithRange) \
(pcl::PointWithViewpoint) \
(pcl::MomentInvariants) \
(pcl::NormalBasedSignature12) \
(pcl::FPFHSignature33) \
(pcl::VFHSignature308) \
+ (pcl::GRSDSignature21) \
(pcl::ESFSignature640) \
+ (pcl::BRISKSignature512) \
(pcl::Narf36) \
(pcl::IntensityGradient) \
(pcl::PointWithScale) \
(pcl::PointSurfel) \
(pcl::ShapeContext1980) \
+ (pcl::UniqueShapeContext1960) \
(pcl::SHOT352) \
(pcl::SHOT1344) \
(pcl::PointUV) \
- (pcl::ReferenceFrame)
+ (pcl::ReferenceFrame) \
+ (pcl::PointDEM)
// Define all point types that include RGB data
#define PCL_RGB_POINT_TYPES \
(pcl::PointNormal) \
(pcl::PointXYZRGBNormal) \
(pcl::PointXYZINormal) \
+ (pcl::PointXYZLNormal) \
(pcl::PointWithRange) \
(pcl::PointWithViewpoint) \
(pcl::PointWithScale) \
- (pcl::PointSurfel)
+ (pcl::PointSurfel) \
+ (pcl::PointDEM)
// Define all point types with XYZ and label
#define PCL_XYZL_POINT_TYPES \
(pcl::PointXYZL) \
- (pcl::PointXYZRGBL)
+ (pcl::PointXYZRGBL) \
+ (pcl::PointXYZLNormal)
// Define all point types that include normal[3] data
#define PCL_NORMAL_POINT_TYPES \
(pcl::PointNormal) \
(pcl::PointXYZRGBNormal) \
(pcl::PointXYZINormal) \
+ (pcl::PointXYZLNormal) \
(pcl::PointSurfel)
// Define all point types that represent features
(pcl::NormalBasedSignature12) \
(pcl::FPFHSignature33) \
(pcl::VFHSignature308) \
+ (pcl::GRSDSignature21) \
(pcl::ESFSignature640) \
+ (pcl::BRISKSignature512) \
(pcl::Narf36)
namespace pcl
data[3] = 1.0f;
normal_x = normal_y = normal_z = data_n[3] = 0.0f;
intensity = 0.0f;
+ curvature = 0;
}
friend std::ostream& operator << (std::ostream& os, const PointXYZINormal& p);
};
+//----
+ struct EIGEN_ALIGN16 _PointXYZLNormal
+ {
+ PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
+ PCL_ADD_NORMAL4D; // This adds the member normal[3] which can also be accessed using the point (which is float[4])
+ union
+ {
+ struct
+ {
+ uint32_t label;
+ float curvature;
+ };
+ float data_c[4];
+ };
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+
+ PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZLNormal& p);
+ /** \brief A point structure representing Euclidean xyz coordinates, a label, together with normal coordinates and the surface curvature estimate.
+ * \ingroup common
+ */
+ struct PointXYZLNormal : public _PointXYZLNormal
+ {
+ inline PointXYZLNormal (const _PointXYZLNormal &p)
+ {
+ x = p.x; y = p.y; z = p.z; data[3] = 1.0f;
+ normal_x = p.normal_x; normal_y = p.normal_y; normal_z = p.normal_z; data_n[3] = 0.0f;
+ curvature = p.curvature;
+ label = p.label;
+ }
+
+ inline PointXYZLNormal ()
+ {
+ x = y = z = 0.0f;
+ data[3] = 1.0f;
+ normal_x = normal_y = normal_z = data_n[3] = 0.0f;
+ label = 0;
+ curvature = 0;
+ }
+
+ friend std::ostream& operator << (std::ostream& os, const PointXYZLNormal& p);
+ };
+
+// ---
+
struct EIGEN_ALIGN16 _PointWithRange
{
friend std::ostream& operator << (std::ostream& os, const ShapeContext1980& p);
};
+ PCL_EXPORTS std::ostream& operator << (std::ostream& os, const UniqueShapeContext1960& p);
+ /** \brief A point structure representing a Unique Shape Context.
+ * \ingroup common
+ */
+ struct UniqueShapeContext1960
+ {
+ float descriptor[1960];
+ float rf[9];
+ static int descriptorSize () { return 1960; }
+
+ friend std::ostream& operator << (std::ostream& os, const UniqueShapeContext1960& p);
+ };
PCL_EXPORTS std::ostream& operator << (std::ostream& os, const SHOT352& p);
/** \brief A point structure representing the generic Signature of Histograms of OrienTations (SHOT) - shape only.
friend std::ostream& operator << (std::ostream& os, const VFHSignature308& p);
};
+
+ PCL_EXPORTS std::ostream& operator << (std::ostream& os, const GRSDSignature21& p);
+ /** \brief A point structure representing the Global Radius-based Surface Descriptor (GRSD).
+ * \ingroup common
+ */
+ struct GRSDSignature21
+ {
+ float histogram[21];
+ static int descriptorSize () { return 21; }
+
+ friend std::ostream& operator << (std::ostream& os, const GRSDSignature21& p);
+ };
+
+ PCL_EXPORTS std::ostream& operator << (std::ostream& os, const BRISKSignature512& p);
+ /** \brief A point structure representing the Binary Robust Invariant Scalable Keypoints (BRISK).
+ * \ingroup common
+ */
+ struct BRISKSignature512
+ {
+ float scale;
+ float orientation;
+ unsigned char descriptor[64];
+ static int descriptorSize () { return 64; }
+
+ friend std::ostream& operator << (std::ostream& os, const BRISKSignature512& p);
+ };
PCL_EXPORTS std::ostream& operator << (std::ostream& os, const ESFSignature640& p);
/** \brief A point structure representing the Ensemble of Shape Functions (ESF).
friend std::ostream& operator << (std::ostream& os, const PointSurfel& p);
};
+ struct EIGEN_ALIGN16 _PointDEM
+ {
+ PCL_ADD_POINT4D;
+ float intensity;
+ float intensity_variance;
+ float height_variance;
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+
+ PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointDEM& p);
+ /** \brief A point structure representing Digital Elevation Map.
+ * \ingroup common
+ */
+ struct PointDEM : public _PointDEM
+ {
+ inline PointDEM (const _PointDEM &p)
+ {
+ x = p.x; y = p.y; x = p.z; data[3] = 1.0f;
+ intensity = p.intensity;
+ intensity_variance = p.intensity_variance;
+ height_variance = p.height_variance;
+ }
+
+ inline PointDEM ()
+ {
+ x = y = z = 0.0f; data[3] = 1.0f;
+ intensity = 0.0f;
+ intensity_variance = height_variance = 0.0f;
+ }
+
+ friend std::ostream& operator << (std::ostream& os, const PointDEM& p);
+ };
+
template <int N> std::ostream&
operator << (std::ostream& os, const Histogram<N>& p)
{
* \author Patrick Mihelich, Radu B. Rusu
*/
template <typename PointT>
- class PointCloud
+ class PCL_EXPORTS PointCloud
{
public:
/** \brief Default constructor. Sets \ref is_dense to true, \ref width
}
};
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ template <>
+ class DefaultPointRepresentation<UniqueShapeContext1960> : public PointRepresentation<UniqueShapeContext1960>
+ {
+ public:
+ DefaultPointRepresentation ()
+ {
+ nr_dimensions_ = 1960;
+ }
+
+ virtual void
+ copyToFloatArray (const UniqueShapeContext1960 &p, float * out) const
+ {
+ for (int i = 0; i < nr_dimensions_; ++i)
+ out[i] = p.descriptor[i];
+ }
+ };
+
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <>
class DefaultPointRepresentation<SHOT352> : public PointRepresentation<SHOT352>
*/
struct PointXYZINormal;
+ /** \brief Members: float x, y, z, label, normal[3], curvature
+ * \ingroup common
+ */
+ struct PointXYZLNormal;
+
/** \brief Members: float x, y, z (union with float point[4]), range
* \ingroup common
*/
*/
struct ShapeContext1980;
+ /** \brief Members: float descriptor[1960], rf[9]
+ * \ingroup common
+ */
+ struct UniqueShapeContext1960;
+
/** \brief Members: float pfh[125]
* \ingroup common
*/
*/
struct VFHSignature308;
+ /** \brief Members: float grsd[21]
+ * \ingroup common
+ */
+ struct GRSDSignature21;
+
/** \brief Members: float esf[640]
* \ingroup common
*/
*/
struct GFPFHSignature16;
+ /** \brief Members: float scale; float orientation; uint8_t descriptor[64]
+ * \ingroup common
+ */
+ struct BRISKSignature512;
+
/** \brief Members: float x, y, z, roll, pitch, yaw; float descriptor[36]
* \ingroup common
*/
* \ingroup common
*/
struct PointSurfel;
+
+ /** \brief Members: float x, y, z, intensity, intensity_variance, height_variance
+ * \ingroup common
+ */
+ struct PointDEM;
}
/** @} */
(float, normal_z, normal_z)
(float, curvature, curvature)
)
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXYZLNormal,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (uint32_t, label, label)
+ (float, normal_x, normal_x)
+ (float, normal_y, normal_y)
+ (float, normal_z, normal_z)
+ (float, curvature, curvature)
+)
POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointWithRange,
(float, x, x)
(float, y, y)
(float[9], rf, rf)
)
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::UniqueShapeContext1960,
+ (float[1960], descriptor, shape_context)
+ (float[9], rf, rf)
+)
+
POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::SHOT352,
(float[352], descriptor, shot)
(float[9], rf, rf)
(float[33], histogram, fpfh)
)
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::BRISKSignature512,
+ (float, scale, brisk_scale)
+ (float, orientation, brisk_orientation)
+ (unsigned char[64], descriptor, brisk_descriptor512)
+)
+
POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::VFHSignature308,
(float[308], histogram, vfh)
)
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::GRSDSignature21,
+ (float[21], histogram, grsd)
+)
+
POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::ESFSignature640,
(float[640], histogram, esf)
)
)
POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::ReferenceFrame, pcl::_ReferenceFrame)
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointDEM,
+ (float, x, x)
+ (float, y, y)
+ (float, z, z)
+ (float, intensity, intensity)
+ (float, intensity_variance, intensity_variance)
+ (float, height_variance, height_variance)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointDEM, pcl::_PointDEM)
+
namespace pcl
{
// Allow float 'rgb' data to match to the newer uint32 'rgba' tag. This is so
* \param[out] out the output XYZI point
*/
inline void
- PointXYZRGBtoXYZI (PointXYZRGB& in,
- PointXYZI& out)
+ PointXYZRGBtoXYZI (const PointXYZRGB& in,
+ PointXYZI& out)
{
out.x = in.x; out.y = in.y; out.z = in.z;
out.intensity = 0.299f * static_cast <float> (in.r) + 0.587f * static_cast <float> (in.g) + 0.114f * static_cast <float> (in.b);
* \param[out] out the output Intensity point
*/
inline void
- PointRGBtoI (RGB& in,
+ PointRGBtoI (const RGB& in,
Intensity& out)
{
out.intensity = 0.299f * static_cast <float> (in.r) + 0.587f * static_cast <float> (in.g) + 0.114f * static_cast <float> (in.b);
* \param[out] out the output Intensity point
*/
inline void
- PointRGBtoI (RGB& in,
+ PointRGBtoI (const RGB& in,
Intensity8u& out)
{
out.intensity = static_cast<uint8_t>(std::numeric_limits<uint8_t>::max() * 0.299f * static_cast <float> (in.r)
* \param[out] out the output Intensity point
*/
inline void
- PointRGBtoI (RGB& in,
+ PointRGBtoI (const RGB& in,
Intensity32u& out)
{
out.intensity = static_cast<uint32_t>(static_cast<float>(std::numeric_limits<uint32_t>::max()) * 0.299f * static_cast <float> (in.r)
* \param[out] out the output XYZHSV point
*/
inline void
- PointXYZRGBtoXYZHSV (PointXYZRGB& in,
- PointXYZHSV& out)
+ PointXYZRGBtoXYZHSV (const PointXYZRGB& in,
+ PointXYZHSV& out)
{
const unsigned char max = std::max (in.r, std::max (in.g, in.b));
const unsigned char min = std::min (in.r, std::min (in.g, in.b));
+ out.x = in.x; out.y = in.y; out.z = in.z;
out.v = static_cast <float> (max) / 255.f;
if (max == 0) // division by zero
* \todo include the A parameter but how?
*/
inline void
- PointXYZRGBAtoXYZHSV (PointXYZRGBA& in,
- PointXYZHSV& out)
+ PointXYZRGBAtoXYZHSV (const PointXYZRGBA& in,
+ PointXYZHSV& out)
{
const unsigned char max = std::max (in.r, std::max (in.g, in.b));
const unsigned char min = std::min (in.r, std::min (in.g, in.b));
+ out.x = in.x; out.y = in.y; out.z = in.z;
out.v = static_cast <float> (max) / 255.f;
if (max == 0) // division by zero
* \param[out] out the output XYZRGB point
*/
inline void
- PointXYZHSVtoXYZRGB (PointXYZHSV& in,
- PointXYZRGB& out)
+ PointXYZHSVtoXYZRGB (const PointXYZHSV& in,
+ PointXYZRGB& out)
{
out.x = in.x; out.y = in.y; out.z = in.z;
if (in.s == 0)
* \param[out] out the output Intensity point cloud
*/
inline void
- PointCloudRGBtoI (PointCloud<RGB>& in,
+ PointCloudRGBtoI (const PointCloud<RGB>& in,
PointCloud<Intensity>& out)
{
out.width = in.width;
* \param[out] out the output Intensity point cloud
*/
inline void
- PointCloudRGBtoI (PointCloud<RGB>& in,
+ PointCloudRGBtoI (const PointCloud<RGB>& in,
PointCloud<Intensity8u>& out)
{
out.width = in.width;
* \param[out] out the output Intensity point cloud
*/
inline void
- PointCloudRGBtoI (PointCloud<RGB>& in,
+ PointCloudRGBtoI (const PointCloud<RGB>& in,
PointCloud<Intensity32u>& out)
{
out.width = in.width;
* \param[out] out the output XYZHSV point cloud
*/
inline void
- PointCloudXYZRGBtoXYZHSV (PointCloud<PointXYZRGB>& in,
- PointCloud<PointXYZHSV>& out)
+ PointCloudXYZRGBtoXYZHSV (
const PointCloud<PointXYZRGB>& in,
+ PointCloud<PointXYZHSV>& out)
{
out.width = in.width;
out.height = in.height;
* \param[out] out the output XYZHSV point cloud
*/
inline void
- PointCloudXYZRGBAtoXYZHSV (PointCloud<PointXYZRGBA>& in,
- PointCloud<PointXYZHSV>& out)
+ PointCloudXYZRGBAtoXYZHSV (
const PointCloud<PointXYZRGBA>& in,
+ PointCloud<PointXYZHSV>& out)
{
out.width = in.width;
out.height = in.height;
* \param[out] out the output XYZI point cloud
*/
inline void
- PointCloudXYZRGBtoXYZI (PointCloud<PointXYZRGB>& in,
- PointCloud<PointXYZI>& out)
+ PointCloudXYZRGBtoXYZI (
const PointCloud<PointXYZRGB>& in,
+ PointCloud<PointXYZI>& out)
{
out.width = in.width;
out.height = in.height;
* \param[out] out the output pointcloud
* **/
inline void
- PointCloudDepthAndRGBtoXYZRGBA (PointCloud<Intensity>& depth,
- PointCloud<RGB>& image,
- float& focal,
+ PointCloudDepthAndRGBtoXYZRGBA (const PointCloud<Intensity>& depth,
+ const PointCloud<RGB>& image,
+ const float& focal,
PointCloud<PointXYZRGBA>& out)
{
float bad_point = std::numeric_limits<float>::quiet_NaN();
/** \brief class BearingAngleImage is used as an interface to generate Bearing Angle(BA) image.
* \author: Qinghua Li (qinghua__li@163.com)
*/
- class BearingAngleImage : public pcl::PointCloud<PointXYZRGBA>
+ class
PCL_EXPORTS BearingAngleImage : public pcl::PointCloud<PointXYZRGBA>
{
public:
// ===== TYPEDEFS =====
#pragma warning (disable: 4244)
#endif
+//https://bugreports.qt-project.org/browse/QTBUG-22829
+#ifndef Q_MOC_RUN
#include <pcl/pcl_macros.h>
#include <pcl/point_traits.h>
#include <boost/mpl/vector.hpp>
#include <boost/preprocessor/cat.hpp>
#include <boost/preprocessor/comparison.hpp>
#include <boost/utility.hpp>
-//https://bugreports.qt-project.org/browse/QTBUG-22829
-#ifndef Q_MOC_RUN
#include <boost/type_traits.hpp>
#endif
#include <stddef.h> //offsetof
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/point_types.h>
+#include <pcl/common/colors.h>
+
+/// Lookup table
+static const unsigned char GLASBEY_LUT[] =
+{
+ 77 , 175, 74 ,
+ 228, 26 , 28 ,
+ 55 , 126, 184,
+ 152, 78 , 163,
+ 255, 127, 0 ,
+ 255, 255, 51 ,
+ 166, 86 , 40 ,
+ 247, 129, 191,
+ 153, 153, 153,
+ 0 , 0 , 255,
+ 255, 0 , 255,
+ 0 , 255, 248,
+ 0 , 255, 0 ,
+ 0 , 101, 255,
+ 255, 255, 180,
+ 52 , 68 , 1 ,
+ 0 , 0 , 68 ,
+ 96 , 0 , 41 ,
+ 158, 147, 0 ,
+ 116, 0 , 185,
+ 255, 0 , 114,
+ 0 , 149, 125,
+ 209, 186, 255,
+ 255, 183, 156,
+ 240, 0 , 174,
+ 208, 255, 245,
+ 0 , 255, 176,
+ 170, 255, 93 ,
+ 0 , 207, 255,
+ 255, 190, 1 ,
+ 241, 117, 255,
+ 52 , 74 , 167,
+ 150, 166, 103,
+ 255, 114, 114,
+ 171, 100, 109,
+ 161, 0 , 41 ,
+ 160, 135, 255,
+ 105, 86 , 121,
+ 178, 21 , 105,
+ 0 , 3 , 123,
+ 255, 221, 236,
+ 160, 238, 173,
+ 237, 161, 77 ,
+ 0 , 141, 255,
+ 0 , 97 , 109,
+ 1 , 238, 98 ,
+ 81 , 0 , 78 ,
+ 128, 103, 66 ,
+ 0 , 108, 44 ,
+ 209, 224, 94 ,
+ 155, 0 , 255,
+ 0 , 45 , 223,
+ 88 , 28 , 0 ,
+ 166, 2 , 162,
+ 165, 205, 239,
+ 84 , 121, 0 ,
+ 76 , 109, 80 ,
+ 122, 180, 0 ,
+ 104, 204, 204,
+ 145, 95 , 255,
+ 214, 208, 177,
+ 185, 130, 176,
+ 130, 120, 194,
+ 128, 96 , 0 ,
+ 247, 161, 255,
+ 10 , 65 , 119,
+ 232, 102, 54 ,
+ 7 , 191, 131,
+ 105, 54 , 171,
+ 10 , 177, 0 ,
+ 215, 191, 105,
+ 198, 66 , 249,
+ 140, 255, 145,
+ 135, 60 , 105,
+ 254, 170, 191,
+ 130, 173, 255,
+ 161, 35 , 0 ,
+ 197, 255, 0 ,
+ 40 , 153, 180,
+ 215, 83 , 185,
+ 255, 77 , 161,
+ 128, 175, 147,
+ 216, 91 , 124,
+ 193, 144, 91 ,
+ 210, 196, 0 ,
+ 232, 39 , 73 ,
+ 76 , 52 , 116,
+ 159, 206, 110,
+ 138, 147, 187,
+ 140, 5 , 114,
+ 0 , 56 , 183,
+ 191, 105, 0 ,
+ 83 , 58 , 0 ,
+ 94 , 224, 0 ,
+ 121, 99 , 99 ,
+ 212, 123, 104,
+ 89 , 160, 99 ,
+ 146, 58 , 54 ,
+ 231, 46 , 215,
+ 93 , 245, 200,
+ 191, 147, 133,
+ 255, 211, 89 ,
+ 171, 77 , 214,
+ 0 , 121, 0 ,
+ 60 , 14 , 107,
+ 255, 82 , 1 ,
+ 112, 115, 43 ,
+ 0 , 172, 245,
+ 255, 184, 240,
+ 1 , 210, 111,
+ 203, 151, 0 ,
+ 95 , 114, 129,
+ 183, 215, 17 ,
+ 80 , 110, 231,
+ 201, 25 , 87 ,
+ 218, 250, 203,
+ 255, 148, 103,
+ 255, 217, 163,
+ 198, 172, 199,
+ 78 , 139, 135,
+ 197, 255, 134,
+ 38 , 0 , 165,
+ 197, 208, 211,
+ 193, 117, 225,
+ 111, 0 , 128,
+ 147, 255, 238,
+ 125, 62 , 254,
+ 112, 213, 78 ,
+ 198, 76 , 61 ,
+ 155, 48 , 82 ,
+ 0 , 199, 176,
+ 118, 6 , 0 ,
+ 2 , 106, 192,
+ 140, 167, 49 ,
+ 189, 81 , 145,
+ 254, 162, 38 ,
+ 134, 138, 106,
+ 0 , 68 , 17 ,
+ 122, 73 , 61 ,
+ 255, 251, 239,
+ 127, 94 , 193,
+ 181, 140, 48 ,
+ 66 , 235, 255,
+ 189, 140, 218,
+ 190, 0 , 138,
+ 132, 177, 185,
+ 90 , 54 , 202,
+ 0 , 35 , 131,
+ 251, 139, 149,
+ 74 , 0 , 225,
+ 0 , 106, 90 ,
+ 106, 199, 155,
+ 104, 169, 217,
+ 255, 239, 134,
+ 44 , 94 , 130,
+ 126, 0 , 78 ,
+ 196, 214, 145,
+ 160, 238, 255,
+ 222, 215, 255,
+ 255, 87 , 126,
+ 170, 161, 255,
+ 81 , 120, 60 ,
+ 255, 242, 91 ,
+ 168, 130, 145,
+ 158, 153, 64 ,
+ 211, 123, 156,
+ 255, 0 , 3 ,
+ 210, 118, 197,
+ 0 , 41 , 111,
+ 198, 178, 125,
+ 211, 255, 169,
+ 109, 215, 130,
+ 41 , 90 , 0 ,
+ 235, 193, 183,
+ 114, 58 , 0 ,
+ 140, 96 , 155,
+ 163, 223, 193,
+ 255, 142, 63 ,
+ 66 , 155, 1 ,
+ 83 , 96 , 152,
+ 106, 133, 230,
+ 255, 85 , 72 ,
+ 141, 216, 0 ,
+ 162, 102, 73 ,
+ 79 , 0 , 146,
+ 190, 0 , 30 ,
+ 165, 0 , 193,
+ 81 , 84 , 255,
+ 0 , 148, 74 ,
+ 203, 0 , 255,
+ 121, 54 , 71 ,
+ 215, 255, 97 ,
+ 163, 178, 0 ,
+ 111, 154, 68 ,
+ 120, 93 , 222,
+ 254, 187, 126,
+ 112, 0 , 27 ,
+ 204, 59 , 0 ,
+ 0 , 165, 167,
+ 151, 255, 0 ,
+ 104, 41 , 124,
+ 138, 89 , 113,
+ 255, 94 , 224,
+ 86 , 91 , 48 ,
+ 75 , 255, 76 ,
+ 204, 190, 67 ,
+ 255, 224, 0 ,
+ 49 , 126, 85 ,
+ 145, 196, 135,
+ 187, 64 , 79 ,
+ 255, 130, 233,
+ 205, 127, 68 ,
+ 105, 195, 223,
+ 161, 213, 81 ,
+ 165, 183, 240,
+ 125, 255, 180,
+ 126, 255, 111,
+ 67 , 255, 145,
+ 154, 138, 83 ,
+ 46 , 145, 231,
+ 118, 121, 0 ,
+ 154, 2 , 222,
+ 185, 119, 255,
+ 255, 0 , 62 ,
+ 131, 28 , 170,
+ 177, 70 , 183,
+ 217, 0 , 115,
+ 186, 196, 95 ,
+ 97 , 46 , 97 ,
+ 84 , 134, 167,
+ 81 , 54 , 145,
+ 107, 117, 107,
+ 51 , 15 , 80 ,
+ 215, 139, 143,
+ 255, 247, 203,
+ 255, 86 , 192,
+ 153, 91 , 0 ,
+ 255, 1 , 156,
+ 183, 79 , 19 ,
+ 235, 255, 146,
+ 211, 1 , 224,
+ 178, 167, 144,
+ 217, 97 , 0 ,
+ 134, 91 , 38 ,
+ 175, 151, 206,
+ 0 , 182, 63 ,
+ 210, 40 , 179,
+ 2 , 213, 42 ,
+ 94 , 84 , 160,
+ 136, 48 , 0 ,
+ 255, 110, 163,
+ 144, 121, 157,
+ 153, 61 , 225,
+ 237, 87 , 255,
+ 87 , 24 , 206,
+ 117, 143, 207,
+};
+
+/// Number of colors in Glasbey lookup table
+static const unsigned int GLASBEY_LUT_SIZE = sizeof (GLASBEY_LUT) / (sizeof (GLASBEY_LUT[0]) * 3);
+
+pcl::RGB
+pcl::GlasbeyLUT::at (unsigned int color_id)
+{
+ assert (color_id < GLASBEY_LUT_SIZE);
+ pcl::RGB color;
+ color.r = GLASBEY_LUT[color_id * 3 + 0];
+ color.g = GLASBEY_LUT[color_id * 3 + 1];
+ color.b = GLASBEY_LUT[color_id * 3 + 2];
+ return (color);
+}
+
+size_t
+pcl::GlasbeyLUT::size ()
+{
+ return GLASBEY_LUT_SIZE;
+}
+
+const unsigned char*
+pcl::GlasbeyLUT::data ()
+{
+ return GLASBEY_LUT;
+}
+
+pcl::RGB
+pcl::getRandomColor (double min, double max)
+{
+ double sum;
+ static unsigned stepRGBA = 100;
+ double r, g, b;
+ do
+ {
+ sum = 0;
+ r = (rand () % stepRGBA) / static_cast<double> (stepRGBA);
+ while ((g = (rand () % stepRGBA) / static_cast<double> (stepRGBA)) == r) {}
+ while (((b = (rand () % stepRGBA) / static_cast<double> (stepRGBA)) == r) && (b == g)) {}
+ sum = r + g + b;
+ }
+ while (sum <= min || sum >= max);
+ pcl::RGB color;
+ color.r = uint8_t (r * 255.0);
+ color.g = uint8_t (g * 255.0);
+ color.b = uint8_t (b * 255.0);
+ return (color);
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <pcl/common/feature_histogram.h>
+
+#include <algorithm>
+
+#include <pcl/console/print.h>
+
+pcl::FeatureHistogram::FeatureHistogram (size_t const number_of_bins,
+ const float min, const float max) :
+ histogram_ (number_of_bins, 0)
+{
+ // Initialize thresholds.
+ if (min < max)
+ {
+ threshold_min_ = min;
+ threshold_max_ = max;
+ step_ = (max - min) / static_cast<float> (number_of_bins_);
+ }
+ else
+ {
+ threshold_min_ = 0.0f;
+ threshold_max_ = static_cast<float> (number_of_bins);
+ step_ = 1.0f;
+ PCL_WARN ("[FeatureHistogram::setThresholds] Variable \"max\" must be greater then \"min\".\n");
+ }
+
+ // Initialize sum.
+ number_of_elements_ = 0;
+
+ // Initialize size;
+ number_of_bins_ = number_of_bins;
+}
+
+pcl::FeatureHistogram::~FeatureHistogram ()
+{
+
+}
+
+float
+pcl::FeatureHistogram::getThresholdMin () const
+{
+ return (threshold_min_);
+}
+
+float
+pcl::FeatureHistogram::getThresholdMax () const
+{
+ return (threshold_max_);
+}
+
+size_t
+pcl::FeatureHistogram::getNumberOfElements () const
+{
+ return (number_of_elements_);
+}
+
+size_t
+pcl::FeatureHistogram::getNumberOfBins () const
+{
+ return (number_of_bins_);
+}
+
+void
+pcl::FeatureHistogram::addValue (float value)
+{
+ // Check, if value in the allowed range.
+ if (threshold_min_ < value && value < threshold_max_)
+ {
+ // Increase the sum.
+ ++number_of_elements_;
+
+ // Increase the bin.
+ size_t bin_number = static_cast<size_t> ((value - threshold_min_) / step_);
+ ++histogram_[bin_number];
+ }
+}
+
+float
+pcl::FeatureHistogram::getMeanValue ()
+{
+ // Check, if the histogram is empty.
+ if (number_of_elements_ == 0)
+ {
+ return (0.0f);
+ }
+ // Smoothe the histogram and find a bin with a max smoothed value.
+ size_t max_idx = 0;
+ float max = 0.50f * histogram_[0] +
+ 0.25f * histogram_[1] * 2.0f;
+ for (size_t bin = 1; bin < histogram_.size () - 1; ++bin)
+ {
+ float smothed_value = 0.25f * histogram_[bin - 1] +
+ 0.50f * histogram_[bin] +
+ 0.25f * histogram_[bin + 1];
+ if (smothed_value > max)
+ {
+ max = smothed_value;
+ max_idx = bin;
+ }
+ }
+ // Check last bin.
+ float last_value = 0.50f * histogram_[histogram_.size () - 1] +
+ 0.25f * histogram_[histogram_.size () - 2] * 2.0f;
+ if (last_value > max)
+ {
+ max_idx = histogram_.size ();
+ }
+
+ // Compute mean value.
+ float mean = step_ * (static_cast<float> (max_idx) + 0.5f) + threshold_min_;
+
+ return (mean);
+}
+
+float
+pcl::FeatureHistogram::getVariance (float mean)
+{
+ // Check, if the histogram is empty.
+ if (number_of_elements_ == 0)
+ {
+ return (0.0f);
+ }
+ // The histogram is not empty.
+ // Variable to accumulate the terms of variance.
+ float variances_sum = 0;
+
+ for (size_t bin = 0; bin < number_of_bins_; ++bin)
+ {
+ if (histogram_[bin] > 0)
+ {
+ // Value corresponding to the bin.
+ float value = step_ * (static_cast<float> (bin) + 0.5f) + threshold_min_;
+ float dif = value - mean;
+ variances_sum += static_cast<float> (histogram_[bin]) * dif * dif;
+ }
+ }
+
+ // Compute variance and return it.
+ return (variances_sum / static_cast<float> (number_of_elements_));
+}
\ No newline at end of file
{
indices_->resize (input_->width * input_->height);
}
- catch (std::bad_alloc)
+ catch (const std::bad_alloc&)
{
PCL_ERROR ("[initCompute] Failed to allocate %lu indices.\n", (input_->width * input_->height));
}
return (os);
}
+ std::ostream&
+ operator << (std::ostream& os, const PointXYZLNormal& p)
+ {
+ os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.label << " - " << p.normal[0] << "," << p.normal[1] << "," << p.normal[2] << " - " << p.curvature << ")";
+ return (os);
+ }
+
std::ostream&
operator << (std::ostream& os, const PointWithRange& p)
{
return (os);
}
+ std::ostream&
+ operator << (std::ostream& os, const UniqueShapeContext1960& p)
+ {
+ for (int i = 0; i < 9; ++i)
+ os << (i == 0 ? "(" : "") << p.rf[i] << (i < 8 ? ", " : ")");
+ for (size_t i = 0; i < 1960; ++i)
+ os << (i == 0 ? "(" : "") << p.descriptor[i] << (i < 1959 ? ", " : ")");
+ return (os);
+ }
+
std::ostream&
operator << (std::ostream& os, const SHOT352& p)
{
return (os);
}
+ std::ostream&
+ operator << (std::ostream& os, const BRISKSignature512& p)
+ {
+ os << p.scale << " " << p.orientation << " ";
+ for (int i = 0; i < 64; ++i)
+ os << (i == 0 ? "(" : "") << p.descriptor[i] << (i < 63 ? ", " : ")");
+ return (os);
+ }
+
std::ostream&
operator << (std::ostream& os, const ESFSignature640& p)
{
p.radius << " - " << p.confidence << " - " << p.curvature << ")";
return (os);
}
+
+ std::ostream&
+ operator << (std::ostream& os, const PointDEM& p)
+ {
+ os << "(" << p.x << "," << p.y << "," << p.z << " - "
+ << p.intensity << " - " << p.intensity_variance << " - "
+ << p.height_variance << ")";
+ return (os);
+ }
+
}
{
boost::unique_lock<boost::mutex> lock (condition_mutex_);
quit_ = true;
- condition_.notify_all ();
- lock.unlock ();
+ condition_.notify_all (); // notify all threads about updated quit_
+ lock.unlock (); // unlock, to join all threads (needs to be done after notify_all)
timer_thread_.join ();
}
boost::signals2::connection
pcl::TimeTrigger::registerCallback (const callback_type& callback)
{
- boost::unique_lock<boost::mutex> lock (condition_mutex_);
return (callbacks_.connect (callback));
}
void
pcl::TimeTrigger::thread_function ()
{
- static double time = 0;
- while (!quit_)
+ double time = 0;
+ while (true)
{
time = getTime ();
boost::unique_lock<boost::mutex> lock (condition_mutex_);
+ if(quit_)
+ break;
if (!running_)
condition_.wait (lock); // wait util start is called or destructor is called
else
{
callbacks_();
double rest = interval_ + time - getTime ();
+#if defined(BOOST_HAS_WINTHREADS) && (BOOST_VERSION < 105500)
+ //infinite timed_wait bug: https://svn.boost.org/trac/boost/ticket/9079
if (rest > 0.0) // without a deadlock is possible, until notify() is called
condition_.timed_wait (lock, boost::posix_time::microseconds (static_cast<int64_t> ((rest * 1000000))));
+#else
+ condition_.timed_wait (lock, boost::posix_time::microseconds (static_cast<int64_t> ((rest * 1000000))));
+#endif
}
}
}
--- /dev/null
+set(SUBSYS_NAME cuda)
+set(SUBSYS_DESC "Point cloud CUDA libraries")
+set(SUBSYS_DEPS )
+
+OPTION(BUILD_CUDA "Build the CUDA-related subsystems" ${DEFAULT})
+
+if(BUILD_CUDA AND CUDA_FOUND)
+
+ if(CMAKE_COMPILER_IS_GNUCXX)
+ string(REPLACE "-Wold-style-cast" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+ string(REPLACE "-Wno-invalid-offsetof" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+ SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-conversion -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function")
+ if (GCC_VERSION VERSION_GREATER 4.3)
+ SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-unused-but-set-variable")
+ endif()
+ endif()
+
+ collect_subproject_directory_names("${CMAKE_CURRENT_SOURCE_DIR}" "CMakeLists.txt" PCL_CUDA_MODULES_NAMES PCL_CUDA_MODULES_DIRS)
+ set(PCL_CUDA_MODULES_NAMES_UNSORTED ${PCL_CUDA_MODULES_NAMES})
+ topological_sort(PCL_CUDA_MODULES_NAMES PCL_ _DEPENDS)
+ sort_relative(PCL_CUDA_MODULES_NAMES_UNSORTED PCL_CUDA_MODULES_NAMES PCL_CUDA_MODULES_DIRS)
+ foreach(subdir ${PCL_CUDA_MODULES_DIRS})
+ add_subdirectory("${CMAKE_CURRENT_SOURCE_DIR}/${subdir}")
+ endforeach(subdir)
+endif()
+
--- /dev/null
+set(SUBSYS_NAME cuda_apps)
+set(SUBSYS_DESC "Point cloud library CUDA apps")
+set(SUBSYS_DEPS cuda_common cuda_io cuda_features cuda_segmentation cuda_sample_consensus common io visualization geometry)
+
+# ---[ Point Cloud Library - pcl/cuda/apps
+# Find VTK
+if(NOT VTK_FOUND)
+ set(DEFAULT FALSE)
+ set(REASON "VTK was not found.")
+else(NOT VTK_FOUND)
+ set(DEFAULT TRUE)
+ set(REASON)
+ message(STATUS "VTK found (include: ${VTK_INCLUDE_DIRS}, lib: ${VTK_LIBRARY_DIRS})")
+ include("${VTK_USE_FILE}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
+endif(NOT VTK_FOUND)
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}")
+mark_as_advanced("BUILD_${SUBSYS_NAME}")
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS})
+
+if(build)
+ if(VTK_FOUND AND WITH_OPENNI)
+
+ PCL_CUDA_ADD_EXECUTABLE(kinect_viewer_cuda "${SUBSYS_NAME}" src/kinect_viewer_cuda.cpp)
+ target_link_libraries (kinect_viewer_cuda pcl_io pcl_cuda_io pcl_visualization pcl_common pcl_kdtree)
+
+ endif()
+endif(build)
+
+
+
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <ros/ros.h>
+#include <pcl/PCLPointCloud2.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <pcl_cuda/io/disparity_to_cloud.h>
+#include <pcl_cuda/sample_consensus/sac_model_plane.h>
+#include <pcl_cuda/sample_consensus/ransac.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl/point_types.h>
+
+using namespace message_filters;
+using namespace pcl;
+using namespace pcl_cuda;
+
+DisparityToCloud d2c;
+ros::Publisher pub;
+
+struct EventHelper
+{
+ void
+ callback (const pcl::PCLImage::ConstPtr &depth,
+ const pcl::PCLImage::ConstPtr &rgb,
+ const pcl::CameraInfo::ConstPtr &info)
+ {
+ //typedef pcl_cuda::SampleConsensusModel<pcl_cuda::Host>::Indices Indices;
+
+ //pcl_cuda::PointCloudAOS<pcl_cuda::Host>::Ptr data (new pcl_cuda::PointCloudAOS<pcl_cuda::Host>);
+ PointCloudAOS<Device>::Ptr data;
+ d2c.callback (depth, rgb, info, data);
+
+ SampleConsensusModelPlane<Device>::Ptr sac_model (new SampleConsensusModelPlane<Device> (data));
+// SampleConsensusModelPlane<Host>::Ptr sac_model (new pcl_cuda::SampleConsensusModelPlane<pcl_cuda::Host> (data));
+ RandomSampleConsensus<Device> sac (sac_model);
+ sac.setDistanceThreshold (0.05);
+
+ if (!sac.computeModel (2))
+ std::cerr << "Failed to compute model" << std::endl;
+
+ // Convert data from Thrust/HOST to PCL
+ pcl::PointCloud<pcl::PointXYZRGB> output;
+// pcl_cuda::toPCL (*data, output);
+
+ output.header.stamp = ros::Time::now ();
+ pub.publish (output);
+ }
+};
+
+int
+main (int argc, char **argv)
+{
+ ros::init (argc, argv, "disparity_to_cloud");
+ ros::NodeHandle nh;
+
+ // Prepare output
+ pub = nh.advertise<PCLPointCloud2>("output", 1);
+
+ // Subscribe to topics
+ Synchronizer<sync_policies::ApproximateTime<PCLImage, PCLImage, CameraInfo> > sync_rgb (30);
+ Synchronizer<sync_policies::ApproximateTime<PCLImage, CameraInfo> > sync (30);
+ Subscriber<PCLImage> sub_depth, sub_rgb;
+ Subscriber<CameraInfo> sub_info;
+ sub_depth.subscribe (nh, "/camera/depth/image", 30);
+ sub_rgb.subscribe (nh, "/camera/rgb/image_color", 30);
+ sub_info.subscribe (nh, "/camera/rgb/camera_info", 120);
+ //sub_info.subscribe (nh, "/camera/depth/camera_info", 120);
+
+ EventHelper h;
+
+ if (argc > 1 && atoi (argv[1]) == 1)
+ {
+ ROS_INFO ("[disparity_to_cloud] Using RGB to color the points.");
+ sync_rgb.connectInput (sub_depth, sub_rgb, sub_info);
+ //sync_rgb.registerCallback (bind (&pcl_cuda::DisparityToCloud::callback, k, _1, _2, _3));
+ sync_rgb.registerCallback (boost::bind (&EventHelper::callback, &h, _1, _2, _3));
+ }
+ else
+ {
+ sync.connectInput (sub_depth, sub_info);
+ //sync.registerCallback (bind (&pcl_cuda::DisparityToCloud::callback, k, _1, PCLImageConstPtr (), _2));
+ sync.registerCallback (boost::bind (&EventHelper::callback, &h, _1, PCLImageConstPtr (), _2));
+ }
+
+ // Do this indefinitely
+ ros::spin ();
+
+ return (0);
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <pcl_cuda/io/cloud_to_pcl.h>
+#include <pcl_cuda/io/debayering.h>
+
+#include <pcl/io/kinect_grabber.h>
+#include <boost/shared_ptr.hpp>
+#include <iostream>
+#include <opencv2/opencv.hpp>
+#include <pcl_cuda/time_cpu.h>
+
+class SimpleKinectTool
+{
+ public:
+ //SimpleKinectTool () : viewer ("KinectGrabber"), init_(false) {}
+
+ void cloud_cb_ (const boost::shared_ptr<openni_wrapper::Image>& image)
+ {
+ thrust::host_vector<pcl_cuda::OpenNIRGB> rgb_image(image->getWidth () * image->getHeight ());
+ cv::Mat cv_image( image->getHeight (), image->getWidth (), CV_8UC3 );
+ {
+ pcl::ScopeTime t ("computeBilinear+memcpy");
+ debayering.computeBilinear (image, rgb_image);
+ //debayering.computeEdgeAware (image, rgb_image);
+ // now fill image and show!
+ pcl::ScopeTime t2 ("memcpy");
+ memcpy (cv_image.data, &rgb_image[0], image->getWidth () * image->getHeight () * 3);
+ }
+ imshow ("test", cv_image);
+ }
+
+ void run (const std::string& device_id)
+ {
+ cv::namedWindow("test", CV_WINDOW_AUTOSIZE);
+ pcl::Grabber* interface = new pcl::OpenNIGrabber(device_id);
+
+ boost::function<void (const boost::shared_ptr<openni_wrapper::Image>& image)> f = boost::bind (&SimpleKinectTool::cloud_cb_, this, _1);
+
+ boost::signals2::connection c = interface->registerCallback (f);
+
+ interface->start ();
+
+ while (true)
+ {
+ //sleep (1);
+ cv::waitKey(10);
+ }
+
+ interface->stop ();
+ }
+
+ pcl_cuda::Debayering<pcl_cuda::Host> debayering;
+ boost::mutex mutex_;
+ bool init_;
+};
+
+int main (int argc, char** argv)
+{
+ std::string device_id = "#1";
+ if (argc == 2)
+ {
+ device_id = argv[1];
+ }
+ SimpleKinectTool v;
+ v.run (device_id);
+ return 0;
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <pcl_cuda/io/cloud_to_pcl.h>
+#include <pcl_cuda/io/disparity_to_cloud.h>
+#include <pcl_cuda/sample_consensus/sac_model_plane.h>
+#include <pcl_cuda/sample_consensus/ransac.h>
+
+#include <pcl/io/openni_grabber.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_cuda/time_cpu.h>
+#include <boost/shared_ptr.hpp>
+#include <pcl/visualization/cloud_viewer.h>
+#include <iostream>
+
+class SimpleKinectTool
+{
+ public:
+ SimpleKinectTool () : viewer ("KinectGrabber"), init_(false) {}
+
+ void cloud_cb_ (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float constant)
+ {
+ pcl_cuda::PointCloudAOS<pcl_cuda::Device>::Ptr data;
+ {
+ pcl::ScopeTime t ("time:");
+ d2c.compute<pcl_cuda::Device> (depth_image, image, constant, data);
+ }
+ //d2c.callback (depth_image, constant, *data);
+
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl_cuda::toPCL (*data, *output);
+
+ viewer.showCloud (output);
+
+ }
+
+ void run (const std::string& device_id)
+ {
+ pcl::Grabber* interface = new pcl::OpenNIGrabber(device_id);
+
+ boost::function<void (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float)> f = boost::bind (&SimpleKinectTool::cloud_cb_, this, _1, _2, _3);
+
+ boost::signals2::connection c = interface->registerCallback (f);
+
+ //viewer.runOnVisualizationThread (fn, "viz_cb");
+ interface->start ();
+
+ while (true)
+ {
+ sleep (1);
+ }
+
+ interface->stop ();
+ }
+
+ pcl_cuda::DisparityToCloud d2c;
+ pcl::visualization::CloudViewer viewer;
+ boost::mutex mutex_;
+ bool init_;
+};
+
+int main (int argc, char** argv)
+{
+ std::string device_id = "#1";
+ if (argc == 2)
+ {
+ device_id = argv[1];
+ }
+ SimpleKinectTool v;
+ v.run (device_id);
+ return 0;
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <boost/filesystem.hpp>
+#include <boost/shared_ptr.hpp>
+
+#include <pcl_cuda/time_cpu.h>
+#include <pcl_cuda/time_gpu.h>
+#include <pcl_cuda/io/cloud_to_pcl.h>
+#include <pcl_cuda/io/extract_indices.h>
+#include <pcl_cuda/io/disparity_to_cloud.h>
+
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/pcd_grabber.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/visualization/point_cloud_handlers.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+#include <pcl/common/transform.h>
+
+#include <iostream>
+
+#include <pcl_cuda/sample_consensus/sac_model_1point_plane.h>
+#include <pcl_cuda/sample_consensus/multi_ransac.h>
+#include <pcl_cuda/segmentation/connected_components.h>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "opencv2/opencv.hpp"
+#include "opencv2/gpu/gpu.hpp"
+
+using namespace pcl_cuda;
+
+template <template <typename> class Storage>
+struct ImageType
+{
+ typedef void type;
+};
+
+template <>
+struct ImageType<pcl_cuda::Device>
+{
+ typedef cv::gpu::GpuMat type;
+};
+
+template <>
+struct ImageType<pcl_cuda::Host>
+{
+ typedef cv::Mat type;
+};
+
+class MultiRansac
+{
+ public:
+ MultiRansac () : viewer ("PCL CUDA - MultiSac"), new_cloud(false), use_viewer(true) {}
+
+ void logo_cb (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr & cloud)
+ {
+ //logo_cloud_ = cloud;
+ //viewer.showCloud (logo_cloud_, "logo");
+ std::cerr << "got logo" << std::endl;
+ }
+
+ void viz_cb (pcl::visualization::PCLVisualizer& viz)
+ {
+ static bool first_time = true;
+ boost::mutex::scoped_lock l(m_mutex);
+ if (new_cloud)
+ {
+ typedef pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal> ColorHandler;
+
+ ColorHandler Color_handler (normal_cloud);
+ if (!first_time)
+ {
+ viz.removePointCloud ("normalcloud");
+ viz.removePointCloud ("cloud");
+ }
+ else
+ first_time = false;
+
+ viz.addPointCloudNormals<pcl::PointXYZRGBNormal> (normal_cloud, 200, 0.1, "normalcloud");
+ viz.addPointCloud<pcl::PointXYZRGBNormal> (normal_cloud, Color_handler, std::string("cloud"), 0);
+ new_cloud = false;
+ }
+ }
+
+ template <template <typename> class Storage> void
+ cloud_cb (const boost::shared_ptr<openni_wrapper::Image>& image,
+ const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ float constant)
+ {
+ static unsigned count = 0;
+ static double last = pcl::getTime ();
+ double now = pcl::getTime ();
+ if (++count == 30 || (now - last) > 5)
+ {
+ std::cerr << "Average framerate: " << double(count)/double(now - last) << " Hz" << std::endl;
+ count = 0;
+ last = now;
+ }
+ //static unsigned int nr_planes_last_time = 0;
+
+ std::cerr << "received callback" << std::endl;
+ typename PointCloudAOS<Storage>::Ptr data;
+ typename SampleConsensusModel1PointPlane<Storage>::Ptr sac_model;
+ {
+ pcl::ScopeTime timer ("All: ");
+ // Compute the PointCloud on the device
+ d2c.compute<Storage> (depth_image, image, constant, data, true, 2);
+
+ {
+ pcl::ScopeTime t ("creating sac_model");
+ sac_model.reset (new SampleConsensusModel1PointPlane<Storage> (data));
+ }
+ MultiRandomSampleConsensus<Storage> sac (sac_model);
+ sac.setMinimumCoverage (0.90); // at least 95% points should be explained by planes
+ sac.setMaximumBatches (5);
+ sac.setIerationsPerBatch (1000);
+ sac.setDistanceThreshold (0.05);
+
+ {
+ pcl::ScopeTime timer ("computeModel: ");
+ if (!sac.computeModel (0))
+ {
+ std::cerr << "Failed to compute model" << std::endl;
+ }
+ else
+ {
+ if (use_viewer)
+ {
+ std::cerr << "getting inliers.. ";
+
+ std::vector<typename SampleConsensusModel1PointPlane<Storage>::IndicesPtr> planes;
+// thrust::host_vector<int> regions_host = region_mask;
+// std::copy (regions_host.begin (), regions_host.end(), std::ostream_iterator<int>(std::cerr, " "));
+ {
+ ScopeTime t ("retrieving inliers");
+ planes = sac.getAllInliers ();
+ }
+
+ typename Storage<int>::type region_mask;
+ markInliers<Storage> (data, region_mask, planes);
+ std::cerr << "image_size: " << data->width << " x " << data->height << " = " << region_mask.size () << std::endl;
+
+ typename StoragePointer<Storage,uchar>::type ptr = StorageAllocator<Storage,uchar>::alloc (data->width * data->height);
+
+ createIndicesImage<Storage> (ptr, region_mask);
+
+ typename ImageType<Storage>::type dst (data->height, data->width, CV_8UC1, thrust::raw_pointer_cast<char4>(ptr), data->width);
+ cv::imshow ("Result", cv::Mat(dst));
+ cv::waitKey (2);
+
+ std::vector<int> planes_inlier_counts = sac.getAllInlierCounts ();
+ std::vector<float4> coeffs = sac.getAllModelCoefficients ();
+ std::vector<float3> centroids = sac.getAllModelCentroids ();
+ std::cerr << "Found " << planes_inlier_counts.size () << " planes" << std::endl;
+ int best_plane = 0;
+ int best_plane_inliers_count = -1;
+
+ for (unsigned int i = 0; i < planes.size (); i++)
+ {
+ if (planes_inlier_counts[i] > best_plane_inliers_count)
+ {
+ best_plane = i;
+ best_plane_inliers_count = planes_inlier_counts[i];
+ }
+
+ typename SampleConsensusModel1PointPlane<Storage>::IndicesPtr inliers_stencil;
+ inliers_stencil = planes[i];//sac.getInliersStencil ();
+
+ OpenNIRGB color;
+ double trand = 255 / (RAND_MAX + 1.0);
+ //int idx = (int)(rand () * trand);
+
+ color.r = (int)(rand () * trand);
+ color.g = (int)(rand () * trand);
+ color.b = (int)(rand () * trand);
+ {
+ ScopeTime t ("coloring planes");
+ colorIndices<Storage> (data, inliers_stencil, color);
+ }
+ }
+ }
+
+ //{
+ // ScopeTime t ("copying & transforming logo");
+ //for (unsigned int i = 0; i < planes.size (); i++)
+ //{
+// // if (i == best_plane) // assume first plane is biggest plane
+ // {
+ // if (logo_cloud_)
+ // {
+ // boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > transformed_logo (new pcl::PointCloud<pcl::PointXYZRGB>);
+ // Eigen::Affine3f transformation;
+
+ // Eigen::Vector3f plane_normal (coeffs[i].x, coeffs[i].y, coeffs[i].z);
+ // plane_normal.normalize ();
+ // if (plane_normal.dot (Eigen::Vector3f::Zero()) - coeffs[i].w > 0)
+ // plane_normal = -plane_normal;
+
+ // Eigen::Vector3f logo_normal (0,0,-1);
+
+ // Eigen::Vector3f trans (Eigen::Vector3f(centroids[i].x, centroids[i].y, centroids[i].z) * 0.97);
+ // Eigen::AngleAxisf rot (acos (logo_normal.dot (plane_normal)), logo_normal.cross (plane_normal).normalized ());
+
+ // transformation = Eigen::Affine3f::Identity();// = ....;
+ // transformation.translate (trans);// = ....;
+ // transformation.rotate (rot);// = ....;
+ // transformation.scale (0.001 * sqrt (planes_inlier_counts[i]));// = ....;
+
+ // std::cerr << "Plane centroid " << centroids[i].x << " " << centroids[i].y << " " << centroids[i].z << std::endl;
+ // pcl::getTransformedPointCloud<pcl::PointCloud<pcl::PointXYZRGB> > (*logo_cloud_, transformation, *transformed_logo);
+
+ // std::stringstream ss;
+ // ss << "logo" << i;
+ // //viewer.showCloud (transformed_logo, ss.str ());
+ // }
+ // }
+ //}
+ //}
+
+ //boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > dummy_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+ //for (unsigned int i = planes.size (); i < nr_planes_last_time; i++)
+ //{
+ // std::stringstream ss;
+ // ss << "logo" << i;
+ // viewer.showCloud (dummy_cloud, ss.str ());
+ //}
+ //nr_planes_last_time = planes.size ();
+ }
+ }
+
+ }
+ if (use_viewer)
+ {
+ typename Storage<float4>::type normals = sac_model->getNormals ();
+
+ boost::mutex::scoped_lock l(m_mutex);
+ normal_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+ pcl_cuda::toPCL (*data, normals, *normal_cloud);
+ new_cloud = true;
+ }
+
+
+ //pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);
+ //pcl_cuda::toPCL (*data, *output);
+ //viewer.showCloud (output);
+ }
+
+ void
+ run (bool use_device, bool use_viewer)
+ {
+ this->use_viewer = use_viewer;
+ //cudaDeviceSetCacheConfig (cudaFuncCachePreferL1);
+ pcl::Grabber* interface = new pcl::OpenNIGrabber();
+
+ boost::signals2::connection c;
+ if (use_device)
+ {
+ std::cerr << "[RANSAC] Using GPU..." << std::endl;
+ boost::function<void (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float)> f = boost::bind (&MultiRansac::cloud_cb<pcl_cuda::Device>, this, _1, _2, _3);
+ c = interface->registerCallback (f);
+ }
+ else
+ {
+ std::cerr << "[RANSAC] Using CPU..." << std::endl;
+ boost::function<void (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float)> f = boost::bind (&MultiRansac::cloud_cb<pcl_cuda::Host>, this, _1, _2, _3);
+ c = interface->registerCallback (f);
+ }
+
+ if (use_viewer)
+ viewer.runOnVisualizationThread (boost::bind(&MultiRansac::viz_cb, this, _1), "viz_cb");
+
+ interface->start ();
+
+ //--------------------- load pcl logo file
+ //pcl::Grabber* filegrabber = 0;
+
+ //float frames_per_second = 1;
+ //bool repeat = false;
+
+ //std::string path = "./pcl_logo.pcd";
+ //if (path != "" && boost::filesystem::exists (path))
+ //{
+ // filegrabber = new pcl::PCDGrabber<pcl::PointXYZRGB > (path, frames_per_second, repeat);
+ //}
+ //else
+ // std::cerr << "did not find file" << std::endl;
+ //
+ //boost::function<void(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&) > f = boost::bind (&MultiRansac::logo_cb, this, _1);
+ //boost::signals2::connection c1 = filegrabber->registerCallback (f);
+
+ //filegrabber->start ();
+ //------- END --------- load pcl logo file
+ //
+ while (!viewer.wasStopped ())
+ {
+ sleep (1);
+ }
+
+ //filegrabber->stop ();
+ interface->stop ();
+ }
+
+ pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr logo_cloud_;
+ pcl_cuda::DisparityToCloud d2c;
+ pcl::visualization::CloudViewer viewer;
+
+ boost::mutex::mutex m_mutex;
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr normal_cloud;
+ bool new_cloud;
+ bool use_viewer;
+};
+
+int
+main (int argc, char **argv)
+{
+ sleep (1);
+ bool use_device = false;
+ bool use_viewer = true;
+ if (argc >= 2)
+ use_device = true;
+ if (argc >= 3)
+ use_viewer = false;
+ MultiRansac v;
+ v.run (use_device, use_viewer);
+ return 0;
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include "opencv2/opencv.hpp"
+#include "opencv2/gpu/gpu.hpp"
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/pcl_macros.h>
+
+#include <boost/shared_ptr.hpp>
+
+#include <pcl/cuda/features/normal_3d.h>
+#include <pcl/cuda/time_cpu.h>
+#include <pcl/cuda/time_gpu.h>
+#include <pcl/cuda/io/cloud_to_pcl.h>
+#include <pcl/cuda/io/extract_indices.h>
+#include <pcl/cuda/io/disparity_to_cloud.h>
+#include <pcl/cuda/io/host_device.h>
+
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/pcd_grabber.h>
+#include <pcl/visualization/cloud_viewer.h>
+
+#include <iostream>
+
+using namespace pcl::cuda;
+
+class NormalEstimation
+{
+ public:
+ NormalEstimation () : viewer ("PCL CUDA - NormalEstimation"), new_cloud(false), go_on(true) {}
+
+ void viz_cb (pcl::visualization::PCLVisualizer& viz)
+ {
+ static bool first_time = true;
+ double psize = 1.0,opacity = 1.0,linesize =1.0;
+ std::string cloud_name ("cloud");
+ boost::mutex::scoped_lock l(m_mutex);
+ if (new_cloud)
+ {
+ //typedef pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal> ColorHandler;
+ typedef pcl::visualization::PointCloudColorHandlerGenericField <pcl::PointXYZRGBNormal> ColorHandler;
+ //ColorHandler Color_handler (normal_cloud);
+ ColorHandler Color_handler (normal_cloud,"curvature");
+ if (!first_time)
+ {
+ viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, linesize, cloud_name);
+ viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, cloud_name);
+ viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize, cloud_name);
+ //viz.removePointCloud ("normalcloud");
+ viz.removePointCloud ("cloud");
+ }
+ else
+ first_time = false;
+
+ //viz.addPointCloudNormals<pcl::PointXYZRGBNormal> (normal_cloud, 139, 0.1, "normalcloud");
+ viz.addPointCloud<pcl::PointXYZRGBNormal> (normal_cloud, Color_handler, std::string("cloud"), 0);
+ viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, linesize, cloud_name);
+ viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, cloud_name);
+ viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize, cloud_name);
+ new_cloud = false;
+ }
+ }
+
+ template <template <typename> class Storage> void
+ file_cloud_cb (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud)
+ {
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);
+ PointCloudAOS<Host> data_host;
+ data_host.points.resize (cloud->points.size());
+ for (size_t i = 0; i < cloud->points.size (); ++i)
+ {
+ PointXYZRGB pt;
+ pt.x = cloud->points[i].x;
+ pt.y = cloud->points[i].y;
+ pt.z = cloud->points[i].z;
+ // Pack RGB into a float
+ pt.rgb = *(float*)(&cloud->points[i].rgb);
+ data_host.points[i] = pt;
+ }
+ data_host.width = cloud->width;
+ data_host.height = cloud->height;
+ data_host.is_dense = cloud->is_dense;
+ typename PointCloudAOS<Storage>::Ptr data = toStorage<Host, Storage> (data_host);
+
+ // we got a cloud in device..
+
+ boost::shared_ptr<typename Storage<float4>::type> normals;
+ float focallength = 580/2.0;
+ {
+ ScopeTimeCPU time ("Normal Estimation");
+ normals = computePointNormals<Storage, typename PointIterator<Storage,PointXYZRGB>::type > (data->points.begin (), data->points.end (), focallength, data, 0.05, 30);
+ }
+ go_on = false;
+
+ boost::mutex::scoped_lock l(m_mutex);
+ normal_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+ toPCL (*data, *normals, *normal_cloud);
+ new_cloud = true;
+ }
+
+ template <template <typename> class Storage> void
+ cloud_cb (const boost::shared_ptr<openni_wrapper::Image>& image,
+ const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ float constant)
+ {
+ static int smoothing_nr_iterations = 10;
+ static int smoothing_filter_size = 2;
+ cv::namedWindow("Parameters", CV_WINDOW_NORMAL);
+ cvCreateTrackbar ("iterations", "Parameters", &smoothing_nr_iterations, 50, NULL);
+ cvCreateTrackbar ("filter_size", "Parameters", &smoothing_filter_size, 10, NULL);
+ cv::waitKey (2);
+
+ static unsigned count = 0;
+ static double last = getTime ();
+ double now = getTime ();
+ if (++count == 30 || (now - last) > 5)
+ {
+ std::cout << "Average framerate: " << double(count)/double(now - last) << " Hz..........................................." << std::endl;
+ count = 0;
+ last = now;
+ }
+
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);
+ typename PointCloudAOS<Storage>::Ptr data;
+
+ ScopeTimeCPU timer ("All: ");
+ // Compute the PointCloud on the device
+ d2c.compute<Storage> (depth_image, image, constant, data, false, 1, smoothing_nr_iterations, smoothing_filter_size);
+ //d2c.compute<Storage> (depth_image, image, constant, data, true, 2);
+
+ boost::shared_ptr<typename Storage<float4>::type> normals;
+ float focallength = 580/2.0;
+ {
+ ScopeTimeCPU time ("Normal Estimation");
+ normals = computeFastPointNormals<Storage> (data);
+ //normals = computePointNormals<Storage, typename PointIterator<Storage,PointXYZRGB>::type > (data->points.begin (), data->points.end (), focallength, data, 0.05, 30);
+ }
+
+ boost::mutex::scoped_lock l(m_mutex);
+ normal_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+ toPCL (*data, *normals, *normal_cloud);
+ new_cloud = true;
+ }
+
+ void
+ run (bool use_device, bool use_file)
+ {
+ if (use_file)
+ {
+ pcl::Grabber* filegrabber = 0;
+
+ float frames_per_second = 1;
+ bool repeat = false;
+
+ std::string path = "./frame_0.pcd";
+ filegrabber = new pcl::PCDGrabber<pcl::PointXYZRGB > (path, frames_per_second, repeat);
+
+ if (use_device)
+ {
+ std::cerr << "[NormalEstimation] Using GPU..." << std::endl;
+ boost::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f = boost::bind (&NormalEstimation::file_cloud_cb<Device>, this, _1);
+ filegrabber->registerCallback (f);
+ }
+ else
+ {
+ std::cerr << "[NormalEstimation] Using CPU..." << std::endl;
+ boost::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f = boost::bind (&NormalEstimation::file_cloud_cb<Host>, this, _1);
+ filegrabber->registerCallback (f);
+ }
+
+ filegrabber->start ();
+ while (go_on)//!viewer.wasStopped () && go_on)
+ {
+ pcl_sleep (1);
+ }
+ filegrabber->stop ();
+ }
+ else
+ {
+ pcl::Grabber* grabber = new pcl::OpenNIGrabber();
+
+ boost::signals2::connection c;
+ if (use_device)
+ {
+ std::cerr << "[NormalEstimation] Using GPU..." << std::endl;
+ boost::function<void (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float)> f = boost::bind (&NormalEstimation::cloud_cb<Device>, this, _1, _2, _3);
+ c = grabber->registerCallback (f);
+ }
+ else
+ {
+ std::cerr << "[NormalEstimation] Using CPU..." << std::endl;
+ boost::function<void (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float)> f = boost::bind (&NormalEstimation::cloud_cb<Host>, this, _1, _2, _3);
+ c = grabber->registerCallback (f);
+ }
+
+ viewer.runOnVisualizationThread (boost::bind(&NormalEstimation::viz_cb, this, _1), "viz_cb");
+
+ grabber->start ();
+
+ while (!viewer.wasStopped ())
+ {
+ pcl_sleep (1);
+ }
+
+ grabber->stop ();
+ }
+ }
+
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr normal_cloud;
+ DisparityToCloud d2c;
+ pcl::visualization::CloudViewer viewer;
+ boost::mutex m_mutex;
+ bool new_cloud, go_on;
+};
+
+int
+main (int argc, char **argv)
+{
+ bool use_device = false;
+ bool use_file = false;
+ if (argc >= 2)
+ use_device = true;
+ if (argc >= 3)
+ use_file = true;
+ NormalEstimation v;
+ v.run (use_device, use_file);
+ return 0;
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include "opencv2/opencv.hpp"
+#include "opencv2/gpu/gpu.hpp"
+
+// pcl::cuda includes
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/pcl_macros.h>
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/pcd_grabber.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/visualization/point_cloud_handlers.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+// pcl::cuda includes
+#include <pcl/cuda/time_cpu.h>
+#include <pcl/cuda/time_gpu.h>
+#include <pcl/cuda/io/cloud_to_pcl.h>
+#include <pcl/cuda/io/extract_indices.h>
+#include <pcl/cuda/io/disparity_to_cloud.h>
+#include <pcl/cuda/sample_consensus/sac_model_1point_plane.h>
+#include <pcl/cuda/sample_consensus/multi_ransac.h>
+#include <pcl/cuda/segmentation/connected_components.h>
+#include <pcl/cuda/features/normal_3d.h>
+
+#include <iostream>
+
+#include <boost/filesystem.hpp>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+using namespace pcl::cuda;
+
+#undef interface
+
+class MultiRansac
+{
+ public:
+ MultiRansac () : viewer ("PCL CUDA - MultiSac"), new_cloud(false), use_viewer(true) {}
+
+ void logo_cb (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr & cloud)
+ {
+ //logo_cloud_ = cloud;
+ //viewer.showCloud (logo_cloud_, "logo");
+ std::cerr << "got logo" << std::endl;
+ }
+
+ void viz_cb (pcl::visualization::PCLVisualizer& viz)
+ {
+ static bool first_time = true;
+ boost::mutex::scoped_lock l(m_mutex);
+ if (new_cloud)
+ {
+ double psize = 1.0,opacity = 1.0,linesize =1.0;
+ std::string cloud_name ("cloud");
+ typedef pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal> ColorHandler;
+
+ ColorHandler Color_handler (normal_cloud);
+ if (!first_time)
+ {
+ viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, linesize, cloud_name);
+ viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, cloud_name);
+ viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize, cloud_name);
+ //viz.removePointCloud ("normalcloud");
+ viz.removePointCloud ("cloud");
+ }
+ else
+ first_time = false;
+
+ //viz.addPointCloudNormals<pcl::PointXYZRGBNormal> (normal_cloud, 139, 0.1, "normalcloud");
+ viz.addPointCloud<pcl::PointXYZRGBNormal> (normal_cloud, Color_handler, std::string("cloud"), 0);
+ viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, linesize, cloud_name);
+ viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, cloud_name);
+ viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize, cloud_name);
+ new_cloud = false;
+ }
+ }
+
+ template <template <typename> class Storage> void
+ cloud_cb (const boost::shared_ptr<openni_wrapper::Image>& image,
+ const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ float constant)
+ {
+ static unsigned count = 0;
+ static double last = pcl::cuda::getTime ();
+ double now = pcl::cuda::getTime ();
+ if (++count == 30 || (now - last) > 5)
+ {
+ std::cerr << "Average framerate: " << double(count)/double(now - last) << " Hz" << std::endl;
+ count = 0;
+ last = now;
+ }
+ //static unsigned int nr_planes_last_time = 0;
+
+ std::cerr << "received callback" << std::endl;
+ typename PointCloudAOS<Storage>::Ptr data;
+ typename SampleConsensusModel1PointPlane<Storage>::Ptr sac_model;
+ {
+ ScopeTimeCPU timer ("All: ");
+ static int threshold_x100 = 25;
+ static int smoothing_nr_iterations = 10;
+ static int smoothing_filter_size = 2;
+ static int enable_coloring = 1;
+
+ // Compute the PointCloud on the device
+ //d2c.compute<Storage> (depth_image, image, constant, data, true, 2);
+ d2c.compute<Storage> (depth_image, image, constant, data, true, 2, smoothing_nr_iterations, smoothing_filter_size);
+
+ // Compute normals
+ boost::shared_ptr<typename Storage<float4>::type> normals;
+ {
+ ScopeTimeCPU time ("Normal Estimation");
+ //normals = computeFastPointNormals<Storage> (data);
+ float focallength = 580/2.0;
+ //normals = computePointNormals<Storage> (data->points.begin (), data->points.end (), focallength, data, 0.11f, 36);
+ normals = computeFastPointNormals<Storage> (data);
+ }
+
+ // Create sac_model
+ {
+ ScopeTimeCPU t ("creating sac_model");
+ sac_model.reset (new SampleConsensusModel1PointPlane<Storage> (data));
+ }
+ sac_model->setNormals (normals);
+
+ MultiRandomSampleConsensus<Storage> sac (sac_model);
+ sac.setMinimumCoverage (0.90); // at least 95% points should be explained by planes
+ sac.setMaximumBatches (5);
+ sac.setIerationsPerBatch (1000);
+ sac.setDistanceThreshold (threshold_x100 / 100.0);
+
+ {
+ ScopeTimeCPU timer ("computeModel: ");
+ if (!sac.computeModel (0))
+ {
+ std::cerr << "Failed to compute model" << std::endl;
+ }
+ else
+ {
+ if (use_viewer)
+ {
+ std::cerr << "getting inliers.. ";
+
+ std::vector<typename SampleConsensusModel1PointPlane<Storage>::IndicesPtr> planes;
+ typename Storage<int>::type region_mask;
+ markInliers<Storage> (data, region_mask, planes);
+ thrust::host_vector<int> regions_host;
+ std::copy (regions_host.begin (), regions_host.end(), std::ostream_iterator<int>(std::cerr, " "));
+ {
+ ScopeTimeCPU t ("retrieving inliers");
+ planes = sac.getAllInliers ();
+ }
+ std::vector<int> planes_inlier_counts = sac.getAllInlierCounts ();
+ std::vector<float4> coeffs = sac.getAllModelCoefficients ();
+ std::vector<float3> centroids = sac.getAllModelCentroids ();
+ std::cerr << "Found " << planes_inlier_counts.size () << " planes" << std::endl;
+ int best_plane = 0;
+ int best_plane_inliers_count = -1;
+
+ for (unsigned int i = 0; i < planes.size (); i++)
+ {
+ if (planes_inlier_counts[i] > best_plane_inliers_count)
+ {
+ best_plane = i;
+ best_plane_inliers_count = planes_inlier_counts[i];
+ }
+
+ typename SampleConsensusModel1PointPlane<Storage>::IndicesPtr inliers_stencil;
+ inliers_stencil = planes[i];//sac.getInliersStencil ();
+
+ OpenNIRGB color;
+ double trand = 255 / (RAND_MAX + 1.0);
+ //int idx = (int)(rand () * trand);
+
+ color.r = (int)(rand () * trand);
+ color.g = (int)(rand () * trand);
+ color.b = (int)(rand () * trand);
+ if (enable_coloring)
+ {
+ ScopeTimeCPU t ("coloring planes");
+ colorIndices<Storage> (data, inliers_stencil, color);
+ }
+ }
+ }
+
+ //{
+ // ScopeTimeCPU t ("copying & transforming logo");
+ //for (unsigned int i = 0; i < planes.size (); i++)
+ //{
+// // if (i == best_plane) // assume first plane is biggest plane
+ // {
+ // if (logo_cloud_)
+ // {
+ // boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > transformed_logo (new pcl::PointCloud<pcl::PointXYZRGB>);
+ // Eigen::Affine3f transformation;
+
+ // Eigen::Vector3f plane_normal (coeffs[i].x, coeffs[i].y, coeffs[i].z);
+ // plane_normal.normalize ();
+ // if (plane_normal.dot (Eigen::Vector3f::Zero()) - coeffs[i].w > 0)
+ // plane_normal = -plane_normal;
+
+ // Eigen::Vector3f logo_normal (0,0,-1);
+
+ // Eigen::Vector3f trans (Eigen::Vector3f(centroids[i].x, centroids[i].y, centroids[i].z) * 0.97);
+ // Eigen::AngleAxisf rot (acos (logo_normal.dot (plane_normal)), logo_normal.cross (plane_normal).normalized ());
+
+ // transformation = Eigen::Affine3f::Identity();// = ....;
+ // transformation.translate (trans);// = ....;
+ // transformation.rotate (rot);// = ....;
+ // transformation.scale (0.001 * sqrt (planes_inlier_counts[i]));// = ....;
+
+ // std::cerr << "Plane centroid " << centroids[i].x << " " << centroids[i].y << " " << centroids[i].z << std::endl;
+ // pcl::getTransformedPointCloud<pcl::PointCloud<pcl::PointXYZRGB> > (*logo_cloud_, transformation, *transformed_logo);
+
+ // std::stringstream ss;
+ // ss << "logo" << i;
+ // //viewer.showCloud (transformed_logo, ss.str ());
+ // }
+ // }
+ //}
+ //}
+
+ //boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > dummy_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
+ //for (unsigned int i = planes.size (); i < nr_planes_last_time; i++)
+ //{
+ // std::stringstream ss;
+ // ss << "logo" << i;
+ // viewer.showCloud (dummy_cloud, ss.str ());
+ //}
+ //nr_planes_last_time = planes.size ();
+ cv::namedWindow("Parameters", CV_WINDOW_NORMAL);
+ cvCreateTrackbar( "iterations", "Parameters", &smoothing_nr_iterations, 50, NULL);
+ cvCreateTrackbar( "filter_size", "Parameters", &smoothing_filter_size, 10, NULL);
+ cvCreateTrackbar( "enable_coloring", "Parameters", &enable_coloring, 1, NULL);
+ cvCreateTrackbar( "threshold", "Parameters", &threshold_x100, 100, NULL);
+ cv::waitKey (2);
+ }
+ }
+
+ }
+ if (use_viewer)
+ {
+ boost::shared_ptr<typename Storage<float4>::type> normals = sac_model->getNormals ();
+
+ boost::mutex::scoped_lock l(m_mutex);
+ normal_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+ toPCL (*data, *normals, *normal_cloud);
+ new_cloud = true;
+ }
+
+ //pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);
+ //toPCL (*data, *output);
+ //viewer.showCloud (output);
+ }
+
+ void
+ run (bool use_device, bool use_viewer)
+ {
+ this->use_viewer = use_viewer;
+ //cudaDeviceSetCacheConfig (cudaFuncCachePreferL1);
+ pcl::Grabber* interface = new pcl::OpenNIGrabber();
+
+ boost::signals2::connection c;
+ if (use_device)
+ {
+ std::cerr << "[RANSAC] Using GPU..." << std::endl;
+ boost::function<void (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float)> f = boost::bind (&MultiRansac::cloud_cb<Device>, this, _1, _2, _3);
+ c = interface->registerCallback (f);
+ }
+ else
+ {
+ std::cerr << "[RANSAC] Using CPU..." << std::endl;
+ boost::function<void (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float)> f = boost::bind (&MultiRansac::cloud_cb<Host>, this, _1, _2, _3);
+ c = interface->registerCallback (f);
+ }
+
+ if (use_viewer)
+ viewer.runOnVisualizationThread (boost::bind(&MultiRansac::viz_cb, this, _1), "viz_cb");
+
+ interface->start ();
+
+ //--------------------- load pcl logo file
+ //pcl::Grabber* filegrabber = 0;
+
+ //float frames_per_second = 1;
+ //bool repeat = false;
+
+ //std::string path = "./pcl_logo.pcd";
+ //if (path != "" && boost::filesystem::exists (path))
+ //{
+ // filegrabber = new pcl::PCDGrabber<pcl::PointXYZRGB > (path, frames_per_second, repeat);
+ //}
+ //else
+ // std::cerr << "did not find file" << std::endl;
+ //
+ //boost::function<void(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&) > f = boost::bind (&MultiRansac::logo_cb, this, _1);
+ //boost::signals2::connection c1 = filegrabber->registerCallback (f);
+
+ //filegrabber->start ();
+ //------- END --------- load pcl logo file
+ //
+ while (!viewer.wasStopped ())
+ {
+ pcl_sleep (1);
+ }
+
+ //filegrabber->stop ();
+ interface->stop ();
+ }
+
+ pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr logo_cloud_;
+ DisparityToCloud d2c;
+ pcl::visualization::CloudViewer viewer;
+
+ boost::mutex m_mutex;
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr normal_cloud;
+ bool new_cloud;
+ bool use_viewer;
+};
+
+int
+main (int argc, char **argv)
+{
+ pcl_sleep (1);
+ bool use_device = false;
+ bool use_viewer = true;
+ if (argc >= 2)
+ use_device = true;
+ if (argc >= 3)
+ use_viewer = false;
+ MultiRansac v;
+ v.run (use_device, use_viewer);
+ return 0;
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <pcl_cuda/time_cpu.h>
+#include <pcl_cuda/time_gpu.h>
+#include <pcl_cuda/io/cloud_to_pcl.h>
+#include <pcl_cuda/io/extract_indices.h>
+#include <pcl_cuda/io/disparity_to_cloud.h>
+#include <pcl_cuda/io/host_device.h>
+
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/pcd_grabber.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <boost/shared_ptr.hpp>
+#include <pcl/visualization/cloud_viewer.h>
+#include <iostream>
+#include <pcl_cuda/sample_consensus/sac_model_1point_plane.h>
+#include <pcl_cuda/sample_consensus/ransac.h>
+
+using namespace pcl_cuda;
+
+class SimpleKinectTool
+{
+ public:
+ SimpleKinectTool () : /*viewer ("KinectGrabber"),*/ go_on(true) {}
+
+ template <template <typename> class Storage> void
+ file_cloud_cb (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud)
+ {
+ pcl::ScopeTime ttt ("all");
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);
+ PointCloudAOS<Host> data_host;
+ data_host.points.resize (cloud->points.size());
+ for (size_t i = 0; i < cloud->points.size (); ++i)
+ {
+ PointXYZRGB pt;
+ pt.x = cloud->points[i].x;
+ pt.y = cloud->points[i].y;
+ pt.z = cloud->points[i].z;
+ // Pack RGB into a float
+ pt.rgb = *(float*)(&cloud->points[i].rgb);
+ data_host.points[i] = pt;
+ }
+ data_host.width = cloud->width;
+ data_host.height = cloud->height;
+ data_host.is_dense = cloud->is_dense;
+ typename PointCloudAOS<Storage>::Ptr data = toStorage<Host, Storage> (data_host);
+
+ typename SampleConsensusModel1PointPlane<Storage>::Ptr sac_model (new SampleConsensusModel1PointPlane<Storage> (data));
+ RandomSampleConsensus<Storage> sac (sac_model);
+ sac.setMaxIterations (10000);
+ sac.setDistanceThreshold (0.05);
+
+ {
+ pcl::ScopeTime timer ("computeModel: ");
+ if (!sac.computeModel (0))
+ {
+ std::cerr << "Failed to compute model" << std::endl;
+ }
+ else
+ {
+ typename SampleConsensusModel1PointPlane<Storage>::IndicesPtr inliers_stencil;
+ inliers_stencil = sac.getInliersStencil ();
+
+ // OpenNIRGB color;
+ // color.r = 253; color.g = 0; color.b = 0;
+ // std::cerr << data->points.size() << " =?= " << inliers_stencil->size () << std::endl;
+ // colorIndices<Storage> (data, inliers_stencil, color);
+ }
+ }
+
+ go_on = false;
+ //std::cerr << "got here" << std::endl;
+ //pcl_cuda::toPCL (*data, *output);
+ //std::cerr << "not here" << std::endl;
+ //viewer.showCloud (output);
+ }
+
+ template <template <typename> class Storage> void
+ cloud_cb (const boost::shared_ptr<openni_wrapper::Image>& image,
+ const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ float constant)
+ {
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);
+ typename PointCloudAOS<Storage>::Ptr data;
+ {
+ pcl::ScopeTime timer ("All: ");
+ // Compute the PointCloud on the device
+ d2c.compute<Storage> (depth_image, image, constant, data);
+
+ typename SampleConsensusModel1PointPlane<Storage>::Ptr sac_model (new SampleConsensusModel1PointPlane<Storage> (data));
+ RandomSampleConsensus<Storage> sac (sac_model);
+ sac.setMaxIterations (10000);
+ sac.setDistanceThreshold (0.05);
+
+ {
+ pcl::ScopeTime timer ("computeModel: ");
+ if (!sac.computeModel (0))
+ {
+ std::cerr << "Failed to compute model" << std::endl;
+ }
+ else
+ {
+ typename SampleConsensusModel1PointPlane<Storage>::IndicesPtr inliers_stencil;
+ inliers_stencil = sac.getInliersStencil ();
+
+ OpenNIRGB color;
+ color.r = 253; color.g = 0; color.b = 0;
+ colorIndices<Storage> (data, inliers_stencil, color);
+ }
+ }
+
+ }
+ pcl_cuda::toPCL (*data, *output);
+ //viewer.showCloud (output);
+ }
+
+ void
+ run (bool use_device)
+ {
+#if 1
+ pcl::Grabber* filegrabber = 0;
+
+ float frames_per_second = 1;
+ bool repeat = false;
+
+ std::string path = "./frame_0.pcd";
+ filegrabber = new pcl::PCDGrabber<pcl::PointXYZRGB > (path, frames_per_second, repeat);
+
+ if (use_device)
+ {
+ std::cerr << "[RANSAC] Using GPU..." << std::endl;
+ boost::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f = boost::bind (&SimpleKinectTool::file_cloud_cb<pcl_cuda::Device>, this, _1);
+ filegrabber->registerCallback (f);
+ }
+ else
+ {
+ std::cerr << "[RANSAC] Using CPU..." << std::endl;
+ boost::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f = boost::bind (&SimpleKinectTool::file_cloud_cb<pcl_cuda::Host>, this, _1);
+ filegrabber->registerCallback (f);
+ }
+
+ filegrabber->start ();
+ while (go_on)//!viewer.wasStopped () && go_on)
+ {
+ sleep (1);
+ }
+ filegrabber->stop ();
+
+
+ //------- END --------- load pcl logo file
+#else
+
+ pcl::Grabber* interface = new pcl::OpenNIGrabber();
+
+
+ boost::signals2::connection c;
+ if (use_device)
+ {
+ std::cerr << "[RANSAC] Using GPU..." << std::endl;
+ boost::function<void (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float)> f = boost::bind (&SimpleKinectTool::cloud_cb<pcl_cuda::Device>, this, _1, _2, _3);
+ c = interface->registerCallback (f);
+ }
+ else
+ {
+ std::cerr << "[RANSAC] Using CPU..." << std::endl;
+ boost::function<void (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float)> f = boost::bind (&SimpleKinectTool::cloud_cb<pcl_cuda::Host>, this, _1, _2, _3);
+ c = interface->registerCallback (f);
+ }
+
+ //viewer.runOnVisualizationThread (fn, "viz_cb");
+ interface->start ();
+ while (!viewer.wasStopped ())
+ {
+ sleep (1);
+ }
+
+ interface->stop ();
+#endif
+ }
+
+ pcl_cuda::DisparityToCloud d2c;
+ //pcl::visualization::CloudViewer viewer;
+ boost::mutex mutex_;
+ bool go_on;
+};
+
+int
+main (int argc, char **argv)
+{
+ bool use_device = false;
+ if (argc >= 2)
+ use_device = true;
+ SimpleKinectTool v;
+ v.run (use_device);
+ return 0;
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/highgui/highgui_c.h>
+#include "opencv2/gpu/gpu.hpp"
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/pcl_macros.h>
+
+#include <boost/shared_ptr.hpp>
+
+#include <pcl/cuda/features/normal_3d.h>
+#include <pcl/cuda/time_cpu.h>
+#include <pcl/cuda/time_gpu.h>
+#include <pcl/cuda/io/cloud_to_pcl.h>
+#include <pcl/cuda/io/extract_indices.h>
+#include <pcl/cuda/io/disparity_to_cloud.h>
+#include <pcl/cuda/io/host_device.h>
+#include <pcl/cuda/sample_consensus/sac_model_1point_plane.h>
+#include <pcl/cuda/sample_consensus/multi_ransac.h>
+#include <pcl/cuda/segmentation/connected_components.h>
+
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/pcd_grabber.h>
+#include <pcl/visualization/cloud_viewer.h>
+
+#include <iostream>
+
+using namespace pcl::cuda;
+
+template <template <typename> class Storage>
+struct ImageType
+{
+ typedef void type;
+};
+
+template <>
+struct ImageType<Device>
+{
+ typedef cv::gpu::GpuMat type;
+ static void createContinuous (int h, int w, int typ, type &mat)
+ {
+ cv::gpu::createContinuous (h, w, typ, mat);
+ }
+};
+
+template <>
+struct ImageType<Host>
+{
+ typedef cv::Mat type;
+ static void createContinuous (int h, int w, int typ, type &mat)
+ {
+ mat = cv::Mat (h, w, typ); // assume no padding at the end of line
+ }
+};
+
+class Segmentation
+{
+ public:
+ Segmentation ()
+ : viewer ("PCL CUDA - Segmentation"),
+ new_cloud(false), go_on(true), enable_color(1), normal_method(0), nr_neighbors (36), radius_cm (5), normal_viz_step(200)
+ , enable_normal_viz(false)
+ {}
+
+ void viz_cb (pcl::visualization::PCLVisualizer& viz)
+ {
+ static bool first_time = true;
+ static bool last_enable_normal_viz = enable_normal_viz;
+ boost::mutex::scoped_lock l(m_mutex);
+ if (new_cloud)
+ {
+ double psize = 1.0,opacity = 1.0,linesize =1.0;
+ std::string cloud_name ("cloud");
+
+ if (!first_time)
+ {
+ viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, linesize, cloud_name);
+ viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, cloud_name);
+ viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize, cloud_name);
+ if (last_enable_normal_viz)
+ viz.removePointCloud ("normalcloud");
+ viz.removePointCloud ("cloud");
+ }
+ else
+ first_time = false;
+
+ if (enable_normal_viz)
+ viz.addPointCloudNormals<pcl::PointXYZRGBNormal> (normal_cloud, normal_viz_step, 0.1, "normalcloud");
+
+ if (enable_color == 1)
+ {
+ typedef pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal> ColorHandler;
+ ColorHandler Color_handler (normal_cloud);
+ viz.addPointCloud<pcl::PointXYZRGBNormal> (normal_cloud, Color_handler, std::string(cloud_name));
+ }
+ else
+ {
+ typedef pcl::visualization::PointCloudColorHandlerGenericField <pcl::PointXYZRGBNormal> ColorHandler;
+ ColorHandler Color_handler (normal_cloud,"curvature");
+ viz.addPointCloud<pcl::PointXYZRGBNormal> (normal_cloud, Color_handler, cloud_name, 0);
+ }
+ viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, linesize, cloud_name);
+ viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, cloud_name);
+ viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize, cloud_name);
+ new_cloud = false;
+ }
+ last_enable_normal_viz = enable_normal_viz;
+ }
+
+ template <template <typename> class Storage> void
+ file_cloud_cb (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud)
+ {
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);
+ PointCloudAOS<Host> data_host;
+ data_host.points.resize (cloud->points.size());
+ for (size_t i = 0; i < cloud->points.size (); ++i)
+ {
+ PointXYZRGB pt;
+ pt.x = cloud->points[i].x;
+ pt.y = cloud->points[i].y;
+ pt.z = cloud->points[i].z;
+ // Pack RGB into a float
+ pt.rgb = *(float*)(&cloud->points[i].rgb);
+ data_host.points[i] = pt;
+ }
+ data_host.width = cloud->width;
+ data_host.height = cloud->height;
+ data_host.is_dense = cloud->is_dense;
+ typename PointCloudAOS<Storage>::Ptr data = toStorage<Host, Storage> (data_host);
+
+ // we got a cloud in device..
+
+ boost::shared_ptr<typename Storage<float4>::type> normals;
+ float focallength = 580/2.0;
+ {
+ ScopeTimeCPU time ("Normal Estimation");
+ normals = computePointNormals<Storage, typename PointIterator<Storage,PointXYZRGB>::type > (data->points.begin (), data->points.end (), focallength, data, 0.05, 30);
+ }
+ go_on = false;
+
+ boost::mutex::scoped_lock l(m_mutex);
+ normal_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+ toPCL (*data, *normals, *normal_cloud);
+ new_cloud = true;
+ }
+
+ template <template <typename> class Storage> void
+ cloud_cb (const boost::shared_ptr<openni_wrapper::Image>& image,
+ const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ float constant)
+ {
+ static unsigned count = 0;
+ static double last = getTime ();
+ double now = getTime ();
+ //if (++count == 30 || (now - last) > 5)
+ {
+ std::cout << std::endl;
+ count = 1;
+ std::cout << "Average framerate: " << double(count)/double(now - last) << " Hz --- ";
+ last = now;
+ }
+
+ static int smoothing_nr_iterations = 10;
+ static int smoothing_filter_size = 2;
+ static int enable_visualization = 1;
+ static int enable_mean_shift = 0;
+ static int enable_plane_fitting = 0;
+ static int meanshift_sp=8;
+ static int meanshift_sr=20;
+ static int meanshift_minsize=100;
+
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);
+ typename PointCloudAOS<Storage>::Ptr data;
+
+ //ScopeTimeCPU timer ("All: ");
+ ScopeTimeCPU time ("everything");
+ // Compute the PointCloud on the device
+ {
+ ScopeTimeCPU time ("disparity smoothing");
+ d2c.compute<Storage> (depth_image, image, constant, data, false, 1, smoothing_nr_iterations, smoothing_filter_size);
+ }
+
+ boost::shared_ptr<typename Storage<float4>::type> normals;
+ float focallength = 580/2.0;
+ {
+ ScopeTimeCPU time ("Normal Estimation");
+ if (normal_method == 1)
+ normals = computeFastPointNormals<Storage> (data);
+ else
+ normals = computePointNormals<Storage> (data->points.begin (), data->points.end (), focallength, data, radius_cm / 100.0f, nr_neighbors);
+ cudaThreadSynchronize ();
+ }
+
+ // retrieve normals as an image..
+ typename ImageType<Storage>::type normal_image;
+ typename StoragePointer<Storage,char4>::type ptr;
+ {
+ ScopeTimeCPU time ("Matrix Creation");
+ ImageType<Storage>::createContinuous ((int)data->height, (int)data->width, CV_8UC4, normal_image);
+ ptr = typename StoragePointer<Storage,char4>::type ((char4*)normal_image.data);
+ createNormalsImage<Storage> (ptr, *normals);
+ }
+
+ //TODO: this breaks for pcl::cuda::Host
+ cv::Mat seg;
+ {
+ ScopeTimeCPU time ("Mean Shift");
+ if (enable_mean_shift == 1)
+ {
+ cv::gpu::meanShiftSegmentation (normal_image, seg, meanshift_sp, meanshift_sr, meanshift_minsize);
+ typename Storage<char4>::type new_colors ((char4*)seg.datastart, (char4*)seg.dataend);
+ colorCloud<Storage> (data, new_colors);
+ }
+ }
+
+ typename SampleConsensusModel1PointPlane<Storage>::Ptr sac_model;
+ if (enable_plane_fitting == 1)
+ {
+ // Create sac_model
+ {
+ ScopeTimeCPU t ("creating sac_model");
+ sac_model.reset (new SampleConsensusModel1PointPlane<Storage> (data));
+ }
+ sac_model->setNormals (normals);
+
+ MultiRandomSampleConsensus<Storage> sac (sac_model);
+ sac.setMinimumCoverage (0.90); // at least 95% points should be explained by planes
+ sac.setMaximumBatches (1);
+ sac.setIerationsPerBatch (1024);
+ sac.setDistanceThreshold (0.05);
+
+ {
+ ScopeTimeCPU timer ("computeModel: ");
+ if (!sac.computeModel (0))
+ {
+ std::cerr << "Failed to compute model" << std::endl;
+ }
+ else
+ {
+ if (enable_visualization)
+ {
+// std::cerr << "getting inliers.. ";
+
+ std::vector<typename SampleConsensusModel1PointPlane<Storage>::IndicesPtr> planes;
+ typename Storage<int>::type region_mask;
+ markInliers<Storage> (data, region_mask, planes);
+ thrust::host_vector<int> regions_host;
+ std::copy (regions_host.begin (), regions_host.end(), std::ostream_iterator<int>(std::cerr, " "));
+ {
+ ScopeTimeCPU t ("retrieving inliers");
+ planes = sac.getAllInliers ();
+ }
+ std::vector<int> planes_inlier_counts = sac.getAllInlierCounts ();
+ std::vector<float4> coeffs = sac.getAllModelCoefficients ();
+ std::vector<float3> centroids = sac.getAllModelCentroids ();
+ std::cerr << "Found " << planes_inlier_counts.size () << " planes" << std::endl;
+ int best_plane = 0;
+ int best_plane_inliers_count = -1;
+
+ for (unsigned int i = 0; i < planes.size (); i++)
+ {
+ if (planes_inlier_counts[i] > best_plane_inliers_count)
+ {
+ best_plane = i;
+ best_plane_inliers_count = planes_inlier_counts[i];
+ }
+
+ typename SampleConsensusModel1PointPlane<Storage>::IndicesPtr inliers_stencil;
+ inliers_stencil = planes[i];//sac.getInliersStencil ();
+
+ OpenNIRGB color;
+ //double trand = 255 / (RAND_MAX + 1.0);
+
+ //color.r = (int)(rand () * trand);
+ //color.g = (int)(rand () * trand);
+ //color.b = (int)(rand () * trand);
+ color.r = (1.0f + coeffs[i].x) * 128;
+ color.g = (1.0f + coeffs[i].y) * 128;
+ color.b = (1.0f + coeffs[i].z) * 128;
+ {
+ ScopeTimeCPU t ("coloring planes");
+ colorIndices<Storage> (data, inliers_stencil, color);
+ }
+ }
+ }
+ }
+ }
+ }
+
+ {
+ ScopeTimeCPU time ("Vis");
+ cv::namedWindow("NormalImage", CV_WINDOW_NORMAL);
+ cv::namedWindow("Parameters", CV_WINDOW_NORMAL);
+ cvCreateTrackbar( "iterations", "Parameters", &smoothing_nr_iterations, 50, NULL);
+ cvCreateTrackbar( "filter_size", "Parameters", &smoothing_filter_size, 10, NULL);
+ cvCreateTrackbar( "enable_visualization", "Parameters", &enable_visualization, 1, NULL);
+ cvCreateTrackbar( "enable_color", "Parameters", &enable_color, 1, NULL);
+ cvCreateTrackbar( "normal_method", "Parameters", &normal_method, 1, NULL);
+ cvCreateTrackbar( "neighborhood_radius", "Parameters", &radius_cm, 50, NULL);
+ cvCreateTrackbar( "nr_neighbors", "Parameters", &nr_neighbors, 400, NULL);
+ cvCreateTrackbar( "normal_viz_step", "Parameters", &normal_viz_step, 1000, NULL);
+ cvCreateTrackbar( "enable_mean_shift", "Parameters", &enable_mean_shift, 1, NULL);
+ cvCreateTrackbar( "meanshift_sp", "Parameters", &meanshift_sp, 100, NULL);
+ cvCreateTrackbar( "meanshift_sr", "Parameters", &meanshift_sr, 100, NULL);
+ cvCreateTrackbar( "meanshift_minsize", "Parameters", &meanshift_minsize, 500, NULL);
+ cvCreateTrackbar( "enable_plane_fitting", "Parameters", &enable_plane_fitting, 1, NULL);
+ cvCreateTrackbar( "enable_normal_viz", "Parameters", &enable_normal_viz, 1, NULL);
+ if (enable_visualization == 1)
+ {
+
+ cv::Mat temp;
+ if (enable_mean_shift == 1)
+ cv::cvtColor(seg,temp,CV_BGR2RGB);
+ else
+ cv::cvtColor(cv::Mat(normal_image),temp,CV_BGR2RGB);
+ cv::imshow ("NormalImage", temp);
+
+ boost::mutex::scoped_lock l(m_mutex);
+ normal_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+ toPCL (*data, *normals, *normal_cloud);
+ new_cloud = true;
+ }
+ cv::waitKey (2);
+ }
+ }
+
+ void
+ run (bool use_device, bool use_file)
+ {
+ if (use_file)
+ {
+ pcl::Grabber* filegrabber = 0;
+
+ float frames_per_second = 1;
+ bool repeat = false;
+
+ std::string path = "./frame_0.pcd";
+ filegrabber = new pcl::PCDGrabber<pcl::PointXYZRGB > (path, frames_per_second, repeat);
+
+ if (use_device)
+ {
+ std::cerr << "[Segmentation] Using GPU..." << std::endl;
+ boost::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f = boost::bind (&Segmentation::file_cloud_cb<Device>, this, _1);
+ filegrabber->registerCallback (f);
+ }
+ else
+ {
+// std::cerr << "[Segmentation] Using CPU..." << std::endl;
+// boost::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f = boost::bind (&Segmentation::file_cloud_cb<Host>, this, _1);
+// filegrabber->registerCallback (f);
+ }
+
+ filegrabber->start ();
+ while (go_on)//!viewer.wasStopped () && go_on)
+ {
+ pcl_sleep (1);
+ }
+ filegrabber->stop ();
+ }
+ else
+ {
+ pcl::Grabber* grabber = new pcl::OpenNIGrabber();
+
+ boost::signals2::connection c;
+ if (use_device)
+ {
+ std::cerr << "[Segmentation] Using GPU..." << std::endl;
+ boost::function<void (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float)> f = boost::bind (&Segmentation::cloud_cb<Device>, this, _1, _2, _3);
+ c = grabber->registerCallback (f);
+ }
+ else
+ {
+// std::cerr << "[Segmentation] Using CPU..." << std::endl;
+// boost::function<void (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float)> f = boost::bind (&Segmentation::cloud_cb<Host>, this, _1, _2, _3);
+// c = grabber->registerCallback (f);
+ }
+
+ viewer.runOnVisualizationThread (boost::bind(&Segmentation::viz_cb, this, _1), "viz_cb");
+
+ grabber->start ();
+
+ while (!viewer.wasStopped ())
+ {
+ pcl_sleep (1);
+ }
+
+ grabber->stop ();
+ }
+ }
+
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr normal_cloud;
+ DisparityToCloud d2c;
+ pcl::visualization::CloudViewer viewer;
+ boost::mutex m_mutex;
+ bool new_cloud, go_on;
+ int enable_color;
+ int normal_method;
+ int nr_neighbors;
+ int radius_cm;
+ int normal_viz_step;
+ int enable_normal_viz;
+};
+
+int
+main (int argc, char **argv)
+{
+ bool use_device = false;
+ bool use_file = false;
+ if (argc >= 2)
+ use_device = true;
+ if (argc >= 3)
+ use_file = true;
+ Segmentation s;
+ s.run (use_device, use_file);
+ return 0;
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <boost/shared_ptr.hpp>
+
+#include <pcl/cuda/features/normal_3d.h>
+#include <pcl/cuda/time_cpu.h>
+#include <pcl/cuda/time_gpu.h>
+#include <pcl/cuda/io/cloud_to_pcl.h>
+#include <pcl/cuda/io/extract_indices.h>
+#include <pcl/cuda/io/disparity_to_cloud.h>
+#include <pcl/cuda/io/host_device.h>
+#include <pcl/cuda/segmentation/connected_components.h>
+#include <pcl/cuda/segmentation/mssegmentation.h>
+
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/pcd_grabber.h>
+#include <pcl/visualization/cloud_viewer.h>
+
+#include <opencv2/highgui/highgui.hpp>
+#include "opencv2/gpu/gpu.hpp"
+
+#include <iostream>
+#include <fstream>
+
+using namespace pcl::cuda;
+
+template <template <typename> class Storage>
+struct ImageType
+{
+ typedef void type;
+};
+
+template <>
+struct ImageType<Device>
+{
+ typedef cv::gpu::GpuMat type;
+ static void createContinuous (int h, int w, int typ, type &mat)
+ {
+ cv::gpu::createContinuous (h, w, typ, mat);
+ }
+};
+
+template <>
+struct ImageType<Host>
+{
+ typedef cv::Mat type;
+ static void createContinuous (int h, int w, int typ, type &mat)
+ {
+ mat = cv::Mat (h, w, typ); // assume no padding at the end of line
+ }
+};
+
+class Segmentation
+{
+ public:
+ Segmentation ()
+ : viewer ("PCL CUDA - Segmentation"),
+ new_cloud(false), go_on(true) {}
+
+ void viz_cb (pcl::visualization::PCLVisualizer& viz)
+ {
+ static bool first_time = true;
+ boost::mutex::scoped_lock l(m_mutex);
+ if (new_cloud)
+ {
+ //typedef pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal> ColorHandler;
+ typedef pcl::visualization::PointCloudColorHandlerGenericField <pcl::PointXYZRGBNormal> ColorHandler;
+ ColorHandler Color_handler (normal_cloud,"curvature");
+ if (!first_time)
+ {
+ viz.removePointCloud ("normalcloud");
+ viz.removePointCloud ("cloud");
+ }
+ else
+ first_time = false;
+
+ viz.addPointCloudNormals<pcl::PointXYZRGBNormal> (normal_cloud, 200, 0.1, "normalcloud");
+ viz.addPointCloud<pcl::PointXYZRGBNormal> (normal_cloud, Color_handler, std::string("cloud"), 0);
+ new_cloud = false;
+ }
+ }
+
+ template <template <typename> class Storage> void
+ file_cloud_cb (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud)
+ {
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);
+ PointCloudAOS<Host> data_host;
+ data_host.points.resize (cloud->points.size());
+ for (size_t i = 0; i < cloud->points.size (); ++i)
+ {
+ PointXYZRGB pt;
+ pt.x = cloud->points[i].x;
+ pt.y = cloud->points[i].y;
+ pt.z = cloud->points[i].z;
+ // Pack RGB into a float
+ pt.rgb = *(float*)(&cloud->points[i].rgb);
+ data_host.points[i] = pt;
+ }
+ data_host.width = cloud->width;
+ data_host.height = cloud->height;
+ data_host.is_dense = cloud->is_dense;
+ typename PointCloudAOS<Storage>::Ptr data = toStorage<Host, Storage> (data_host);
+
+ // we got a cloud in device..
+
+ boost::shared_ptr<typename Storage<float4>::type> normals;
+ float focallength = 580/2.0;
+ {
+ ScopeTimeCPU time ("TIMING: Normal Estimation");
+ normals = computePointNormals<Storage, typename PointIterator<Storage,PointXYZRGB>::type > (data->points.begin (), data->points.end (), focallength, data, 0.05, 30);
+ }
+ go_on = false;
+
+ boost::mutex::scoped_lock l(m_mutex);
+ normal_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+ toPCL (*data, *normals, *normal_cloud);
+ new_cloud = true;
+ }
+
+ template <template <typename> class Storage> void
+ cloud_cb (const boost::shared_ptr<openni_wrapper::Image>& image,
+ const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ float constant)
+ {
+ static unsigned count = 0;
+ static double last = getTime ();
+ double now = getTime ();
+ if (++count == 30 || (now - last) > 5)
+ {
+ std::cout << "Average framerate: " << double(count)/double(now - last) << " Hz..........................................." << std::endl;
+ count = 0;
+ last = now;
+ }
+
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);
+ typename PointCloudAOS<Storage>::Ptr data;
+
+ //ScopeTimeCPU timer ("All: ");
+ ScopeTimeCPU time ("TIMING: everything");
+ // Compute the PointCloud on the device
+ d2c.compute<Storage> (depth_image, image, constant, data, true, 2);
+
+ boost::shared_ptr<typename Storage<float4>::type> normals;
+ {
+ ScopeTimeCPU time ("TIMING: Normal Estimation");
+ normals = computeFastPointNormals<Storage> (data);
+ }
+
+ // retrieve normals as an image..
+ // typename StoragePointer<Storage,char4>::type ptr = StorageAllocator<Storage,char4>::alloc (data->width * data->height);
+ // createNormalsImage<Storage> (ptr, normals);
+ // typename ImageType<Storage>::type normal_image (data->height, data->width, CV_8UC4, thrust::raw_pointer_cast<char4>(ptr), data->width);
+ typename ImageType<Storage>::type normal_image;
+ typename StoragePointer<Storage,char4>::type ptr;
+ {
+ ScopeTimeCPU time ("TIMING: Matrix Creation");
+ ImageType<Storage>::createContinuous ((int)data->height, (int)data->width, CV_8UC4, normal_image);
+ ptr = typename StoragePointer<Storage,char4>::type ((char4*)normal_image.data);
+ createNormalsImage<Storage> (ptr, *normals);
+ }
+
+ //TODO: this breaks for pcl::cuda::Host
+ pcl::cuda::detail::DjSets comps(0);
+ cv::Mat seg;
+ {
+ ScopeTimeCPU time ("TIMING: Mean Shift");
+ pcl::cuda::meanShiftSegmentation (normal_image, seg, 8, 20, 100, comps);
+ }
+ std::cerr << "time since callback: " << getTime () - now << std::endl;
+// std::cout << "comps " << comps.parent.size () << " " << comps.rank.size () << " " << comps.size.size () << std::endl;
+// std::cout << "parent" << std::endl;
+// std::copy (comps.parent.begin (), comps.parent.end (), std::ostream_iterator<int>(std::cout, " "));
+// std::cout << "rank" << std::endl;
+// std::copy (comps.rank.begin (), comps.rank.end (), std::ostream_iterator<int>(std::cout, " "));
+// std::cout << "size" << std::endl;
+// std::copy (comps.size.begin (), comps.size.end (), std::ostream_iterator<int>(std::cout, " "));
+
+ {
+ ScopeTimeCPU time ("TIMING: Vis");
+ cv::namedWindow("NormalImage", CV_WINDOW_NORMAL);
+ //cv::imshow ("NormalImage", cv::Mat(normal_image));
+ cv::imshow ("NormalImage", seg);
+ cv::waitKey (2);
+
+ boost::mutex::scoped_lock l(m_mutex);
+ normal_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+ toPCL (*data, *normals, *normal_cloud);
+ new_cloud = true;
+ }
+ }
+
+ void
+ run (bool use_device, bool use_file)
+ {
+ if (use_file)
+ {
+ pcl::Grabber* filegrabber = 0;
+
+ float frames_per_second = 1;
+ bool repeat = false;
+
+ std::string path = "./frame_0.pcd";
+ filegrabber = new pcl::PCDGrabber<pcl::PointXYZRGB > (path, frames_per_second, repeat);
+
+ if (use_device)
+ {
+ std::cerr << "[Segmentation] Using GPU..." << std::endl;
+ boost::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f = boost::bind (&Segmentation::file_cloud_cb<Device>, this, _1);
+ filegrabber->registerCallback (f);
+ }
+ else
+ {
+// std::cerr << "[Segmentation] Using CPU..." << std::endl;
+// boost::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f = boost::bind (&Segmentation::file_cloud_cb<Host>, this, _1);
+// filegrabber->registerCallback (f);
+ }
+
+ filegrabber->start ();
+ while (go_on)//!viewer.wasStopped () && go_on)
+ {
+ pcl_sleep (1);
+ }
+ filegrabber->stop ();
+ }
+ else
+ {
+ pcl::Grabber* grabber = new pcl::OpenNIGrabber();
+
+ boost::signals2::connection c;
+ if (use_device)
+ {
+ std::cerr << "[Segmentation] Using GPU..." << std::endl;
+ boost::function<void (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float)> f = boost::bind (&Segmentation::cloud_cb<Device>, this, _1, _2, _3);
+ c = grabber->registerCallback (f);
+ }
+ else
+ {
+// std::cerr << "[Segmentation] Using CPU..." << std::endl;
+// boost::function<void (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float)> f = boost::bind (&Segmentation::cloud_cb<Host>, this, _1, _2, _3);
+// c = grabber->registerCallback (f);
+ }
+
+ viewer.runOnVisualizationThread (boost::bind(&Segmentation::viz_cb, this, _1), "viz_cb");
+
+ grabber->start ();
+
+ while (!viewer.wasStopped ())
+ {
+ pcl_sleep (1);
+ }
+
+ grabber->stop ();
+ }
+ }
+
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr normal_cloud;
+ DisparityToCloud d2c;
+ pcl::visualization::CloudViewer viewer;
+ boost::mutex m_mutex;
+ bool new_cloud, go_on;
+};
+
+int
+main (int argc, char **argv)
+{
+ bool use_device = false;
+ bool use_file = false;
+ if (argc >= 2)
+ use_device = true;
+ if (argc >= 3)
+ use_file = true;
+ Segmentation s;
+ s.run (use_device, use_file);
+ return 0;
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <pcl/cuda/io/cloud_to_pcl.h>
+#include <pcl/cuda/io/disparity_to_cloud.h>
+
+#include <pcl/io/openni_grabber.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/cuda/time_cpu.h>
+#include <boost/shared_ptr.hpp>
+#include <pcl/visualization/cloud_viewer.h>
+#include <iostream>
+
+using pcl::cuda::PointCloudAOS;
+using pcl::cuda::Device;
+using pcl::cuda::Host;
+
+class SimpleKinectTool
+{
+ public:
+ SimpleKinectTool (bool downsample) : viewer ("KinectGrabber"), downsample_(downsample) {}
+
+ void cloud_cb_ (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float constant)
+ {
+ PointCloudAOS<Device>::Ptr data;
+ {
+ pcl::cuda::ScopeTimeCPU t ("time:");
+ d2c.compute<Device> (depth_image, image, constant, data, downsample_);
+ }
+ //d2c.callback (depth_image, constant, *data);
+
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::cuda::toPCL (*data, *output);
+
+ viewer.showCloud (output);
+
+ }
+
+ void run (const std::string& device_id)
+ {
+ pcl::Grabber* interface = new pcl::OpenNIGrabber(device_id);
+
+ boost::function<void (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float)> f = boost::bind (&SimpleKinectTool::cloud_cb_, this, _1, _2, _3);
+
+ boost::signals2::connection c = interface->registerCallback (f);
+
+ //viewer.runOnVisualizationThread (fn, "viz_cb");
+ interface->start ();
+
+ while (true)
+ {
+ sleep (1);
+ }
+
+ interface->stop ();
+ }
+
+ pcl::cuda::DisparityToCloud d2c;
+ pcl::visualization::CloudViewer viewer;
+ boost::mutex mutex_;
+ bool downsample_;
+};
+
+int main (int argc, char** argv)
+{
+ std::string device_id = "#1";
+ int downsample = false;
+ if (argc >= 2)
+ {
+ device_id = argv[1];
+ }
+ if (argc >= 3)
+ {
+ downsample = atoi (argv[2]);
+ }
+ SimpleKinectTool v (downsample);
+ v.run (device_id);
+ return 0;
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <pcl/cuda/io/cloud_to_pcl.h>
+#include <pcl/cuda/io/disparity_to_cloud.h>
+
+#include <pcl/io/openni_grabber.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/cuda/time_cpu.h>
+#include <boost/shared_ptr.hpp>
+#include <pcl/visualization/cloud_viewer.h>
+#include <iostream>
+
+using pcl::cuda::PointCloudAOS;
+using pcl::cuda::Device;
+
+class KinectViewerCuda
+{
+ public:
+ KinectViewerCuda (bool downsample) : viewer ("KinectGrabber"), downsample_(downsample) {}
+
+ void cloud_cb_ (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float constant)
+ {
+ PointCloudAOS<Device>::Ptr data;
+ {
+ pcl::cuda::ScopeTimeCPU t ("time:");
+ d2c.compute<Device> (depth_image, image, constant, data, downsample_);
+ }
+
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::cuda::toPCL (*data, *output);
+
+ viewer.showCloud (output);
+
+ }
+
+ void run (const std::string& device_id)
+ {
+ pcl::Grabber* interface = new pcl::OpenNIGrabber(device_id);
+
+ boost::function<void (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float)>
+ f = boost::bind (&KinectViewerCuda::cloud_cb_, this, _1, _2, _3);
+
+ boost::signals2::connection c = interface->registerCallback (f);
+
+ interface->start ();
+
+ while (true)
+ {
+ pcl_sleep (1);
+ }
+
+ interface->stop ();
+ }
+
+ pcl::cuda::DisparityToCloud d2c;
+ pcl::visualization::CloudViewer viewer;
+ boost::mutex mutex_;
+ bool downsample_;
+};
+
+int main (int argc, char** argv)
+{
+ std::string device_id = "#1";
+ int downsample = false;
+ if (argc >= 2)
+ {
+ device_id = argv[1];
+ }
+ if (argc >= 3)
+ {
+ downsample = atoi (argv[2]);
+ }
+ KinectViewerCuda v (downsample);
+ v.run (device_id);
+ return 0;
+}
--- /dev/null
+set(SUBSYS_NAME cuda_common)
+set(SUBSYS_PATH cuda/common)
+set(SUBSYS_DESC "Point cloud CUDA common library")
+set(SUBSYS_DEPS)
+
+set(build TRUE)
+PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ON)
+mark_as_advanced("BUILD_${SUBSYS_NAME}")
+PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS})
+PCL_SET_SUBSYS_INCLUDE_DIR(${SUBSYS_NAME} ${SUBSYS_PATH})
+
+## ---[ Point Cloud Library - pcl/cuda/common
+set(incs
+ include/pcl/cuda/cutil.h
+ include/pcl/cuda/cutil_math.h
+ include/pcl/cuda/point_cloud.h
+ include/pcl/cuda/point_types.h
+ include/pcl/cuda/thrust.h
+ include/pcl/cuda/time_gpu.h
+ include/pcl/cuda/time_cpu.h
+ include/pcl/cuda/cutil_inline.h
+ include/pcl/cuda/cutil_inline_bankchecker.h
+ include/pcl/cuda/cutil_inline_drvapi.h
+ include/pcl/cuda/cutil_inline_runtime.h
+ )
+set(common_incs
+ include/pcl/cuda/common/eigen.h
+ include/pcl/cuda/common/point_type_rgb.h
+ )
+
+include_directories(./include)
+#set(LIB_NAME pcl_${SUBSYS_NAME})
+set(EXT_DEPS CUDA)
+#PCL_MAKE_PKGCONFIG(${LIB_NAME} ${SUBSYS_NAME} "${SUBSYS_DESC}"
+# "${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "")
+
+# Install include files
+PCL_ADD_INCLUDES(${SUBSYS_NAME} "cuda" ${incs})
+PCL_ADD_INCLUDES(${SUBSYS_NAME} ${SUBSYS_PATH} ${common_incs})
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// Eigen is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 3 of the License, or (at your option) any later version.
+//
+// Alternatively, you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of
+// the License, or (at your option) any later version.
+//
+// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+// The computeRoots function included in this is based on materials
+// covered by the following copyright and license:
+//
+// Geometric Tools, LLC
+// Copyright (c) 1998-2010
+// Distributed under the Boost Software License, Version 1.0.
+//
+// Permission is hereby granted, free of charge, to any person or organization
+// obtaining a copy of the software and accompanying documentation covered by
+// this license (the "Software") to use, reproduce, display, distribute,
+// execute, and transmit the Software, and to prepare derivative works of the
+// Software, and to permit third-parties to whom the Software is furnished to
+// do so, all subject to the following:
+//
+// The copyright notices in the Software and this entire statement, including
+// the above license grant, this restriction and the following disclaimer,
+// must be included in all copies of the Software, in whole or in part, and
+// all derivative works of the Software, unless such copies or derivative
+// works are solely in the form of machine-executable object code generated by
+// a source language processor.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+// DEALINGS IN THE SOFTWARE.
+
+#ifndef PCL_CUDA_EIGEN_H_
+#define PCL_CUDA_EIGEN_H_
+
+#include <pcl/cuda/point_cloud.h>
+#include <pcl/cuda/cutil_math.h>
+
+#include <limits>
+#include <float.h>
+
+namespace pcl
+{
+ namespace cuda
+ {
+
+ inline __host__ __device__ bool isMuchSmallerThan (float x, float y)
+ {
+ float prec_sqr = FLT_EPSILON * FLT_EPSILON; // copied from <eigen>/include/Eigen/src/Core/NumTraits.h
+ return x * x <= prec_sqr * y * y;
+ }
+
+ inline __host__ __device__ float3 unitOrthogonal (const float3& src)
+ {
+ float3 perp;
+ /* Let us compute the crossed product of *this with a vector
+ * that is not too close to being colinear to *this.
+ */
+
+ /* unless the x and y coords are both close to zero, we can
+ * simply take ( -y, x, 0 ) and normalize it.
+ */
+ if((!isMuchSmallerThan(src.x, src.z))
+ || (!isMuchSmallerThan(src.y, src.z)))
+ {
+ float invnm = 1.0f / sqrtf (src.x*src.x + src.y*src.y);
+ perp.x = -src.y*invnm;
+ perp.y = src.x*invnm;
+ perp.z = 0.0f;
+ }
+ /* if both x and y are close to zero, then the vector is close
+ * to the z-axis, so it's far from colinear to the x-axis for instance.
+ * So we take the crossed product with (1,0,0) and normalize it.
+ */
+ else
+ {
+ float invnm = 1.0f / sqrtf (src.z*src.z + src.y*src.y);
+ perp.x = 0.0f;
+ perp.y = -src.z*invnm;
+ perp.z = src.y*invnm;
+ }
+
+ return perp;
+ }
+
+ inline __host__ __device__ void computeRoots2 (const float& b, const float& c, float3& roots)
+ {
+ roots.x = 0.0f;
+ float d = b * b - 4.0f * c;
+ if (d < 0.0f) // no real roots!!!! THIS SHOULD NOT HAPPEN!
+ d = 0.0f;
+
+ float sd = sqrt (d);
+
+ roots.z = 0.5f * (b + sd);
+ roots.y = 0.5f * (b - sd);
+ }
+
+ inline __host__ __device__ void swap (float& a, float& b)
+ {
+ float c(a); a=b; b=c;
+ }
+
+
+ // template<typename Matrix, typename Roots>
+ inline __host__ __device__ void
+ computeRoots (const CovarianceMatrix& m, float3& roots)
+ {
+ // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The
+ // eigenvalues are the roots to this equation, all guaranteed to be
+ // real-valued, because the matrix is symmetric.
+ float c0 = m.data[0].x*m.data[1].y*m.data[2].z
+ + 2.0f * m.data[0].y*m.data[0].z*m.data[1].z
+ - m.data[0].x*m.data[1].z*m.data[1].z
+ - m.data[1].y*m.data[0].z*m.data[0].z
+ - m.data[2].z*m.data[0].y*m.data[0].y;
+ float c1 = m.data[0].x*m.data[1].y -
+ m.data[0].y*m.data[0].y +
+ m.data[0].x*m.data[2].z -
+ m.data[0].z*m.data[0].z +
+ m.data[1].y*m.data[2].z -
+ m.data[1].z*m.data[1].z;
+ float c2 = m.data[0].x + m.data[1].y + m.data[2].z;
+
+
+ if (fabs(c0) < FLT_EPSILON) // one root is 0 -> quadratic equation
+ computeRoots2 (c2, c1, roots);
+ else
+ {
+ const float s_inv3 = 1.0f/3.0f;
+ const float s_sqrt3 = sqrtf (3.0f);
+ // Construct the parameters used in classifying the roots of the equation
+ // and in solving the equation for the roots in closed form.
+ float c2_over_3 = c2 * s_inv3;
+ float a_over_3 = (c1 - c2 * c2_over_3) * s_inv3;
+ if (a_over_3 > 0.0f)
+ a_over_3 = 0.0f;
+
+ float half_b = 0.5f * (c0 + c2_over_3 * (2.0f * c2_over_3 * c2_over_3 - c1));
+
+ float q = half_b * half_b + a_over_3 * a_over_3 * a_over_3;
+ if (q > 0.0f)
+ q = 0.0f;
+
+ // Compute the eigenvalues by solving for the roots of the polynomial.
+ float rho = sqrtf (-a_over_3);
+ float theta = std::atan2 (sqrtf (-q), half_b) * s_inv3;
+ float cos_theta = cos (theta);
+ float sin_theta = sin (theta);
+ roots.x = c2_over_3 + 2.f * rho * cos_theta;
+ roots.y = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
+ roots.z = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
+
+ // Sort in increasing order.
+ if (roots.x >= roots.y)
+ swap (roots.x, roots.y);
+ if (roots.y >= roots.z)
+ {
+ swap (roots.y, roots.z);
+ if (roots.x >= roots.y)
+ swap (roots.x, roots.y);
+ }
+
+ if (roots.x <= 0.0f) // eigenval for symetric positive semi-definite matrix can not be negative! Set it to 0
+ computeRoots2 (c2, c1, roots);
+ }
+ }
+
+ inline __host__ __device__ void
+ eigen33 (const CovarianceMatrix& mat, CovarianceMatrix& evecs, float3& evals)
+ {
+ evals = evecs.data[0] = evecs.data[1] = evecs.data[2] = make_float3 (0.0f, 0.0f, 0.0f);
+
+ // Scale the matrix so its entries are in [-1,1]. The scaling is applied
+ // only when at least one matrix entry has magnitude larger than 1.
+
+ //Scalar scale = mat.cwiseAbs ().maxCoeff ();
+ float3 scale_tmp = fmaxf (fmaxf (fabs (mat.data[0]), fabs (mat.data[1])), fabs (mat.data[2]));
+ float scale = fmaxf (fmaxf (scale_tmp.x, scale_tmp.y), scale_tmp.z);
+ if (scale <= FLT_MIN)
+ scale = 1.0f;
+
+ CovarianceMatrix scaledMat;
+ scaledMat.data[0] = mat.data[0] / scale;
+ scaledMat.data[1] = mat.data[1] / scale;
+ scaledMat.data[2] = mat.data[2] / scale;
+
+ // Compute the eigenvalues
+ computeRoots (scaledMat, evals);
+
+ if ((evals.z-evals.x) <= FLT_EPSILON)
+ {
+ // all three equal
+ evecs.data[0] = make_float3 (1.0f, 0.0f, 0.0f);
+ evecs.data[1] = make_float3 (0.0f, 1.0f, 0.0f);
+ evecs.data[2] = make_float3 (0.0f, 0.0f, 1.0f);
+ }
+ else if ((evals.y-evals.x) <= FLT_EPSILON)
+ {
+ // first and second equal
+ CovarianceMatrix tmp;
+ tmp.data[0] = scaledMat.data[0];
+ tmp.data[1] = scaledMat.data[1];
+ tmp.data[2] = scaledMat.data[2];
+
+ tmp.data[0].x -= evals.z;
+ tmp.data[1].y -= evals.z;
+ tmp.data[2].z -= evals.z;
+
+ float3 vec1 = cross (tmp.data[0], tmp.data[1]);
+ float3 vec2 = cross (tmp.data[0], tmp.data[2]);
+ float3 vec3 = cross (tmp.data[1], tmp.data[2]);
+
+ float len1 = dot (vec1, vec1);
+ float len2 = dot (vec2, vec2);
+ float len3 = dot (vec3, vec3);
+
+ if (len1 >= len2 && len1 >= len3)
+ evecs.data[2] = vec1 / sqrtf (len1);
+ else if (len2 >= len1 && len2 >= len3)
+ evecs.data[2] = vec2 / sqrtf (len2);
+ else
+ evecs.data[2] = vec3 / sqrtf (len3);
+
+ evecs.data[1] = unitOrthogonal (evecs.data[2]);
+ evecs.data[0] = cross (evecs.data[1], evecs.data[2]);
+ }
+ else if ((evals.z-evals.y) <= FLT_EPSILON)
+ {
+ // second and third equal
+ CovarianceMatrix tmp;
+ tmp.data[0] = scaledMat.data[0];
+ tmp.data[1] = scaledMat.data[1];
+ tmp.data[2] = scaledMat.data[2];
+ tmp.data[0].x -= evals.x;
+ tmp.data[1].y -= evals.x;
+ tmp.data[2].z -= evals.x;
+
+ float3 vec1 = cross (tmp.data[0], tmp.data[1]);
+ float3 vec2 = cross (tmp.data[0], tmp.data[2]);
+ float3 vec3 = cross (tmp.data[1], tmp.data[2]);
+
+ float len1 = dot (vec1, vec1);
+ float len2 = dot (vec2, vec2);
+ float len3 = dot (vec3, vec3);
+
+ if (len1 >= len2 && len1 >= len3)
+ evecs.data[0] = vec1 / sqrtf (len1);
+ else if (len2 >= len1 && len2 >= len3)
+ evecs.data[0] = vec2 / sqrtf (len2);
+ else
+ evecs.data[0] = vec3 / sqrtf (len3);
+
+ evecs.data[1] = unitOrthogonal (evecs.data[0]);
+ evecs.data[2] = cross (evecs.data[0], evecs.data[1]);
+ }
+ else
+ {
+ CovarianceMatrix tmp;
+ tmp.data[0] = scaledMat.data[0];
+ tmp.data[1] = scaledMat.data[1];
+ tmp.data[2] = scaledMat.data[2];
+ tmp.data[0].x -= evals.z;
+ tmp.data[1].y -= evals.z;
+ tmp.data[2].z -= evals.z;
+
+ float3 vec1 = cross (tmp.data[0], tmp.data[1]);
+ float3 vec2 = cross (tmp.data[0], tmp.data[2]);
+ float3 vec3 = cross (tmp.data[1], tmp.data[2]);
+
+ float len1 = dot (vec1, vec1);
+ float len2 = dot (vec2, vec2);
+ float len3 = dot (vec3, vec3);
+
+ float mmax[3];
+ unsigned int min_el = 2;
+ unsigned int max_el = 2;
+ if (len1 >= len2 && len1 >= len3)
+ {
+ mmax[2] = len1;
+ evecs.data[2] = vec1 / sqrtf (len1);
+ }
+ else if (len2 >= len1 && len2 >= len3)
+ {
+ mmax[2] = len2;
+ evecs.data[2] = vec2 / sqrtf (len2);
+ }
+ else
+ {
+ mmax[2] = len3;
+ evecs.data[2] = vec3 / sqrtf (len3);
+ }
+
+ tmp.data[0] = scaledMat.data[0];
+ tmp.data[1] = scaledMat.data[1];
+ tmp.data[2] = scaledMat.data[2];
+ tmp.data[0].x -= evals.y;
+ tmp.data[1].y -= evals.y;
+ tmp.data[2].z -= evals.y;
+
+ vec1 = cross (tmp.data[0], tmp.data[1]);
+ vec2 = cross (tmp.data[0], tmp.data[2]);
+ vec3 = cross (tmp.data[1], tmp.data[2]);
+
+ len1 = dot (vec1, vec1);
+ len2 = dot (vec2, vec2);
+ len3 = dot (vec3, vec3);
+ if (len1 >= len2 && len1 >= len3)
+ {
+ mmax[1] = len1;
+ evecs.data[1] = vec1 / sqrtf (len1);
+ min_el = len1 <= mmax[min_el]? 1: min_el;
+ max_el = len1 > mmax[max_el]? 1: max_el;
+ }
+ else if (len2 >= len1 && len2 >= len3)
+ {
+ mmax[1] = len2;
+ evecs.data[1] = vec2 / sqrtf (len2);
+ min_el = len2 <= mmax[min_el]? 1: min_el;
+ max_el = len2 > mmax[max_el]? 1: max_el;
+ }
+ else
+ {
+ mmax[1] = len3;
+ evecs.data[1] = vec3 / sqrtf (len3);
+ min_el = len3 <= mmax[min_el]? 1: min_el;
+ max_el = len3 > mmax[max_el]? 1: max_el;
+ }
+
+ tmp.data[0] = scaledMat.data[0];
+ tmp.data[1] = scaledMat.data[1];
+ tmp.data[2] = scaledMat.data[2];
+ tmp.data[0].x -= evals.x;
+ tmp.data[1].y -= evals.x;
+ tmp.data[2].z -= evals.x;
+
+ vec1 = cross (tmp.data[0], tmp.data[1]);
+ vec2 = cross (tmp.data[0], tmp.data[2]);
+ vec3 = cross (tmp.data[1], tmp.data[2]);
+
+ len1 = dot (vec1, vec1);
+ len2 = dot (vec2, vec2);
+ len3 = dot (vec3, vec3);
+ if (len1 >= len2 && len1 >= len3)
+ {
+ mmax[0] = len1;
+ evecs.data[0] = vec1 / sqrtf (len1);
+ min_el = len3 <= mmax[min_el]? 0: min_el;
+ max_el = len3 > mmax[max_el]? 0: max_el;
+ }
+ else if (len2 >= len1 && len2 >= len3)
+ {
+ mmax[0] = len2;
+ evecs.data[0] = vec2 / sqrtf (len2);
+ min_el = len3 <= mmax[min_el]? 0: min_el;
+ max_el = len3 > mmax[max_el]? 0: max_el;
+ }
+ else
+ {
+ mmax[0] = len3;
+ evecs.data[0] = vec3 / sqrtf (len3);
+ min_el = len3 <= mmax[min_el]? 0: min_el;
+ max_el = len3 > mmax[max_el]? 0: max_el;
+ }
+
+ unsigned mid_el = 3 - min_el - max_el;
+ evecs.data[min_el] = normalize (cross (evecs.data[(min_el+1)%3], evecs.data[(min_el+2)%3] ));
+ evecs.data[mid_el] = normalize (cross (evecs.data[(mid_el+1)%3], evecs.data[(mid_el+2)%3] ));
+ }
+ // Rescale back to the original size.
+ evals *= scale;
+ }
+
+ /** \brief Simple kernel to add two points. */
+ struct AddPoints
+ {
+ __inline__ __host__ __device__ float3
+ operator () (float3 lhs, float3 rhs)
+ {
+ return lhs + rhs;
+ }
+ };
+
+ /** \brief Adds two matrices element-wise. */
+ struct AddCovariances
+ {
+ __inline__ __host__ __device__
+ CovarianceMatrix
+ operator () (CovarianceMatrix lhs, CovarianceMatrix rhs)
+ {
+ CovarianceMatrix ret;
+ ret.data[0] = lhs.data[0] + rhs.data[0];
+ ret.data[1] = lhs.data[1] + rhs.data[1];
+ ret.data[2] = lhs.data[2] + rhs.data[2];
+ return ret;
+ }
+ };
+
+ /** \brief Simple kernel to convert a PointXYZRGB to float3. Relies the cast operator of PointXYZRGB. */
+ struct convert_point_to_float3
+ {
+ __inline__ __host__ __device__ float3
+ operator () (const PointXYZRGB& pt) {return pt;}
+ };
+
+ /** \brief Kernel to compute a ``covariance matrix'' for a single point. */
+ struct ComputeCovarianceForPoint
+ {
+ float3 centroid_;
+ __inline__ __host__ __device__
+ ComputeCovarianceForPoint (const float3& centroid) : centroid_ (centroid) {}
+
+ __inline__ __host__ __device__ CovarianceMatrix
+ operator () (const PointXYZRGB& point)
+ {
+ CovarianceMatrix cov;
+ float3 pt = point - centroid_;
+ cov.data[1].y = pt.y * pt.y;
+ cov.data[1].z = pt.y * pt.z;
+ cov.data[2].z = pt.z * pt.z;
+
+ pt *= pt.x;
+ cov.data[0].x = pt.x;
+ cov.data[0].y = pt.y;
+ cov.data[0].z = pt.z;
+ return cov;
+ }
+ };
+
+ /** \brief Computes a centroid for a given range of points. */
+ template <class IteratorT>
+ void compute3DCentroid (IteratorT begin, IteratorT end, float3& centroid)
+ {
+ // Compute Centroid:
+ centroid.x = centroid.y = centroid.z = 0;
+ // we need a way to iterate over the inliers in the point cloud.. permutation_iterator to the rescue
+ centroid = transform_reduce (begin, end, convert_point_to_float3 (), centroid, AddPoints ());
+ centroid /= (float) (end - begin);
+ }
+
+ /** \brief Computes a covariance matrix for a given range of points. */
+ template <class IteratorT>
+ void computeCovariance (IteratorT begin, IteratorT end, CovarianceMatrix& cov, float3 centroid)
+ {
+ cov.data[0] = make_float3 (0.0f, 0.0f, 0.0f);
+ cov.data[1] = make_float3 (0.0f, 0.0f, 0.0f);
+ cov.data[2] = make_float3 (0.0f, 0.0f, 0.0f);
+
+ cov = transform_reduce (begin, end,
+ ComputeCovarianceForPoint (centroid),
+ cov,
+ AddCovariances ());
+
+ // fill in the lower triangle (symmetry)
+ cov.data[1].x = cov.data[0].y;
+ cov.data[2].x = cov.data[0].z;
+ cov.data[2].y = cov.data[1].z;
+
+ // divide by number of inliers
+ cov.data[0] /= (float) (end - begin);
+ cov.data[1] /= (float) (end - begin);
+ cov.data[2] /= (float) (end - begin);
+ }
+
+ /** Kernel to compute a radius neighborhood given a organized point cloud (aka range image cloud) */
+ template <typename CloudPtr>
+ class OrganizedRadiusSearch
+ {
+ public:
+ OrganizedRadiusSearch (const CloudPtr &input, float focalLength, float sqr_radius)
+ : points_(thrust::raw_pointer_cast (&input->points[0]))
+ , focalLength_(focalLength)
+ , width_ (input->width)
+ , height_ (input->height)
+ , sqr_radius_ (sqr_radius)
+ {}
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ // returns float4: .x = min_x, .y = max_x, .z = min_y, .w = max_y
+ // note: assumes the projection of a point falls onto the image lattice! otherwise, min_x might be bigger than max_x !!!
+ inline __host__ __device__
+ int4
+ getProjectedRadiusSearchBox (const float3& point_arg)
+ {
+ int4 res;
+ float r_quadr, z_sqr;
+ float sqrt_term_y, sqrt_term_x, norm;
+ float x_times_z, y_times_z;
+
+ // see http://www.wolframalpha.com/input/?i=solve+%5By%2Fsqrt%28f^2%2By^2%29*c-f%2Fsqrt%28f^2%2By^2%29*b%2Br%3D%3D0%2C+f%3D1%2C+y%5D
+ // where b = p_q_arg.y, c = p_q_arg.z, r = radius_arg, f = focalLength_
+
+ r_quadr = sqr_radius_ * sqr_radius_;
+ z_sqr = point_arg.z * point_arg.z;
+
+ sqrt_term_y = sqrt (point_arg.y * point_arg.y * sqr_radius_ + z_sqr * sqr_radius_ - r_quadr);
+ sqrt_term_x = sqrt (point_arg.x * point_arg.x * sqr_radius_ + z_sqr * sqr_radius_ - r_quadr);
+ //sqrt_term_y = sqrt (point_arg.y * point_arg.y * sqr_radius_ + z_sqr * sqr_radius_ - r_quadr);
+ //sqrt_term_x = sqrt (point_arg.x * point_arg.x * sqr_radius_ + z_sqr * sqr_radius_ - r_quadr);
+ norm = 1.0f / (z_sqr - sqr_radius_);
+
+ x_times_z = point_arg.x * point_arg.z;
+ y_times_z = point_arg.y * point_arg.z;
+
+ float4 bounds;
+ bounds.x = (x_times_z - sqrt_term_x) * norm;
+ bounds.y = (x_times_z + sqrt_term_x) * norm;
+ bounds.z = (y_times_z - sqrt_term_y) * norm;
+ bounds.w = (y_times_z + sqrt_term_y) * norm;
+
+ // determine 2-D search window
+ bounds *= focalLength_;
+ bounds.x += width_ / 2.0f;
+ bounds.y += width_ / 2.0f;
+ bounds.z += height_ / 2.0f;
+ bounds.w += height_ / 2.0f;
+
+ res.x = (int)floor (bounds.x);
+ res.y = (int)ceil (bounds.y);
+ res.z = (int)floor (bounds.z);
+ res.w = (int)ceil (bounds.w);
+
+ // clamp the coordinates to fit to depth image size
+ res.x = clamp (res.x, 0, width_-1);
+ res.y = clamp (res.y, 0, width_-1);
+ res.z = clamp (res.z, 0, height_-1);
+ res.w = clamp (res.w, 0, height_-1);
+ return res;
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ inline __host__ __device__
+ int radiusSearch (const float3 &query_pt, int k_indices[], int max_nnn)
+ {
+ // bounds.x = min_x, .y = max_x, .z = min_y, .w = max_y
+ int4 bounds = getProjectedRadiusSearchBox(query_pt);
+
+ int nnn = 0;
+ // iterate over all pixels in the rectangular region
+ for (int x = bounds.x; (x <= bounds.y) & (nnn < max_nnn); ++x)
+ {
+ for (int y = bounds.z; (y <= bounds.w) & (nnn < max_nnn); ++y)
+ {
+ int idx = y * width_ + x;
+
+ if (isnan (points_[idx].x))
+ continue;
+
+ float3 point_dif = points_[idx] - query_pt;
+
+ // check distance and add to results
+ if (dot (point_dif, point_dif) <= sqr_radius_)
+ {
+ k_indices[nnn] = idx;
+ ++nnn;
+ }
+ }
+ }
+
+ return nnn;
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ inline __host__ __device__
+ int computeCovarianceOnline (const float3 &query_pt, CovarianceMatrix &cov, float sqrt_desired_nr_neighbors)
+ {
+ // bounds.x = min_x, .y = max_x, .z = min_y, .w = max_y
+ //
+ //sqr_radius_ = query_pt.z * (0.2f / 4.0f);
+ //sqr_radius_ *= sqr_radius_;
+ int4 bounds = getProjectedRadiusSearchBox(query_pt);
+
+ // This implements a fixed window size in image coordinates (pixels)
+ //int2 proj_point = make_int2 ( query_pt.x/(query_pt.z/focalLength_)+width_/2.0f, query_pt.y/(query_pt.z/focalLength_)+height_/2.0f);
+ //int window_size = 1;
+ //int4 bounds = make_int4 (
+ // proj_point.x - window_size,
+ // proj_point.x + window_size,
+ // proj_point.y - window_size,
+ // proj_point.y + window_size
+ // );
+
+ // clamp the coordinates to fit to depth image size
+ bounds.x = clamp (bounds.x, 0, width_-1);
+ bounds.y = clamp (bounds.y, 0, width_-1);
+ bounds.z = clamp (bounds.z, 0, height_-1);
+ bounds.w = clamp (bounds.w, 0, height_-1);
+ //int4 bounds = getProjectedRadiusSearchBox(query_pt);
+
+ // number of points in rectangular area
+ //int boundsarea = (bounds.y-bounds.x) * (bounds.w-bounds.z);
+ //float skip = max (sqrtf ((float)boundsarea) / sqrt_desired_nr_neighbors, 1.0);
+ float skipX = max (sqrtf ((float)bounds.y-bounds.x) / sqrt_desired_nr_neighbors, 1.0f);
+ float skipY = max (sqrtf ((float)bounds.w-bounds.z) / sqrt_desired_nr_neighbors, 1.0f);
+ skipX = 1;
+ skipY = 1;
+
+ cov.data[0] = make_float3(0,0,0);
+ cov.data[1] = make_float3(0,0,0);
+ cov.data[2] = make_float3(0,0,0);
+ float3 centroid = make_float3(0,0,0);
+ int nnn = 0;
+ // iterate over all pixels in the rectangular region
+ for (float y = (float) bounds.z; y <= bounds.w; y += skipY)
+ {
+ for (float x = (float) bounds.x; x <= bounds.y; x += skipX)
+ {
+ // find index in point cloud from x,y pixel positions
+ int idx = ((int)y) * width_ + ((int)x);
+
+ // ignore invalid points
+ if (isnan (points_[idx].x) | isnan (points_[idx].y) | isnan (points_[idx].z))
+ continue;
+
+ float3 point_dif = points_[idx] - query_pt;
+
+ // check distance and update covariance matrix
+ if (dot (point_dif, point_dif) <= sqr_radius_)
+ {
+ ++nnn;
+ float3 demean_old = points_[idx] - centroid;
+ centroid += demean_old / (float) nnn;
+ float3 demean_new = points_[idx] - centroid;
+
+ cov.data[1].y += demean_new.y * demean_old.y;
+ cov.data[1].z += demean_new.y * demean_old.z;
+ cov.data[2].z += demean_new.z * demean_old.z;
+
+ demean_old *= demean_new.x;
+ cov.data[0].x += demean_old.x;
+ cov.data[0].y += demean_old.y;
+ cov.data[0].z += demean_old.z;
+ }
+ }
+ }
+
+ cov.data[1].x = cov.data[0].y;
+ cov.data[2].x = cov.data[0].z;
+ cov.data[2].y = cov.data[1].z;
+ cov.data[0] /= (float) nnn;
+ cov.data[1] /= (float) nnn;
+ cov.data[2] /= (float) nnn;
+ return nnn;
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ inline __host__ __device__
+ float3 computeCentroid (const float3 &query_pt, CovarianceMatrix &cov, float sqrt_desired_nr_neighbors)
+ {
+ // bounds.x = min_x, .y = max_x, .z = min_y, .w = max_y
+ //
+ //sqr_radius_ = query_pt.z * (0.2f / 4.0f);
+ //sqr_radius_ *= sqr_radius_;
+ int4 bounds = getProjectedRadiusSearchBox(query_pt);
+
+ // This implements a fixed window size in image coordinates (pixels)
+ //int2 proj_point = make_int2 ( query_pt.x/(query_pt.z/focalLength_)+width_/2.0f, query_pt.y/(query_pt.z/focalLength_)+height_/2.0f);
+ //int window_size = 1;
+ //int4 bounds = make_int4 (
+ // proj_point.x - window_size,
+ // proj_point.x + window_size,
+ // proj_point.y - window_size,
+ // proj_point.y + window_size
+ // );
+
+ // clamp the coordinates to fit to depth image size
+ bounds.x = clamp (bounds.x, 0, width_-1);
+ bounds.y = clamp (bounds.y, 0, width_-1);
+ bounds.z = clamp (bounds.z, 0, height_-1);
+ bounds.w = clamp (bounds.w, 0, height_-1);
+
+ // number of points in rectangular area
+ //int boundsarea = (bounds.y-bounds.x) * (bounds.w-bounds.z);
+ //float skip = max (sqrtf ((float)boundsarea) / sqrt_desired_nr_neighbors, 1.0);
+ float skipX = max (sqrtf ((float)bounds.y-bounds.x) / sqrt_desired_nr_neighbors, 1.0f);
+ float skipY = max (sqrtf ((float)bounds.w-bounds.z) / sqrt_desired_nr_neighbors, 1.0f);
+
+ skipX = 1;
+ skipY = 1;
+ float3 centroid = make_float3(0,0,0);
+ int nnn = 0;
+ // iterate over all pixels in the rectangular region
+ for (float y = (float) bounds.z; y <= bounds.w; y += skipY)
+ {
+ for (float x = (float) bounds.x; x <= bounds.y; x += skipX)
+ {
+ // find index in point cloud from x,y pixel positions
+ int idx = ((int)y) * width_ + ((int)x);
+
+ // ignore invalid points
+ if (isnan (points_[idx].x) | isnan (points_[idx].y) | isnan (points_[idx].z))
+ continue;
+
+ float3 point_dif = points_[idx] - query_pt;
+
+ // check distance and update covariance matrix
+ if (dot (point_dif, point_dif) <= sqr_radius_)
+ {
+ centroid += points_[idx];
+ ++nnn;
+ }
+ }
+ }
+
+ return centroid / (float) nnn;
+ }
+
+ float focalLength_;
+ const PointXYZRGB *points_;
+ int width_, height_;
+ float sqr_radius_;
+ };
+
+ } // namespace
+} // namespace
+
+#endif //#ifndef PCL_CUDA_EIGEN_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_CUDA_COMMON_POINT_TYPE_RGB_H_
+#define PCL_CUDA_COMMON_POINT_TYPE_RGB_H_
+
+#include <cuda.h>
+#include <pcl/cuda/cutil_math.h>
+
+#ifdef RGB
+# undef RGB
+#endif
+
+namespace pcl
+{
+namespace cuda
+{
+
+ /** \brief Default RGB structure, defined as a union over 4 chars. */
+ union RGB
+ {
+ int rgb;
+ struct
+ {
+ char r;
+ char g;
+ char b;
+ char alpha;
+ };
+
+ inline __host__ __device__ RGB () {}
+ inline __host__ __device__ RGB (int _rgb) : rgb(_rgb) {}
+ inline __host__ __device__ RGB (char _r, char _g, char _b, char _alpha) :
+ r(_r), g(_g), b(_b), alpha(_alpha) {}
+
+ inline __host__ __device__ bool operator == (const RGB &rhs)
+ {
+ return (r == rhs.r && g == rhs.g && b == rhs.b && alpha == rhs.alpha);
+ }
+
+ inline __host__ __device__ RGB& operator - (RGB &rhs)
+ {
+ r = -r;
+ g = -g;
+ b = -b;
+ alpha = -alpha;
+ return (*this);
+ }
+
+ inline __host__ __device__ RGB& operator += (const RGB &rhs)
+ {
+ r += rhs.r;
+ g += rhs.g;
+ b += rhs.b;
+ alpha += rhs.alpha;
+ return (*this);
+ }
+
+ inline __host__ __device__ RGB& operator -= (const RGB &rhs)
+ {
+ r -= rhs.r;
+ g -= rhs.g;
+ b -= rhs.b;
+ alpha -= rhs.alpha;
+ return (*this);
+ }
+
+ inline __host__ __device__ RGB& operator *= (const RGB &rhs)
+ {
+ r *= rhs.r;
+ g *= rhs.g;
+ b *= rhs.b;
+ alpha *= rhs.alpha;
+ return (*this);
+ }
+
+ inline __host__ __device__ RGB& operator /= (const RGB &rhs)
+ {
+ r /= rhs.r;
+ g /= rhs.g;
+ b /= rhs.b;
+ alpha /= rhs.alpha;
+ return (*this);
+ }
+ };
+
+} // namespace
+} // namespace
+#endif //#ifndef PCL_CUDA_COMMON_POINT_TYPE_RGB_H_
+
--- /dev/null
+/*
+ * Copyright 1993-2010 NVIDIA Corporation. All rights reserved.
+ *
+ * Please refer to the NVIDIA end user license agreement (EULA) associated
+ * with this source code for terms and conditions that govern your use of
+ * this software. Any use, reproduction, disclosure, or distribution of
+ * this software and related documentation outside the terms of the EULA
+ * is strictly prohibited.
+ *
+ */
+
+ /*
+* Copyright 1993-2010 NVIDIA Corporation. All rights reserved.
+*
+* Please refer to the NVIDIA end user license agreement (EULA) associated
+* with this source code for terms and conditions that govern your use of
+* this software. Any use, reproduction, disclosure, or distribution of
+* this software and related documentation outside the terms of the EULA
+* is strictly prohibited.
+*
+*/
+
+
+/* CUda UTility Library */
+
+#ifndef _CUTIL_H_
+#define _CUTIL_H_
+
+#ifdef _WIN32
+# pragma warning( disable : 4996 ) // disable deprecated warning
+#endif
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ // helper typedefs for building DLL
+#ifdef _WIN32
+# ifdef BUILD_DLL
+# define DLL_MAPPING __declspec(dllexport)
+# else
+# define DLL_MAPPING __declspec(dllimport)
+# endif
+#else
+# define DLL_MAPPING
+#endif
+
+#ifdef _WIN32
+ #define CUTIL_API __stdcall
+#else
+ #define CUTIL_API
+#endif
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! CUT bool type
+ ////////////////////////////////////////////////////////////////////////////
+ enum CUTBoolean
+ {
+ CUTFalse = 0,
+ CUTTrue = 1
+ };
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Deallocate memory allocated within Cutil
+ //! @param pointer to memory
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ void CUTIL_API
+ cutFree( void* ptr);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Helper for bank conflict checking (should only be used with the
+ //! CUT_BANK_CHECKER macro)
+ //! @param tidx thread id in x dimension of block
+ //! @param tidy thread id in y dimension of block
+ //! @param tidz thread id in z dimension of block
+ //! @param bdimx block size in x dimension
+ //! @param bdimy block size in y dimension
+ //! @param bdimz block size in z dimension
+ //! @param file name of the source file where the access takes place
+ //! @param line line in the source file where the access takes place
+ //! @param aname name of the array which is accessed
+ //! @param index index into the array
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ void CUTIL_API
+ cutCheckBankAccess( unsigned int tidx, unsigned int tidy, unsigned int tidz,
+ unsigned int bdimx, unsigned int bdimy,
+ unsigned int bdimz, const char* file, const int line,
+ const char* aname, const int index);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Find the path for a filename
+ //! @return the path if succeeded, otherwise 0
+ //! @param filename name of the file
+ //! @param executablePath optional absolute path of the executable
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ char* CUTIL_API
+ cutFindFilePath(const char* filename, const char* executablePath);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Read file \filename containing single precision floating point data
+ //! @return CUTTrue if reading the file succeeded, otherwise false
+ //! @param filename name of the source file
+ //! @param data uninitialized pointer, returned initialized and pointing to
+ //! the data read
+ //! @param len number of data elements in data, -1 on error
+ //! @note If a NULL pointer is passed to this function and it is
+ //! initialized within Cutil then cutFree() has to be used to
+ //! deallocate the memory
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutReadFilef( const char* filename, float** data, unsigned int* len,
+ bool verbose = false);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Read file \filename containing double precision floating point data
+ //! @return CUTTrue if reading the file succeeded, otherwise false
+ //! @param filename name of the source file
+ //! @param data uninitialized pointer, returned initialized and pointing to
+ //! the data read
+ //! @param len number of data elements in data, -1 on error
+ //! @note If a NULL pointer is passed to this function and it is
+ //! initialized within Cutil then cutFree() has to be used to
+ //! deallocate the memory
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutReadFiled( const char* filename, double** data, unsigned int* len,
+ bool verbose = false);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Read file \filename containing integer data
+ //! @return CUTTrue if reading the file succeeded, otherwise false
+ //! @param filename name of the source file
+ //! @param data uninitialized pointer, returned initialized and pointing to
+ //! the data read
+ //! @param len number of data elements in data, -1 on error
+ //! @note If a NULL pointer is passed to this function and it is
+ //! initialized within Cutil then cutFree() has to be used to
+ //! deallocate the memory
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutReadFilei( const char* filename, int** data, unsigned int* len, bool verbose = false);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Read file \filename containing unsigned integer data
+ //! @return CUTTrue if reading the file succeeded, otherwise false
+ //! @param filename name of the source file
+ //! @param data uninitialized pointer, returned initialized and pointing to
+ //! the data read
+ //! @param len number of data elements in data, -1 on error
+ //! @note If a NULL pointer is passed to this function and it is
+ //! initialized within Cutil then cutFree() has to be used to
+ //! deallocate the memory
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutReadFileui( const char* filename, unsigned int** data,
+ unsigned int* len, bool verbose = false);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Read file \filename containing char / byte data
+ //! @return CUTTrue if reading the file succeeded, otherwise false
+ //! @param filename name of the source file
+ //! @param data uninitialized pointer, returned initialized and pointing to
+ //! the data read
+ //! @param len number of data elements in data, -1 on error
+ //! @note If a NULL pointer is passed to this function and it is
+ //! initialized within Cutil then cutFree() has to be used to
+ //! deallocate the memory
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutReadFileb( const char* filename, char** data, unsigned int* len,
+ bool verbose = false);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Read file \filename containing unsigned char / byte data
+ //! @return CUTTrue if reading the file succeeded, otherwise false
+ //! @param filename name of the source file
+ //! @param data uninitialized pointer, returned initialized and pointing to
+ //! the data read
+ //! @param len number of data elements in data, -1 on error
+ //! @note If a NULL pointer is passed to this function and it is
+ //! initialized within Cutil then cutFree() has to be used to
+ //! deallocate the memory
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutReadFileub( const char* filename, unsigned char** data,
+ unsigned int* len, bool verbose = false);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Write a data file \filename containing single precision floating point
+ //! data
+ //! @return CUTTrue if writing the file succeeded, otherwise false
+ //! @param filename name of the file to write
+ //! @param data pointer to data to write
+ //! @param len number of data elements in data, -1 on error
+ //! @param epsilon epsilon for comparison
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutWriteFilef( const char* filename, const float* data, unsigned int len,
+ const float epsilon, bool verbose = false);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Write a data file \filename containing double precision floating point
+ //! data
+ //! @return CUTTrue if writing the file succeeded, otherwise false
+ //! @param filename name of the file to write
+ //! @param data pointer to data to write
+ //! @param len number of data elements in data, -1 on error
+ //! @param epsilon epsilon for comparison
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutWriteFiled( const char* filename, const float* data, unsigned int len,
+ const double epsilon, bool verbose = false);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Write a data file \filename containing integer data
+ //! @return CUTTrue if writing the file succeeded, otherwise false
+ //! @param filename name of the file to write
+ //! @param data pointer to data to write
+ //! @param len number of data elements in data, -1 on error
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutWriteFilei( const char* filename, const int* data, unsigned int len,
+ bool verbose = false);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Write a data file \filename containing unsigned integer data
+ //! @return CUTTrue if writing the file succeeded, otherwise false
+ //! @param filename name of the file to write
+ //! @param data pointer to data to write
+ //! @param len number of data elements in data, -1 on error
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutWriteFileui( const char* filename,const unsigned int* data,
+ unsigned int len, bool verbose = false);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Write a data file \filename containing char / byte data
+ //! @return CUTTrue if writing the file succeeded, otherwise false
+ //! @param filename name of the file to write
+ //! @param data pointer to data to write
+ //! @param len number of data elements in data, -1 on error
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutWriteFileb( const char* filename, const char* data, unsigned int len,
+ bool verbose = false);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Write a data file \filename containing unsigned char / byte data
+ //! @return CUTTrue if writing the file succeeded, otherwise false
+ //! @param filename name of the file to write
+ //! @param data pointer to data to write
+ //! @param len number of data elements in data, -1 on error
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutWriteFileub( const char* filename,const unsigned char* data,
+ unsigned int len, bool verbose = false);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Load PGM image file (with unsigned char as data element type)
+ //! @return CUTTrue if reading the file succeeded, otherwise false
+ //! @param file name of the image file
+ //! @param data handle to the data read
+ //! @param w width of the image
+ //! @param h height of the image
+ //! @note If a NULL pointer is passed to this function and it is
+ //! initialized within Cutil then cutFree() has to be used to
+ //! deallocate the memory
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutLoadPGMub( const char* file, unsigned char** data,
+ unsigned int *w,unsigned int *h);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Load PPM image file (with unsigned char as data element type)
+ //! @return CUTTrue if reading the file succeeded, otherwise false
+ //! @param file name of the image file
+ //! @param data handle to the data read
+ //! @param w width of the image
+ //! @param h height of the image
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutLoadPPMub( const char* file, unsigned char** data,
+ unsigned int *w,unsigned int *h);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Load PPM image file (with unsigned char as data element type), padding
+ //! 4th component
+ //! @return CUTTrue if reading the file succeeded, otherwise false
+ //! @param file name of the image file
+ //! @param data handle to the data read
+ //! @param w width of the image
+ //! @param h height of the image
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutLoadPPM4ub( const char* file, unsigned char** data,
+ unsigned int *w,unsigned int *h);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Load PGM image file (with unsigned int as data element type)
+ //! @return CUTTrue if reading the file succeeded, otherwise false
+ //! @param file name of the image file
+ //! @param data handle to the data read
+ //! @param w width of the image
+ //! @param h height of the image
+ //! @note If a NULL pointer is passed to this function and it is
+ //! initialized within Cutil then cutFree() has to be used to
+ //! deallocate the memory
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutLoadPGMi( const char* file, unsigned int** data,
+ unsigned int* w, unsigned int* h);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Load PGM image file (with unsigned short as data element type)
+ //! @return CUTTrue if reading the file succeeded, otherwise false
+ //! @param file name of the image file
+ //! @param data handle to the data read
+ //! @param w width of the image
+ //! @param h height of the image
+ //! @note If a NULL pointer is passed to this function and it is
+ //! initialized withing Cutil then cutFree() has to be used to
+ //! deallocate the memory
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutLoadPGMs( const char* file, unsigned short** data,
+ unsigned int* w, unsigned int* h);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Load PGM image file (with float as data element type)
+ //! @param file name of the image file
+ //! @param data handle to the data read
+ //! @param w width of the image
+ //! @param h height of the image
+ //! @note If a NULL pointer is passed to this function and it is
+ //! initialized withing Cutil then cutFree() has to be used to
+ //! deallocate the memory
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutLoadPGMf( const char* file, float** data,
+ unsigned int* w, unsigned int* h);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Save PGM image file (with unsigned char as data element type)
+ //! @param file name of the image file
+ //! @param data handle to the data read
+ //! @param w width of the image
+ //! @param h height of the image
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutSavePGMub( const char* file, unsigned char* data,
+ unsigned int w, unsigned int h);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Save PPM image file (with unsigned char as data element type)
+ //! @param file name of the image file
+ //! @param data handle to the data read
+ //! @param w width of the image
+ //! @param h height of the image
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutSavePPMub( const char* file, unsigned char *data,
+ unsigned int w, unsigned int h);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Save PPM image file (with unsigned char as data element type, padded to
+ //! 4 bytes)
+ //! @param file name of the image file
+ //! @param data handle to the data read
+ //! @param w width of the image
+ //! @param h height of the image
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutSavePPM4ub( const char* file, unsigned char *data,
+ unsigned int w, unsigned int h);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Save PGM image file (with unsigned int as data element type)
+ //! @param file name of the image file
+ //! @param data handle to the data read
+ //! @param w width of the image
+ //! @param h height of the image
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutSavePGMi( const char* file, unsigned int* data,
+ unsigned int w, unsigned int h);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Save PGM image file (with unsigned short as data element type)
+ //! @param file name of the image file
+ //! @param data handle to the data read
+ //! @param w width of the image
+ //! @param h height of the image
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutSavePGMs( const char* file, unsigned short* data,
+ unsigned int w, unsigned int h);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Save PGM image file (with float as data element type)
+ //! @param file name of the image file
+ //! @param data handle to the data read
+ //! @param w width of the image
+ //! @param h height of the image
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutSavePGMf( const char* file, float* data,
+ unsigned int w, unsigned int h);
+
+ ////////////////////////////////////////////////////////////////////////////
+ // Command line arguments: General notes
+ // * All command line arguments begin with '--' followed by the token;
+ // token and value are seperated by '='; example --samples=50
+ // * Arrays have the form --model=[one.obj,two.obj,three.obj]
+ // (without whitespaces)
+ ////////////////////////////////////////////////////////////////////////////
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Check if command line argument \a flag-name is given
+ //! @return CUTTrue if command line argument \a flag_name has been given,
+ //! otherwise 0
+ //! @param argc argc as passed to main()
+ //! @param argv argv as passed to main()
+ //! @param flag_name name of command line flag
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutCheckCmdLineFlag( const int argc, const char** argv,
+ const char* flag_name);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Get the value of a command line argument of type int
+ //! @return CUTTrue if command line argument \a arg_name has been given and
+ //! is of the requested type, otherwise CUTFalse
+ //! @param argc argc as passed to main()
+ //! @param argv argv as passed to main()
+ //! @param arg_name name of the command line argument
+ //! @param val value of the command line argument
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutGetCmdLineArgumenti( const int argc, const char** argv,
+ const char* arg_name, int* val);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Get the value of a command line argument of type float
+ //! @return CUTTrue if command line argument \a arg_name has been given and
+ //! is of the requested type, otherwise CUTFalse
+ //! @param argc argc as passed to main()
+ //! @param argv argv as passed to main()
+ //! @param arg_name name of the command line argument
+ //! @param val value of the command line argument
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutGetCmdLineArgumentf( const int argc, const char** argv,
+ const char* arg_name, float* val);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Get the value of a command line argument of type string
+ //! @return CUTTrue if command line argument \a arg_name has been given and
+ //! is of the requested type, otherwise CUTFalse
+ //! @param argc argc as passed to main()
+ //! @param argv argv as passed to main()
+ //! @param arg_name name of the command line argument
+ //! @param val value of the command line argument
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutGetCmdLineArgumentstr( const int argc, const char** argv,
+ const char* arg_name, char** val);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Get the value of a command line argument list those element are strings
+ //! @return CUTTrue if command line argument \a arg_name has been given and
+ //! is of the requested type, otherwise CUTFalse
+ //! @param argc argc as passed to main()
+ //! @param argv argv as passed to main()
+ //! @param arg_name name of the command line argument
+ //! @param val command line argument list
+ //! @param len length of the list / number of elements
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutGetCmdLineArgumentListstr( const int argc, const char** argv,
+ const char* arg_name, char** val,
+ unsigned int* len);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Extended assert
+ //! @return CUTTrue if the condition \a val holds, otherwise CUTFalse
+ //! @param val condition to test
+ //! @param file __FILE__ macro
+ //! @param line __LINE__ macro
+ //! @note This function should be used via the CONDITION(val) macro
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutCheckCondition( int val, const char* file, const int line);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Compare two float arrays
+ //! @return CUTTrue if \a reference and \a data are identical,
+ //! otherwise CUTFalse
+ //! @param reference handle to the reference data / gold image
+ //! @param data handle to the computed data
+ //! @param len number of elements in reference and data
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutComparef( const float* reference, const float* data,
+ const unsigned int len);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Compare two integer arrays
+ //! @return CUTTrue if \a reference and \a data are identical,
+ //! otherwise CUTFalse
+ //! @param reference handle to the reference data / gold image
+ //! @param data handle to the computed data
+ //! @param len number of elements in reference and data
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutComparei( const int* reference, const int* data,
+ const unsigned int len );
+
+ ////////////////////////////////////////////////////////////////////////////////
+ //! Compare two unsigned integer arrays, with epsilon and threshold
+ //! @return CUTTrue if \a reference and \a data are identical,
+ //! otherwise CUTFalse
+ //! @param reference handle to the reference data / gold image
+ //! @param data handle to the computed data
+ //! @param len number of elements in reference and data
+ //! @param threshold tolerance % # of comparison errors (0.15f = 15%)
+ ////////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutCompareuit( const unsigned int* reference, const unsigned int* data,
+ const unsigned int len, const float epsilon, const float threshold );
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Compare two unsigned char arrays
+ //! @return CUTTrue if \a reference and \a data are identical,
+ //! otherwise CUTFalse
+ //! @param reference handle to the reference data / gold image
+ //! @param data handle to the computed data
+ //! @param len number of elements in reference and data
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutCompareub( const unsigned char* reference, const unsigned char* data,
+ const unsigned int len );
+
+ ////////////////////////////////////////////////////////////////////////////////
+ //! Compare two integers with a tolernance for # of byte errors
+ //! @return CUTTrue if \a reference and \a data are identical,
+ //! otherwise CUTFalse
+ //! @param reference handle to the reference data / gold image
+ //! @param data handle to the computed data
+ //! @param len number of elements in reference and data
+ //! @param epsilon epsilon to use for the comparison
+ //! @param threshold tolerance % # of comparison errors (0.15f = 15%)
+ ////////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutCompareubt( const unsigned char* reference, const unsigned char* data,
+ const unsigned int len, const float epsilon, const float threshold );
+
+ ////////////////////////////////////////////////////////////////////////////////
+ //! Compare two integer arrays witha n epsilon tolerance for equality
+ //! @return CUTTrue if \a reference and \a data are identical,
+ //! otherwise CUTFalse
+ //! @param reference handle to the reference data / gold image
+ //! @param data handle to the computed data
+ //! @param len number of elements in reference and data
+ //! @param epsilon epsilon to use for the comparison
+ ////////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutCompareube( const unsigned char* reference, const unsigned char* data,
+ const unsigned int len, const float epsilon );
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Compare two float arrays with an epsilon tolerance for equality
+ //! @return CUTTrue if \a reference and \a data are identical,
+ //! otherwise CUTFalse
+ //! @param reference handle to the reference data / gold image
+ //! @param data handle to the computed data
+ //! @param len number of elements in reference and data
+ //! @param epsilon epsilon to use for the comparison
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutComparefe( const float* reference, const float* data,
+ const unsigned int len, const float epsilon );
+
+ ////////////////////////////////////////////////////////////////////////////////
+ //! Compare two float arrays with an epsilon tolerance for equality and a
+ //! threshold for # pixel errors
+ //! @return CUTTrue if \a reference and \a data are identical,
+ //! otherwise CUTFalse
+ //! @param reference handle to the reference data / gold image
+ //! @param data handle to the computed data
+ //! @param len number of elements in reference and data
+ //! @param epsilon epsilon to use for the comparison
+ ////////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutComparefet( const float* reference, const float* data,
+ const unsigned int len, const float epsilon, const float threshold );
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Compare two float arrays using L2-norm with an epsilon tolerance for
+ //! equality
+ //! @return CUTTrue if \a reference and \a data are identical,
+ //! otherwise CUTFalse
+ //! @param reference handle to the reference data / gold image
+ //! @param data handle to the computed data
+ //! @param len number of elements in reference and data
+ //! @param epsilon epsilon to use for the comparison
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutCompareL2fe( const float* reference, const float* data,
+ const unsigned int len, const float epsilon );
+
+ ////////////////////////////////////////////////////////////////////////////////
+ //! Compare two PPM image files with an epsilon tolerance for equality
+ //! @return CUTTrue if \a reference and \a data are identical,
+ //! otherwise CUTFalse
+ //! @param src_file filename for the image to be compared
+ //! @param data filename for the reference data / gold image
+ //! @param epsilon epsilon to use for the comparison
+ //! @param threshold threshold of pixels that can still mismatch to pass (i.e. 0.15f = 15% must pass)
+ //! $param verboseErrors output details of image mismatch to std::err
+ ////////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutComparePPM( const char *src_file, const char *ref_file, const float epsilon, const float threshold, bool verboseErrors = false );
+
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Timer functionality
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Create a new timer
+ //! @return CUTTrue if a time has been created, otherwise false
+ //! @param name of the new timer, 0 if the creation failed
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutCreateTimer( unsigned int* name);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Delete a timer
+ //! @return CUTTrue if a time has been deleted, otherwise false
+ //! @param name of the timer to delete
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutDeleteTimer( unsigned int name);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Start the time with name \a name
+ //! @param name name of the timer to start
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutStartTimer( const unsigned int name);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Stop the time with name \a name. Does not reset.
+ //! @param name name of the timer to stop
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutStopTimer( const unsigned int name);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Resets the timer's counter.
+ //! @param name name of the timer to reset.
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ CUTBoolean CUTIL_API
+ cutResetTimer( const unsigned int name);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Returns total execution time in milliseconds for the timer over all
+ //! runs since the last reset or timer creation.
+ //! @param name name of the timer to return the time of
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ float CUTIL_API
+ cutGetTimerValue( const unsigned int name);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Return the average time in milliseconds for timer execution as the
+ //! total time for the timer dividied by the number of completed (stopped)
+ //! runs the timer has made.
+ //! Excludes the current running time if the timer is currently running.
+ //! @param name name of the timer to return the time of
+ ////////////////////////////////////////////////////////////////////////////
+ DLL_MAPPING
+ float CUTIL_API
+ cutGetAverageTimerValue( const unsigned int name);
+
+ ////////////////////////////////////////////////////////////////////////////
+ //! Macros
+
+#if CUDART_VERSION >= 4000
+#define CUT_DEVICE_SYNCHRONIZE( ) cudaDeviceSynchronize();
+#else
+#define CUT_DEVICE_SYNCHRONIZE( ) cudaThreadSynchronize();
+#endif
+
+#if CUDART_VERSION >= 4000
+#define CUT_DEVICE_RESET( ) cudaDeviceReset();
+#else
+#define CUT_DEVICE_RESET( ) cudaThreadExit();
+#endif
+
+// This is for the CUTIL bank checker
+#ifdef _DEBUG
+ #if __DEVICE_EMULATION__
+ // Interface for bank conflict checker
+ #define CUT_BANK_CHECKER( array, index) \
+ (cutCheckBankAccess( threadIdx.x, threadIdx.y, threadIdx.z, blockDim.x, \
+ blockDim.y, blockDim.z, \
+ __FILE__, __LINE__, #array, index ), \
+ array[index])
+ #else
+ #define CUT_BANK_CHECKER( array, index) array[index]
+ #endif
+#else
+ #define CUT_BANK_CHECKER( array, index) array[index]
+#endif
+
+# define CU_SAFE_CALL_NO_SYNC( call ) { \
+ CUresult err = call; \
+ if( CUDA_SUCCESS != err) { \
+ fprintf(stderr, "Cuda driver error %x in file '%s' in line %i.\n", \
+ err, __FILE__, __LINE__ ); \
+ exit(EXIT_FAILURE); \
+ } }
+
+# define CU_SAFE_CALL( call ) CU_SAFE_CALL_NO_SYNC(call);
+
+# define CU_SAFE_CTX_SYNC( ) { \
+ CUresult err = cuCtxSynchronize(); \
+ if( CUDA_SUCCESS != err) { \
+ fprintf(stderr, "Cuda driver error %x in file '%s' in line %i.\n", \
+ err, __FILE__, __LINE__ ); \
+ exit(EXIT_FAILURE); \
+ } }
+
+# define CUDA_SAFE_CALL_NO_SYNC( call) { \
+ cudaError err = call; \
+ if( cudaSuccess != err) { \
+ fprintf(stderr, "Cuda error in file '%s' in line %i : %s.\n", \
+ __FILE__, __LINE__, cudaGetErrorString( err) ); \
+ exit(EXIT_FAILURE); \
+ } }
+
+# define CUDA_SAFE_CALL( call) CUDA_SAFE_CALL_NO_SYNC(call); \
+
+# define CUDA_SAFE_THREAD_SYNC( ) { \
+ cudaError err = CUT_DEVICE_SYNCHRONIZE(); \
+ if ( cudaSuccess != err) { \
+ fprintf(stderr, "Cuda error in file '%s' in line %i : %s.\n", \
+ __FILE__, __LINE__, cudaGetErrorString( err) ); \
+ } }
+
+# define CUFFT_SAFE_CALL( call) { \
+ cufftResult err = call; \
+ if( CUFFT_SUCCESS != err) { \
+ fprintf(stderr, "CUFFT error in file '%s' in line %i.\n", \
+ __FILE__, __LINE__); \
+ exit(EXIT_FAILURE); \
+ } }
+
+# define CUT_SAFE_CALL( call) \
+ if( CUTTrue != call) { \
+ fprintf(stderr, "Cut error in file '%s' in line %i.\n", \
+ __FILE__, __LINE__); \
+ exit(EXIT_FAILURE); \
+ }
+
+ //! Check for CUDA error
+#ifdef _DEBUG
+# define CUT_CHECK_ERROR(errorMessage) { \
+ cudaError_t err = cudaGetLastError(); \
+ if( cudaSuccess != err) { \
+ fprintf(stderr, "Cuda error: %s in file '%s' in line %i : %s.\n", \
+ errorMessage, __FILE__, __LINE__, cudaGetErrorString( err) );\
+ exit(EXIT_FAILURE); \
+ } \
+ err = CUT_DEVICE_SYNCHRONIZE(); \
+ if( cudaSuccess != err) { \
+ fprintf(stderr, "Cuda error: %s in file '%s' in line %i : %s.\n", \
+ errorMessage, __FILE__, __LINE__, cudaGetErrorString( err) );\
+ exit(EXIT_FAILURE); \
+ } \
+ }
+#else
+# define CUT_CHECK_ERROR(errorMessage) { \
+ cudaError_t err = cudaGetLastError(); \
+ if( cudaSuccess != err) { \
+ fprintf(stderr, "Cuda error: %s in file '%s' in line %i : %s.\n", \
+ errorMessage, __FILE__, __LINE__, cudaGetErrorString( err) );\
+ exit(EXIT_FAILURE); \
+ } \
+ }
+#endif
+
+ //! Check for malloc error
+# define CUT_SAFE_MALLOC( mallocCall ) { \
+ if( !(mallocCall)) { \
+ fprintf(stderr, "Host malloc failure in file '%s' in line %i\n", \
+ __FILE__, __LINE__); \
+ exit(EXIT_FAILURE); \
+ } } while(0);
+
+ //! Check if conditon is true (flexible assert)
+# define CUT_CONDITION( val) \
+ if( CUTFalse == cutCheckCondition( val, __FILE__, __LINE__)) { \
+ exit(EXIT_FAILURE); \
+ }
+
+#if __DEVICE_EMULATION__
+
+# define CUT_DEVICE_INIT(ARGC, ARGV)
+
+#else
+
+# define CUT_DEVICE_INIT(ARGC, ARGV) { \
+ int deviceCount; \
+ CUDA_SAFE_CALL_NO_SYNC(cudaGetDeviceCount(&deviceCount)); \
+ if (deviceCount == 0) { \
+ fprintf(stderr, "cutil error: no devices supporting CUDA.\n"); \
+ exit(EXIT_FAILURE); \
+ } \
+ int dev = 0; \
+ cutGetCmdLineArgumenti(ARGC, (const char **) ARGV, "device", &dev); \
+ if (dev < 0) dev = 0; \
+ if (dev > deviceCount-1) dev = deviceCount - 1; \
+ cudaDeviceProp deviceProp; \
+ CUDA_SAFE_CALL_NO_SYNC(cudaGetDeviceProperties(&deviceProp, dev)); \
+ if (deviceProp.major < 1) { \
+ fprintf(stderr, "cutil error: device does not support CUDA.\n"); \
+ exit(EXIT_FAILURE); \
+ } \
+ if (cutCheckCmdLineFlag(ARGC, (const char **) ARGV, "quiet") == CUTFalse) \
+ fprintf(stderr, "Using device %d: %s\n", dev, deviceProp.name); \
+ CUDA_SAFE_CALL(cudaSetDevice(dev)); \
+}
+
+
+ //! Check for CUDA context lost
+# define CUDA_CHECK_CTX_LOST(errorMessage) { \
+ cudaError_t err = cudaGetLastError(); \
+ if( cudaSuccess != err) { \
+ fprintf(stderr, "Cuda error: %s in file '%s' in line %i : %s.\n", \
+ errorMessage, __FILE__, __LINE__, cudaGetErrorString( err) );\
+ exit(EXIT_FAILURE); \
+ } \
+ err = CUT_DEVICE_SYNCHRONIZE(); \
+ if( cudaSuccess != err) { \
+ fprintf(stderr, "Cuda error: %s in file '%s' in line %i : %s.\n", \
+ errorMessage, __FILE__, __LINE__, cudaGetErrorString( err) );\
+ exit(EXIT_FAILURE); \
+ } }
+
+//! Check for CUDA context lost
+# define CU_CHECK_CTX_LOST(errorMessage) { \
+ cudaError_t err = cudaGetLastError(); \
+ if( CUDA_ERROR_INVALID_CONTEXT != err) { \
+ fprintf(stderr, "Cuda error: %s in file '%s' in line %i : %s.\n", \
+ errorMessage, __FILE__, __LINE__, cudaGetErrorString( err) );\
+ exit(EXIT_FAILURE); \
+ } \
+ err = CUT_DEVICE_SYNCHRONIZE(); \
+ if( cudaSuccess != err) { \
+ fprintf(stderr, "Cuda error: %s in file '%s' in line %i : %s.\n", \
+ errorMessage, __FILE__, __LINE__, cudaGetErrorString( err) );\
+ exit(EXIT_FAILURE); \
+ } }
+
+
+#endif
+
+# define CUT_DEVICE_INIT_DRV(cuDevice, ARGC, ARGV) { \
+ cuDevice = 0; \
+ int deviceCount = 0; \
+ CUresult err = cuInit(0); \
+ if (CUDA_SUCCESS == err) \
+ CU_SAFE_CALL_NO_SYNC(cuDeviceGetCount(&deviceCount)); \
+ if (deviceCount == 0) { \
+ fprintf(stderr, "cutil error: no devices supporting CUDA\n"); \
+ exit(EXIT_FAILURE); \
+ } \
+ int dev = 0; \
+ cutGetCmdLineArgumenti(ARGC, (const char **) ARGV, "device", &dev); \
+ if (dev < 0) dev = 0; \
+ if (dev > deviceCount-1) dev = deviceCount - 1; \
+ CU_SAFE_CALL_NO_SYNC(cuDeviceGet(&cuDevice, dev)); \
+ char name[100]; \
+ cuDeviceGetName(name, 100, cuDevice); \
+ if (cutCheckCmdLineFlag(ARGC, (const char **) ARGV, "quiet") == CUTFalse) \
+ fprintf(stderr, "Using device %d: %s\n", dev, name); \
+}
+
+#define CUT_EXIT(argc, argv) \
+ if (!cutCheckCmdLineFlag(argc, (const char**)argv, "noprompt")) { \
+ printf("\nPress ENTER to exit...\n"); \
+ fflush( stdout); \
+ fflush( stderr); \
+ getchar(); \
+ } \
+ exit(EXIT_SUCCESS);
+
+
+#ifdef __cplusplus
+}
+#endif // #ifdef _DEBUG (else branch)
+
+#endif // #ifndef _CUTIL_H_
--- /dev/null
+/*
+ * Copyright 1993-2010 NVIDIA Corporation. All rights reserved.
+ *
+ * Please refer to the NVIDIA end user license agreement (EULA) associated
+ * with this source code for terms and conditions that govern your use of
+ * this software. Any use, reproduction, disclosure, or distribution of
+ * this software and related documentation outside the terms of the EULA
+ * is strictly prohibited.
+ *
+ */
+
+#ifndef _CUTIL_INLINE_H_
+#define _CUTIL_INLINE_H_
+
+#include <cuda.h>
+#include <pcl/cuda/cutil.h>
+#include <cuda_runtime_api.h>
+
+#include <pcl/cuda/cutil_inline_bankchecker.h>
+#include <pcl/cuda/cutil_inline_runtime.h>
+#include <pcl/cuda/cutil_inline_drvapi.h>
+
+inline void print_NVCC_min_spec(const char *sSDKsample, const char *sNVCCReq, const char *sDriverReq)
+{
+ printf("CUDA %d.%02d Toolkit built this project.\n", CUDART_VERSION/1000, (CUDART_VERSION%100));
+ printf(" [ %s ] requirements:\n", sSDKsample);
+ printf(" -> CUDA %s Toolkit\n" , sNVCCReq);
+ printf(" -> %s NVIDIA Display Driver.\n", sDriverReq);
+}
+
+#define ALIGN_OFFSET(offset, alignment) offset = (offset + (alignment-1)) & ~((alignment-1))
+
+
+#endif // _CUTIL_INLINE_H_
--- /dev/null
+/*
+ * Copyright 1993-2010 NVIDIA Corporation. All rights reserved.
+ *
+ * Please refer to the NVIDIA end user license agreement (EULA) associated
+ * with this source code for terms and conditions that govern your use of
+ * this software. Any use, reproduction, disclosure, or distribution of
+ * this software and related documentation outside the terms of the EULA
+ * is strictly prohibited.
+ *
+ */
+
+ #ifndef _CUTIL_INLINE_BANKCHECKER_H_
+#define _CUTIL_INLINE_BANKCHECKER_H_
+
+#ifdef _DEBUG
+ #if __DEVICE_EMULATION__
+ #define cutilBankChecker(array, idx) (__cutilBankChecker (threadIdx.x, threadIdx.y, threadIdx.z, \
+ blockDim.x, blockDim.y, blockDim.z, \
+ #array, idx, __FILE__, __LINE__), \
+ array[idx])
+
+ #else
+ #define cutilBankChecker(array, idx) array[idx]
+ #endif
+#else
+ #define cutilBankChecker(array, idx) array[idx]
+#endif
+
+ // Interface for bank conflict checker
+inline void __cutilBankChecker(unsigned int tidx, unsigned int tidy, unsigned int tidz,
+ unsigned int bdimx, unsigned int bdimy, unsigned int bdimz,
+ char *aname, int index, char *file, int line)
+{
+ cutCheckBankAccess( tidx, tidy, tidz, bdimx, bdimy, bdimz, file, line, aname, index);
+}
+
+#endif // _CUTIL_INLINE_BANKCHECKER_H_
--- /dev/null
+/*
+ * Copyright 1993-2010 NVIDIA Corporation. All rights reserved.
+ *
+ * Please refer to the NVIDIA end user license agreement (EULA) associated
+ * with this source code for terms and conditions that govern your use of
+ * this software. Any use, reproduction, disclosure, or distribution of
+ * this software and related documentation outside the terms of the EULA
+ * is strictly prohibited.
+ *
+ */
+
+#ifndef _CUTIL_INLINE_FUNCTIONS_DRVAPI_H_
+#define _CUTIL_INLINE_FUNCTIONS_DRVAPI_H_
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+
+
+// We define these calls here, so the user doesn't need to include __FILE__ and __LINE__
+// The advantage is the developers gets to use the inline function so they can debug
+#define cutilDrvSafeCallNoSync(err) __cuSafeCallNoSync (err, __FILE__, __LINE__)
+#define cutilDrvSafeCall(err) __cuSafeCall (err, __FILE__, __LINE__)
+#define cutilDrvCtxSync() __cuCtxSync (__FILE__, __LINE__)
+#define cutilDrvCheckMsg(msg) __cuCheckMsg (msg, __FILE__, __LINE__)
+#define cutilDrvAlignOffset(offset, alignment) ( offset = (offset + (alignment-1)) & ~((alignment-1)) )
+
+// These are the inline versions for all of the CUTIL functions
+inline void __cuSafeCallNoSync( CUresult err, const char *file, const int line )
+{
+ if( CUDA_SUCCESS != err) {
+ fprintf(stderr, "cuSafeCallNoSync() Driver API error = %04d from file <%s>, line %i.\n",
+ err, file, line );
+ exit(-1);
+ }
+}
+inline void __cuSafeCall( CUresult err, const char *file, const int line )
+{
+ __cuSafeCallNoSync( err, file, line );
+}
+
+inline void __cuCtxSync(const char *file, const int line )
+{
+ CUresult err = cuCtxSynchronize();
+ if( CUDA_SUCCESS != err ) {
+ fprintf(stderr, "cuCtxSynchronize() API error = %04d in file <%s>, line %i.\n",
+ err, file, line );
+ exit(-1);
+ }
+}
+
+#define MIN(a,b) ((a < b) ? a : b)
+#define MAX(a,b) ((a > b) ? a : b)
+
+// Beginning of GPU Architecture definitions
+inline int _ConvertSMVer2CoresDrvApi(int major, int minor)
+{
+ // Defines for GPU Architecture types (using the SM version to determine the # of cores per SM
+ typedef struct {
+ int SM; // 0xMm (hexidecimal notation), M = SM Major version, and m = SM minor version
+ int Cores;
+ } sSMtoCores;
+
+ sSMtoCores nGpuArchCoresPerSM[] =
+ { { 0x10, 8 },
+ { 0x11, 8 },
+ { 0x12, 8 },
+ { 0x13, 8 },
+ { 0x20, 32 },
+ { 0x21, 48 },
+ { -1, -1 }
+ };
+
+ int index = 0;
+ while (nGpuArchCoresPerSM[index].SM != -1) {
+ if (nGpuArchCoresPerSM[index].SM == ((major << 4) + minor) ) {
+ return nGpuArchCoresPerSM[index].Cores;
+ }
+ index++;
+ }
+ printf("MapSMtoCores undefined SMversion %d.%d!\n", major, minor);
+ return -1;
+}
+// end of GPU Architecture definitions
+
+// This function returns the best GPU based on performance
+inline int cutilDrvGetMaxGflopsDeviceId()
+{
+ CUdevice current_device = 0, max_perf_device = 0;
+ int device_count = 0, sm_per_multiproc = 0;
+ int max_compute_perf = 0, best_SM_arch = 0;
+ int major = 0, minor = 0, multiProcessorCount, clockRate;
+
+ cuInit(0);
+ cutilDrvSafeCallNoSync(cuDeviceGetCount(&device_count));
+
+ // Find the best major SM Architecture GPU device
+ while ( current_device < device_count ) {
+ cutilDrvSafeCallNoSync( cuDeviceComputeCapability(&major, &minor, current_device ) );
+ if (major > 0 && major < 9999) {
+ best_SM_arch = MAX(best_SM_arch, major);
+ }
+ current_device++;
+ }
+
+ // Find the best CUDA capable GPU device
+ current_device = 0;
+ while( current_device < device_count ) {
+ cutilDrvSafeCallNoSync( cuDeviceGetAttribute( &multiProcessorCount,
+ CU_DEVICE_ATTRIBUTE_MULTIPROCESSOR_COUNT,
+ current_device ) );
+ cutilDrvSafeCallNoSync( cuDeviceGetAttribute( &clockRate,
+ CU_DEVICE_ATTRIBUTE_CLOCK_RATE,
+ current_device ) );
+ cutilDrvSafeCallNoSync( cuDeviceComputeCapability(&major, &minor, current_device ) );
+
+ if (major == 9999 && minor == 9999) {
+ sm_per_multiproc = 1;
+ } else {
+ sm_per_multiproc = _ConvertSMVer2CoresDrvApi(major, minor);
+ }
+
+ int compute_perf = multiProcessorCount * sm_per_multiproc * clockRate;
+ if( compute_perf > max_compute_perf ) {
+ // If we find GPU with SM major > 2, search only these
+ if ( best_SM_arch > 2 ) {
+ // If our device==dest_SM_arch, choose this, or else pass
+ if (major == best_SM_arch) {
+ max_compute_perf = compute_perf;
+ max_perf_device = current_device;
+ }
+ } else {
+ max_compute_perf = compute_perf;
+ max_perf_device = current_device;
+ }
+ }
+ ++current_device;
+ }
+ return max_perf_device;
+}
+
+// This function returns the best Graphics GPU based on performance
+inline int cutilDrvGetMaxGflopsGraphicsDeviceId()
+{
+ CUdevice current_device = 0, max_perf_device = 0;
+ int device_count = 0, sm_per_multiproc = 0;
+ int max_compute_perf = 0, best_SM_arch = 0;
+ int major = 0, minor = 0, multiProcessorCount, clockRate;
+ int bTCC = 0;
+ char deviceName[256];
+
+ cuInit(0);
+ cutilDrvSafeCallNoSync(cuDeviceGetCount(&device_count));
+
+ // Find the best major SM Architecture GPU device that are graphics devices
+ while ( current_device < device_count ) {
+ cutilDrvSafeCallNoSync( cuDeviceGetName(deviceName, 256, current_device) );
+ cutilDrvSafeCallNoSync( cuDeviceComputeCapability(&major, &minor, current_device ) );
+
+#if CUDA_VERSION >= 3020
+ cutilDrvSafeCallNoSync( cuDeviceGetAttribute( &bTCC, CU_DEVICE_ATTRIBUTE_TCC_DRIVER, current_device ) );
+#else
+ // Assume a Tesla GPU is running in TCC if we are running CUDA 3.1
+ if (deviceName[0] == 'T') bTCC = 1;
+#endif
+ if (!bTCC) {
+ if (major > 0 && major < 9999) {
+ best_SM_arch = MAX(best_SM_arch, major);
+ }
+ }
+ current_device++;
+ }
+
+ // Find the best CUDA capable GPU device
+ current_device = 0;
+ while( current_device < device_count ) {
+ cutilDrvSafeCallNoSync( cuDeviceGetAttribute( &multiProcessorCount,
+ CU_DEVICE_ATTRIBUTE_MULTIPROCESSOR_COUNT,
+ current_device ) );
+ cutilDrvSafeCallNoSync( cuDeviceGetAttribute( &clockRate,
+ CU_DEVICE_ATTRIBUTE_CLOCK_RATE,
+ current_device ) );
+ cutilDrvSafeCallNoSync( cuDeviceComputeCapability(&major, &minor, current_device ) );
+
+#if CUDA_VERSION >= 3020
+ cutilDrvSafeCallNoSync( cuDeviceGetAttribute( &bTCC, CU_DEVICE_ATTRIBUTE_TCC_DRIVER, current_device ) );
+#else
+ // Assume a Tesla GPU is running in TCC if we are running CUDA 3.1
+ if (deviceName[0] == 'T') bTCC = 1;
+#endif
+
+ if (major == 9999 && minor == 9999) {
+ sm_per_multiproc = 1;
+ } else {
+ sm_per_multiproc = _ConvertSMVer2CoresDrvApi(major, minor);
+ }
+
+ // If this is a Tesla based GPU and SM 2.0, and TCC is disabled, this is a contendor
+ if (!bTCC) // Is this GPU running the TCC driver? If so we pass on this
+ {
+ int compute_perf = multiProcessorCount * sm_per_multiproc * clockRate;
+ if( compute_perf > max_compute_perf ) {
+ // If we find GPU with SM major > 2, search only these
+ if ( best_SM_arch > 2 ) {
+ // If our device = dest_SM_arch, then we pick this one
+ if (major == best_SM_arch) {
+ max_compute_perf = compute_perf;
+ max_perf_device = current_device;
+ }
+ } else {
+ max_compute_perf = compute_perf;
+ max_perf_device = current_device;
+ }
+ }
+ }
+ ++current_device;
+ }
+ return max_perf_device;
+}
+
+inline void __cuCheckMsg( const char * msg, const char *file, const int line )
+{
+ CUresult err = cuCtxSynchronize();
+ if( CUDA_SUCCESS != err) {
+ fprintf(stderr, "cutilDrvCheckMsg -> %s", msg);
+ fprintf(stderr, "cutilDrvCheckMsg -> cuCtxSynchronize API error = %04d in file <%s>, line %i.\n",
+ err, file, line );
+ exit(-1);
+ }
+}
+
+
+#if __DEVICE_EMULATION__
+ inline int cutilDeviceInitDrv(int ARGC, char **ARGV) { }
+#else
+ inline int cutilDeviceInitDrv(int ARGC, char ** ARGV)
+ {
+ int cuDevice = 0;
+ int deviceCount = 0;
+ CUresult err = cuInit(0);
+ if (CUDA_SUCCESS == err)
+ cutilDrvSafeCallNoSync(cuDeviceGetCount(&deviceCount));
+ if (deviceCount == 0) {
+ fprintf(stderr, "CUTIL DeviceInitDrv error: no devices supporting CUDA\n");
+ exit(-1);
+ }
+ int dev = 0;
+ cutGetCmdLineArgumenti(ARGC, (const char **) ARGV, "device", &dev);
+ if (dev < 0) dev = 0;
+ if (dev > deviceCount-1) {
+ fprintf(stderr, "\n");
+ fprintf(stderr, ">> %d CUDA capable GPU device(s) detected. <<\n", deviceCount);
+ fprintf(stderr, ">> cutilDeviceInit (-device=%d) is not a valid GPU device. <<\n", dev);
+ fprintf(stderr, "\n");
+ return -dev;
+ }
+ cutilDrvSafeCallNoSync(cuDeviceGet(&cuDevice, dev));
+ char name[100];
+ cuDeviceGetName(name, 100, cuDevice);
+ if (cutCheckCmdLineFlag(ARGC, (const char **) ARGV, "quiet") == CUTFalse) {
+ printf("> Using CUDA Device [%d]: %s\n", dev, name);
+ }
+ return dev;
+ }
+#endif
+
+ // General initialization call to pick the best CUDA Device
+#if __DEVICE_EMULATION__
+ inline CUdevice cutilChooseCudaDeviceDrv(int argc, char **argv, int *p_devID)
+#else
+ inline CUdevice cutilChooseCudaDeviceDrv(int argc, char **argv, int *p_devID)
+ {
+ CUdevice cuDevice;
+ int devID = 0;
+ // If the command-line has a device number specified, use it
+ if( cutCheckCmdLineFlag(argc, (const char**)argv, "device") ) {
+ devID = cutilDeviceInitDrv(argc, argv);
+ if (devID < 0) {
+ printf("exiting...\n");
+ exit(0);
+ }
+ } else {
+ // Otherwise pick the device with highest Gflops/s
+ char name[100];
+ devID = cutilDrvGetMaxGflopsDeviceId();
+ cutilDrvSafeCallNoSync(cuDeviceGet(&cuDevice, devID));
+ cuDeviceGetName(name, 100, cuDevice);
+ printf("> Using CUDA Device [%d]: %s\n", devID, name);
+ }
+ cuDeviceGet(&cuDevice, devID);
+ if (p_devID) *p_devID = devID;
+ return cuDevice;
+ }
+#endif
+
+
+//! Check for CUDA context lost
+inline void cutilDrvCudaCheckCtxLost(const char *errorMessage, const char *file, const int line )
+{
+ CUresult err = cuCtxSynchronize();
+ if( CUDA_ERROR_INVALID_CONTEXT != err) {
+ fprintf(stderr, "Cuda error: %s in file '%s' in line %i\n",
+ errorMessage, file, line );
+ exit(-1);
+ }
+ err = cuCtxSynchronize();
+ if( CUDA_SUCCESS != err) {
+ fprintf(stderr, "Cuda error: %s in file '%s' in line %i\n",
+ errorMessage, file, line );
+ exit(-1);
+ }
+}
+
+#ifndef STRCASECMP
+#ifdef _WIN32
+#define STRCASECMP _stricmp
+#else
+#define STRCASECMP strcasecmp
+#endif
+#endif
+
+#ifndef STRNCASECMP
+#ifdef _WIN32
+#define STRNCASECMP _strnicmp
+#else
+#define STRNCASECMP strncasecmp
+#endif
+#endif
+
+inline void __cutilDrvQAFinish(int argc, char **argv, bool bStatus)
+{
+ const char *sStatus[] = { "FAILED", "PASSED", "WAIVED", NULL };
+
+ bool bFlag = false;
+ for (int i=1; i < argc; i++) {
+ if (!STRCASECMP(argv[i], "-qatest") || !STRCASECMP(argv[i], "-noprompt")) {
+ bFlag |= true;
+ }
+ }
+
+ if (bFlag) {
+ printf("&&&& %s %s", sStatus[bStatus], argv[0]);
+ for (int i=1; i < argc; i++) printf(" %s", argv[i]);
+ } else {
+ printf("[%s] test result\n%s\n", argv[0], sStatus[bStatus]);
+ }
+}
+
+// General check for CUDA GPU SM Capabilities for a specific device #
+inline bool cutilDrvCudaDevCapabilities(int major_version, int minor_version, int deviceNum, int argc, char** argv)
+{
+ int major, minor, dev;
+ char device_name[256];
+
+#ifdef __DEVICE_EMULATION__
+ printf("> Compute Device Emulation Mode \n");
+#endif
+
+ cutilDrvSafeCallNoSync( cuDeviceGet(&dev, deviceNum) );
+ cutilDrvSafeCallNoSync( cuDeviceComputeCapability(&major, &minor, dev));
+ cutilDrvSafeCallNoSync( cuDeviceGetName(device_name, 256, dev) );
+
+ if((major > major_version) ||
+ (major == major_version && minor >= minor_version))
+ {
+ printf("> Device %d: < %s >, Compute SM %d.%d detected\n", dev, device_name, major, minor);
+ return true;
+ }
+ else
+ {
+ printf("There is no device supporting CUDA compute capability %d.%d.\n", major_version, minor_version);
+ __cutilDrvQAFinish(argc, argv, true);
+ return false;
+ }
+}
+
+// General check for CUDA GPU SM Capabilities
+inline bool cutilDrvCudaCapabilities(int major_version, int minor_version, int argc, char **argv)
+{
+ return cutilDrvCudaDevCapabilities(major_version, minor_version, 0, argc, argv);
+}
+
+
+#endif // _CUTIL_INLINE_FUNCTIONS_DRVAPI_H_
--- /dev/null
+/*
+ * Copyright 1993-2010 NVIDIA Corporation. All rights reserved.
+ *
+ * Please refer to the NVIDIA end user license agreement (EULA) associated
+ * with this source code for terms and conditions that govern your use of
+ * this software. Any use, reproduction, disclosure, or distribution of
+ * this software and related documentation outside the terms of the EULA
+ * is strictly prohibited.
+ *
+ */
+
+#ifndef _CUTIL_INLINE_FUNCTIONS_RUNTIME_H_
+#define _CUTIL_INLINE_FUNCTIONS_RUNTIME_H_
+
+#ifdef _WIN32
+#ifdef _DEBUG // Do this only in debug mode...
+# define WINDOWS_LEAN_AND_MEAN
+# include <windows.h>
+# include <stdlib.h>
+# undef min
+# undef max
+#endif
+#endif
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include <cufft.h>
+
+// We define these calls here, so the user doesn't need to include __FILE__ and __LINE__
+// The advantage is the developers gets to use the inline function so they can debug
+#define cutilSafeCallNoSync(err) __cudaSafeCallNoSync(err, __FILE__, __LINE__)
+#define cutilSafeCall(err) __cudaSafeCall (err, __FILE__, __LINE__)
+#define cutilSafeThreadSync() __cudaSafeThreadSync(__FILE__, __LINE__)
+#define cufftSafeCall(err) __cufftSafeCall (err, __FILE__, __LINE__)
+#define cutilCheckError(err) __cutilCheckError (err, __FILE__, __LINE__)
+#define cutilCheckMsg(msg) __cutilGetLastError (msg, __FILE__, __LINE__)
+#define cutilCheckMsgAndSync(msg) __cutilGetLastErrorAndSync (msg, __FILE__, __LINE__)
+#define cutilSafeMalloc(mallocCall) __cutilSafeMalloc ((mallocCall), __FILE__, __LINE__)
+#define cutilCondition(val) __cutilCondition (val, __FILE__, __LINE__)
+#define cutilExit(argc, argv) __cutilExit (argc, argv)
+
+inline cudaError cutilDeviceSynchronize()
+{
+#if CUDART_VERSION >= 4000
+ return cudaDeviceSynchronize();
+#else
+ return cudaThreadSynchronize();
+#endif
+}
+
+inline cudaError cutilDeviceReset()
+{
+#if CUDART_VERSION >= 4000
+ return cudaDeviceReset();
+#else
+ return cudaThreadExit();
+#endif
+}
+
+inline void __cutilCondition(int val, char *file, int line)
+{
+ if( CUTFalse == cutCheckCondition( val, file, line ) ) {
+ exit(EXIT_FAILURE);
+ }
+}
+
+inline void __cutilExit(int argc, char **argv)
+{
+ if (!cutCheckCmdLineFlag(argc, (const char**)argv, "noprompt")) {
+ printf("\nPress ENTER to exit...\n");
+ fflush( stdout);
+ fflush( stderr);
+ getchar();
+ }
+ exit(EXIT_SUCCESS);
+}
+
+#define MIN(a,b) ((a < b) ? a : b)
+#define MAX(a,b) ((a > b) ? a : b)
+
+// Beginning of GPU Architecture definitions
+inline int _ConvertSMVer2Cores(int major, int minor)
+{
+ // Defines for GPU Architecture types (using the SM version to determine the # of cores per SM
+ typedef struct {
+ int SM; // 0xMm (hexidecimal notation), M = SM Major version, and m = SM minor version
+ int Cores;
+ } sSMtoCores;
+
+ sSMtoCores nGpuArchCoresPerSM[] =
+ { { 0x10, 8 },
+ { 0x11, 8 },
+ { 0x12, 8 },
+ { 0x13, 8 },
+ { 0x20, 32 },
+ { 0x21, 48 },
+ { -1, -1 }
+ };
+
+ int index = 0;
+ while (nGpuArchCoresPerSM[index].SM != -1) {
+ if (nGpuArchCoresPerSM[index].SM == ((major << 4) + minor) ) {
+ return nGpuArchCoresPerSM[index].Cores;
+ }
+ index++;
+ }
+ printf("MapSMtoCores undefined SMversion %d.%d!\n", major, minor);
+ return -1;
+}
+// end of GPU Architecture definitions
+
+// This function returns the best GPU (with maximum GFLOPS)
+inline int cutGetMaxGflopsDeviceId()
+{
+ int current_device = 0, sm_per_multiproc = 0;
+ int max_compute_perf = 0, max_perf_device = 0;
+ int device_count = 0, best_SM_arch = 0;
+ cudaDeviceProp deviceProp;
+
+ cudaGetDeviceCount( &device_count );
+ // Find the best major SM Architecture GPU device
+ while ( current_device < device_count ) {
+ cudaGetDeviceProperties( &deviceProp, current_device );
+ if (deviceProp.major > 0 && deviceProp.major < 9999) {
+ best_SM_arch = MAX(best_SM_arch, deviceProp.major);
+ }
+ current_device++;
+ }
+
+ // Find the best CUDA capable GPU device
+ current_device = 0;
+ while( current_device < device_count ) {
+ cudaGetDeviceProperties( &deviceProp, current_device );
+ if (deviceProp.major == 9999 && deviceProp.minor == 9999) {
+ sm_per_multiproc = 1;
+ } else {
+ sm_per_multiproc = _ConvertSMVer2Cores(deviceProp.major, deviceProp.minor);
+ }
+
+ int compute_perf = deviceProp.multiProcessorCount * sm_per_multiproc * deviceProp.clockRate;
+ if( compute_perf > max_compute_perf ) {
+ // If we find GPU with SM major > 2, search only these
+ if ( best_SM_arch > 2 ) {
+ // If our device==dest_SM_arch, choose this, or else pass
+ if (deviceProp.major == best_SM_arch) {
+ max_compute_perf = compute_perf;
+ max_perf_device = current_device;
+ }
+ } else {
+ max_compute_perf = compute_perf;
+ max_perf_device = current_device;
+ }
+ }
+ ++current_device;
+ }
+ return max_perf_device;
+}
+
+// This function returns the best GPU (with maximum GFLOPS)
+inline int cutGetMaxGflopsGraphicsDeviceId()
+{
+ int current_device = 0, sm_per_multiproc = 0;
+ int max_compute_perf = 0, max_perf_device = 0;
+ int device_count = 0, best_SM_arch = 0;
+ int bTCC = 0;
+ cudaDeviceProp deviceProp;
+
+ cudaGetDeviceCount( &device_count );
+ // Find the best major SM Architecture GPU device that is graphics capable
+ while ( current_device < device_count ) {
+ cudaGetDeviceProperties( &deviceProp, current_device );
+
+#if CUDA_VERSION >= 3020
+ if (deviceProp.tccDriver) bTCC = 1;
+#else
+ // Assume a Tesla GPU is running in TCC if we are running CUDA 3.1
+ if (deviceProp.name[0] == 'T') bTCC = 1;
+#endif
+
+ if (!bTCC) {
+ if (deviceProp.major > 0 && deviceProp.major < 9999) {
+ best_SM_arch = MAX(best_SM_arch, deviceProp.major);
+ }
+ }
+ current_device++;
+ }
+
+ // Find the best CUDA capable GPU device
+ current_device = 0;
+ while( current_device < device_count ) {
+ cudaGetDeviceProperties( &deviceProp, current_device );
+ if (deviceProp.major == 9999 && deviceProp.minor == 9999) {
+ sm_per_multiproc = 1;
+ } else {
+ sm_per_multiproc = _ConvertSMVer2Cores(deviceProp.major, deviceProp.minor);
+ }
+
+#if CUDA_VERSION >= 3020
+ if (deviceProp.tccDriver) bTCC = 1;
+#else
+ // Assume a Tesla GPU is running in TCC if we are running CUDA 3.1
+ if (deviceProp.name[0] == 'T') bTCC = 1;
+#endif
+
+ if (!bTCC) // Is this GPU running the TCC driver? If so we pass on this
+ {
+ int compute_perf = deviceProp.multiProcessorCount * sm_per_multiproc * deviceProp.clockRate;
+ if( compute_perf > max_compute_perf ) {
+ // If we find GPU with SM major > 2, search only these
+ if ( best_SM_arch > 2 ) {
+ // If our device==dest_SM_arch, choose this, or else pass
+ if (deviceProp.major == best_SM_arch) {
+ max_compute_perf = compute_perf;
+ max_perf_device = current_device;
+ }
+ } else {
+ max_compute_perf = compute_perf;
+ max_perf_device = current_device;
+ }
+ }
+ }
+ ++current_device;
+ }
+ return max_perf_device;
+}
+
+// Give a little more for Windows : the console window often disapears before we can read the message
+#ifdef _WIN32
+# if 1//ndef UNICODE
+# ifdef _DEBUG // Do this only in debug mode...
+ inline void VSPrintf(FILE *file, LPCSTR fmt, ...)
+ {
+ size_t fmt2_sz = 2048;
+ char *fmt2 = (char*)malloc(fmt2_sz);
+ va_list vlist;
+ va_start(vlist, fmt);
+ while((_vsnprintf(fmt2, fmt2_sz, fmt, vlist)) < 0) // means there wasn't anough room
+ {
+ fmt2_sz *= 2;
+ if(fmt2) free(fmt2);
+ fmt2 = (char*)malloc(fmt2_sz);
+ }
+ OutputDebugStringA(fmt2);
+ fprintf(file, fmt2);
+ free(fmt2);
+ }
+# define FPRINTF(a) VSPrintf a
+# else //debug
+# define FPRINTF(a) fprintf a
+// For other than Win32
+# endif //debug
+# else //unicode
+// Unicode case... let's give-up for now and keep basic printf
+# define FPRINTF(a) fprintf a
+# endif //unicode
+#else //win32
+# define FPRINTF(a) fprintf a
+#endif //win32
+
+// NOTE: "%s(%i) : " allows Visual Studio to directly jump to the file at the right line
+// when the user double clicks on the error line in the Output pane. Like any compile error.
+
+inline void __cudaSafeCallNoSync( cudaError err, const char *file, const int line )
+{
+ if( cudaSuccess != err) {
+ FPRINTF((stderr, "%s(%i) : cudaSafeCallNoSync() Runtime API error : %s.\n",
+ file, line, cudaGetErrorString( err) ));
+ exit(-1);
+ }
+}
+
+inline void __cudaSafeCall( cudaError err, const char *file, const int line )
+{
+ if( cudaSuccess != err) {
+ FPRINTF((stderr, "%s(%i) : cudaSafeCall() Runtime API error : %s.\n",
+ file, line, cudaGetErrorString( err) ));
+ exit(-1);
+ }
+}
+
+inline void __cudaSafeThreadSync( const char *file, const int line )
+{
+ cudaError err = cutilDeviceSynchronize();
+ if ( cudaSuccess != err) {
+ FPRINTF((stderr, "%s(%i) : cudaDeviceSynchronize() Runtime API error : %s.\n",
+ file, line, cudaGetErrorString( err) ));
+ exit(-1);
+ }
+}
+
+inline void __cufftSafeCall( cufftResult err, const char *file, const int line )
+{
+ if( CUFFT_SUCCESS != err) {
+ FPRINTF((stderr, "%s(%i) : cufftSafeCall() CUFFT error.\n",
+ file, line));
+ exit(-1);
+ }
+}
+
+inline void __cutilCheckError( CUTBoolean err, const char *file, const int line )
+{
+ if( CUTTrue != err) {
+ FPRINTF((stderr, "%s(%i) : CUTIL CUDA error.\n",
+ file, line));
+ exit(-1);
+ }
+}
+
+inline void __cutilGetLastError( const char *errorMessage, const char *file, const int line )
+{
+ cudaError_t err = cudaGetLastError();
+ if( cudaSuccess != err) {
+ FPRINTF((stderr, "%s(%i) : cutilCheckMsg() CUTIL CUDA error : %s : %s.\n",
+ file, line, errorMessage, cudaGetErrorString( err) ));
+ exit(-1);
+ }
+}
+
+inline void __cutilGetLastErrorAndSync( const char *errorMessage, const char *file, const int line )
+{
+ cudaError_t err = cudaGetLastError();
+ if( cudaSuccess != err) {
+ FPRINTF((stderr, "%s(%i) : cutilCheckMsg() CUTIL CUDA error : %s : %s.\n",
+ file, line, errorMessage, cudaGetErrorString( err) ));
+ exit(-1);
+ }
+
+ err = cutilDeviceSynchronize();
+ if( cudaSuccess != err) {
+ FPRINTF((stderr, "%s(%i) : cutilCheckMsg cudaDeviceSynchronize error: %s : %s.\n",
+ file, line, errorMessage, cudaGetErrorString( err) ));
+ exit(-1);
+ }
+}
+
+inline void __cutilSafeMalloc( void *pointer, const char *file, const int line )
+{
+ if( !(pointer)) {
+ FPRINTF((stderr, "%s(%i) : cutilSafeMalloc host malloc failure\n",
+ file, line));
+ exit(-1);
+ }
+}
+
+#if __DEVICE_EMULATION__
+ inline int cutilDeviceInit(int ARGC, char **ARGV) { }
+ inline int cutilChooseCudaDevice(int ARGC, char **ARGV) { }
+#else
+ inline int cutilDeviceInit(int ARGC, char **ARGV)
+ {
+ int deviceCount;
+ cutilSafeCallNoSync(cudaGetDeviceCount(&deviceCount));
+ if (deviceCount == 0) {
+ FPRINTF((stderr, "CUTIL CUDA error: no devices supporting CUDA.\n"));
+ exit(-1);
+ }
+ int dev = 0;
+ cutGetCmdLineArgumenti(ARGC, (const char **) ARGV, "device", &dev);
+ if (dev < 0)
+ dev = 0;
+ if (dev > deviceCount-1) {
+ fprintf(stderr, "\n");
+ fprintf(stderr, ">> %d CUDA capable GPU device(s) detected. <<\n", deviceCount);
+ fprintf(stderr, ">> cutilDeviceInit (-device=%d) is not a valid GPU device. <<\n", dev);
+ fprintf(stderr, "\n");
+ return -dev;
+ }
+ cudaDeviceProp deviceProp;
+ cutilSafeCallNoSync(cudaGetDeviceProperties(&deviceProp, dev));
+ if (deviceProp.major < 1) {
+ FPRINTF((stderr, "cutil error: GPU device does not support CUDA.\n"));
+ exit(-1); \
+ }
+ printf("> Using CUDA device [%d]: %s\n", dev, deviceProp.name);
+ cutilSafeCall(cudaSetDevice(dev));
+
+ return dev;
+ }
+
+ // General initialization call to pick the best CUDA Device
+ inline int cutilChooseCudaDevice(int argc, char **argv)
+ {
+ cudaDeviceProp deviceProp;
+ int devID = 0;
+ // If the command-line has a device number specified, use it
+ if( cutCheckCmdLineFlag(argc, (const char**)argv, "device") ) {
+ devID = cutilDeviceInit(argc, argv);
+ if (devID < 0) {
+ printf("exiting...\n");
+ cutilExit(argc, argv);
+ exit(0);
+ }
+ } else {
+ // Otherwise pick the device with highest Gflops/s
+ devID = cutGetMaxGflopsDeviceId();
+ cutilSafeCallNoSync( cudaSetDevice( devID ) );
+ cutilSafeCallNoSync( cudaGetDeviceProperties(&deviceProp, devID) );
+ printf("> Using CUDA device [%d]: %s\n", devID, deviceProp.name);
+ }
+ return devID;
+ }
+#endif
+
+
+//! Check for CUDA context lost
+inline void cutilCudaCheckCtxLost(const char *errorMessage, const char *file, const int line )
+{
+ cudaError_t err = cudaGetLastError();
+ if( cudaSuccess != err) {
+ FPRINTF((stderr, "%s(%i) : CUDA error: %s : %s.\n",
+ file, line, errorMessage, cudaGetErrorString( err) ));
+ exit(-1);
+ }
+ err = cutilDeviceSynchronize();
+ if( cudaSuccess != err) {
+ FPRINTF((stderr, "%s(%i) : CUDA error: %s : %s.\n",
+ file, line, errorMessage, cudaGetErrorString( err) ));
+ exit(-1);
+ }
+}
+
+#ifndef STRCASECMP
+#ifdef _WIN32
+#define STRCASECMP _stricmp
+#else
+#define STRCASECMP strcasecmp
+#endif
+#endif
+
+#ifndef STRNCASECMP
+#ifdef _WIN32
+#define STRNCASECMP _strnicmp
+#else
+#define STRNCASECMP strncasecmp
+#endif
+#endif
+
+inline void __cutilQAFinish(int argc, char **argv, bool bStatus)
+{
+ const char *sStatus[] = { "FAILED", "PASSED", "WAIVED", NULL };
+
+ bool bFlag = false;
+ for (int i=1; i < argc; i++) {
+ if (!STRCASECMP(argv[i], "-qatest") || !STRCASECMP(argv[i], "-noprompt")) {
+ bFlag |= true;
+ }
+ }
+
+ if (bFlag) {
+ printf("&&&& %s %s", sStatus[bStatus], argv[0]);
+ for (int i=1; i < argc; i++) printf(" %s", argv[i]);
+ } else {
+ printf("[%s] test result\n%s\n", argv[0], sStatus[bStatus]);
+ }
+}
+
+// General check for CUDA GPU SM Capabilities
+inline bool cutilCudaCapabilities(int major_version, int minor_version, int argc, char **argv)
+{
+ cudaDeviceProp deviceProp;
+ deviceProp.major = 0;
+ deviceProp.minor = 0;
+ int dev;
+
+#ifdef __DEVICE_EMULATION__
+ printf("> Compute Device Emulation Mode \n");
+#endif
+
+ cutilSafeCall( cudaGetDevice(&dev) );
+ cutilSafeCall( cudaGetDeviceProperties(&deviceProp, dev));
+
+ if((deviceProp.major > major_version) ||
+ (deviceProp.major == major_version && deviceProp.minor >= minor_version))
+ {
+ printf("> Device %d: <%16s >, Compute SM %d.%d detected\n", dev, deviceProp.name, deviceProp.major, deviceProp.minor);
+ return true;
+ }
+ else
+ {
+ printf("There is no device supporting CUDA compute capability %d.%d.\n", major_version, minor_version);
+ __cutilQAFinish(argc, argv, true);
+ return false;
+ }
+}
+
+#endif // _CUTIL_INLINE_FUNCTIONS_RUNTIME_H_
--- /dev/null
+/*
+ * Copyright 1993-2010 NVIDIA Corporation. All rights reserved.
+ *
+ * Please refer to the NVIDIA end user license agreement (EULA) associated
+ * with this source code for terms and conditions that govern your use of
+ * this software. Any use, reproduction, disclosure, or distribution of
+ * this software and related documentation outside the terms of the EULA
+ * is strictly prohibited.
+ *
+ */
+
+/*
+ This file implements common mathematical operations on vector types
+ (float3, float4 etc.) since these are not provided as standard by CUDA.
+
+ The syntax is modelled on the Cg standard library.
+
+ This is part of the CUTIL library and is not supported by NVIDIA.
+
+ Thanks to Linh Hah for additions and fixes.
+*/
+
+#ifndef CUTIL_MATH_H
+#define CUTIL_MATH_H
+
+#include "cuda_runtime.h"
+
+typedef unsigned int uint;
+typedef unsigned short ushort;
+
+#ifndef __CUDACC__
+#include <math.h>
+
+////////////////////////////////////////////////////////////////////////////////
+// host implementations of CUDA functions
+////////////////////////////////////////////////////////////////////////////////
+
+inline float fminf(float a, float b)
+{
+ return a < b ? a : b;
+}
+
+inline float fmaxf(float a, float b)
+{
+ return a > b ? a : b;
+}
+
+inline int max(int a, int b)
+{
+ return a > b ? a : b;
+}
+
+inline int min(int a, int b)
+{
+ return a < b ? a : b;
+}
+
+inline float rsqrtf(float x)
+{
+ return 1.0f / sqrtf(x);
+}
+#endif
+
+////////////////////////////////////////////////////////////////////////////////
+// constructors
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 make_float2(float s)
+{
+ return make_float2(s, s);
+}
+inline __host__ __device__ float2 make_float2(float3 a)
+{
+ return make_float2(a.x, a.y);
+}
+inline __host__ __device__ float2 make_float2(int2 a)
+{
+ return make_float2(float(a.x), float(a.y));
+}
+inline __host__ __device__ float2 make_float2(uint2 a)
+{
+ return make_float2(float(a.x), float(a.y));
+}
+
+inline __host__ __device__ int2 make_int2(int s)
+{
+ return make_int2(s, s);
+}
+inline __host__ __device__ int2 make_int2(int3 a)
+{
+ return make_int2(a.x, a.y);
+}
+inline __host__ __device__ int2 make_int2(uint2 a)
+{
+ return make_int2(int(a.x), int(a.y));
+}
+inline __host__ __device__ int2 make_int2(float2 a)
+{
+ return make_int2(int(a.x), int(a.y));
+}
+
+inline __host__ __device__ uint2 make_uint2(uint s)
+{
+ return make_uint2(s, s);
+}
+inline __host__ __device__ uint2 make_uint2(uint3 a)
+{
+ return make_uint2(a.x, a.y);
+}
+inline __host__ __device__ uint2 make_uint2(int2 a)
+{
+ return make_uint2(uint(a.x), uint(a.y));
+}
+
+inline __host__ __device__ float3 make_float3(float s)
+{
+ return make_float3(s, s, s);
+}
+inline __host__ __device__ float3 make_float3(float2 a)
+{
+ return make_float3(a.x, a.y, 0.0f);
+}
+inline __host__ __device__ float3 make_float3(float2 a, float s)
+{
+ return make_float3(a.x, a.y, s);
+}
+inline __host__ __device__ float3 make_float3(float4 a)
+{
+ return make_float3(a.x, a.y, a.z);
+}
+inline __host__ __device__ float3 make_float3(int3 a)
+{
+ return make_float3(float(a.x), float(a.y), float(a.z));
+}
+inline __host__ __device__ float3 make_float3(uint3 a)
+{
+ return make_float3(float(a.x), float(a.y), float(a.z));
+}
+
+inline __host__ __device__ int3 make_int3(int s)
+{
+ return make_int3(s, s, s);
+}
+inline __host__ __device__ int3 make_int3(int2 a)
+{
+ return make_int3(a.x, a.y, 0);
+}
+inline __host__ __device__ int3 make_int3(int2 a, int s)
+{
+ return make_int3(a.x, a.y, s);
+}
+inline __host__ __device__ int3 make_int3(uint3 a)
+{
+ return make_int3(int(a.x), int(a.y), int(a.z));
+}
+inline __host__ __device__ int3 make_int3(float3 a)
+{
+ return make_int3(int(a.x), int(a.y), int(a.z));
+}
+
+inline __host__ __device__ uint3 make_uint3(uint s)
+{
+ return make_uint3(s, s, s);
+}
+inline __host__ __device__ uint3 make_uint3(uint2 a)
+{
+ return make_uint3(a.x, a.y, 0);
+}
+inline __host__ __device__ uint3 make_uint3(uint2 a, uint s)
+{
+ return make_uint3(a.x, a.y, s);
+}
+inline __host__ __device__ uint3 make_uint3(uint4 a)
+{
+ return make_uint3(a.x, a.y, a.z);
+}
+inline __host__ __device__ uint3 make_uint3(int3 a)
+{
+ return make_uint3(uint(a.x), uint(a.y), uint(a.z));
+}
+
+inline __host__ __device__ float4 make_float4(float s)
+{
+ return make_float4(s, s, s, s);
+}
+inline __host__ __device__ float4 make_float4(float3 a)
+{
+ return make_float4(a.x, a.y, a.z, 0.0f);
+}
+inline __host__ __device__ float4 make_float4(float3 a, float w)
+{
+ return make_float4(a.x, a.y, a.z, w);
+}
+inline __host__ __device__ float4 make_float4(int4 a)
+{
+ return make_float4(float(a.x), float(a.y), float(a.z), float(a.w));
+}
+inline __host__ __device__ float4 make_float4(uint4 a)
+{
+ return make_float4(float(a.x), float(a.y), float(a.z), float(a.w));
+}
+
+inline __host__ __device__ int4 make_int4(int s)
+{
+ return make_int4(s, s, s, s);
+}
+inline __host__ __device__ int4 make_int4(int3 a)
+{
+ return make_int4(a.x, a.y, a.z, 0);
+}
+inline __host__ __device__ int4 make_int4(int3 a, int w)
+{
+ return make_int4(a.x, a.y, a.z, w);
+}
+inline __host__ __device__ int4 make_int4(uint4 a)
+{
+ return make_int4(int(a.x), int(a.y), int(a.z), int(a.w));
+}
+inline __host__ __device__ int4 make_int4(float4 a)
+{
+ return make_int4(int(a.x), int(a.y), int(a.z), int(a.w));
+}
+
+
+inline __host__ __device__ uint4 make_uint4(uint s)
+{
+ return make_uint4(s, s, s, s);
+}
+inline __host__ __device__ uint4 make_uint4(uint3 a)
+{
+ return make_uint4(a.x, a.y, a.z, 0);
+}
+inline __host__ __device__ uint4 make_uint4(uint3 a, uint w)
+{
+ return make_uint4(a.x, a.y, a.z, w);
+}
+inline __host__ __device__ uint4 make_uint4(int4 a)
+{
+ return make_uint4(uint(a.x), uint(a.y), uint(a.z), uint(a.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// negate
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator-(float2 &a)
+{
+ return make_float2(-a.x, -a.y);
+}
+inline __host__ __device__ int2 operator-(int2 &a)
+{
+ return make_int2(-a.x, -a.y);
+}
+inline __host__ __device__ float3 operator-(float3 &a)
+{
+ return make_float3(-a.x, -a.y, -a.z);
+}
+inline __host__ __device__ int3 operator-(int3 &a)
+{
+ return make_int3(-a.x, -a.y, -a.z);
+}
+inline __host__ __device__ float4 operator-(float4 &a)
+{
+ return make_float4(-a.x, -a.y, -a.z, -a.w);
+}
+inline __host__ __device__ int4 operator-(int4 &a)
+{
+ return make_int4(-a.x, -a.y, -a.z, -a.w);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// addition
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator+(float2 a, float2 b)
+{
+ return make_float2(a.x + b.x, a.y + b.y);
+}
+inline __host__ __device__ void operator+=(float2 &a, float2 b)
+{
+ a.x += b.x; a.y += b.y;
+}
+inline __host__ __device__ float2 operator+(float2 a, float b)
+{
+ return make_float2(a.x + b, a.y + b);
+}
+inline __host__ __device__ float2 operator+(float b, float2 a)
+{
+ return make_float2(a.x + b, a.y + b);
+}
+inline __host__ __device__ void operator+=(float2 &a, float b)
+{
+ a.x += b; a.y += b;
+}
+
+inline __host__ __device__ int2 operator+(int2 a, int2 b)
+{
+ return make_int2(a.x + b.x, a.y + b.y);
+}
+inline __host__ __device__ void operator+=(int2 &a, int2 b)
+{
+ a.x += b.x; a.y += b.y;
+}
+inline __host__ __device__ int2 operator+(int2 a, int b)
+{
+ return make_int2(a.x + b, a.y + b);
+}
+inline __host__ __device__ int2 operator+(int b, int2 a)
+{
+ return make_int2(a.x + b, a.y + b);
+}
+inline __host__ __device__ void operator+=(int2 &a, int b)
+{
+ a.x += b; a.y += b;
+}
+
+inline __host__ __device__ uint2 operator+(uint2 a, uint2 b)
+{
+ return make_uint2(a.x + b.x, a.y + b.y);
+}
+inline __host__ __device__ void operator+=(uint2 &a, uint2 b)
+{
+ a.x += b.x; a.y += b.y;
+}
+inline __host__ __device__ uint2 operator+(uint2 a, uint b)
+{
+ return make_uint2(a.x + b, a.y + b);
+}
+inline __host__ __device__ uint2 operator+(uint b, uint2 a)
+{
+ return make_uint2(a.x + b, a.y + b);
+}
+inline __host__ __device__ void operator+=(uint2 &a, uint b)
+{
+ a.x += b; a.y += b;
+}
+
+
+inline __host__ __device__ float3 operator+(float3 a, float3 b)
+{
+ return make_float3(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+inline __host__ __device__ void operator+=(float3 &a, float3 b)
+{
+ a.x += b.x; a.y += b.y; a.z += b.z;
+}
+inline __host__ __device__ float3 operator+(float3 a, float b)
+{
+ return make_float3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ void operator+=(float3 &a, float b)
+{
+ a.x += b; a.y += b; a.z += b;
+}
+
+inline __host__ __device__ int3 operator+(int3 a, int3 b)
+{
+ return make_int3(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+inline __host__ __device__ void operator+=(int3 &a, int3 b)
+{
+ a.x += b.x; a.y += b.y; a.z += b.z;
+}
+inline __host__ __device__ int3 operator+(int3 a, int b)
+{
+ return make_int3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ void operator+=(int3 &a, int b)
+{
+ a.x += b; a.y += b; a.z += b;
+}
+
+inline __host__ __device__ uint3 operator+(uint3 a, uint3 b)
+{
+ return make_uint3(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+inline __host__ __device__ void operator+=(uint3 &a, uint3 b)
+{
+ a.x += b.x; a.y += b.y; a.z += b.z;
+}
+inline __host__ __device__ uint3 operator+(uint3 a, uint b)
+{
+ return make_uint3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ void operator+=(uint3 &a, uint b)
+{
+ a.x += b; a.y += b; a.z += b;
+}
+
+inline __host__ __device__ int3 operator+(int b, int3 a)
+{
+ return make_int3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ uint3 operator+(uint b, uint3 a)
+{
+ return make_uint3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ float3 operator+(float b, float3 a)
+{
+ return make_float3(a.x + b, a.y + b, a.z + b);
+}
+
+inline __host__ __device__ float4 operator+(float4 a, float4 b)
+{
+ return make_float4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
+}
+inline __host__ __device__ void operator+=(float4 &a, float4 b)
+{
+ a.x += b.x; a.y += b.y; a.z += b.z; a.w += b.w;
+}
+inline __host__ __device__ float4 operator+(float4 a, float b)
+{
+ return make_float4(a.x + b, a.y + b, a.z + b, a.w + b);
+}
+inline __host__ __device__ float4 operator+(float b, float4 a)
+{
+ return make_float4(a.x + b, a.y + b, a.z + b, a.w + b);
+}
+inline __host__ __device__ void operator+=(float4 &a, float b)
+{
+ a.x += b; a.y += b; a.z += b; a.w += b;
+}
+
+inline __host__ __device__ int4 operator+(int4 a, int4 b)
+{
+ return make_int4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
+}
+inline __host__ __device__ void operator+=(int4 &a, int4 b)
+{
+ a.x += b.x; a.y += b.y; a.z += b.z; a.w += b.w;
+}
+inline __host__ __device__ int4 operator+(int4 a, int b)
+{
+ return make_int4(a.x + b, a.y + b, a.z + b, a.w + b);
+}
+inline __host__ __device__ int4 operator+(int b, int4 a)
+{
+ return make_int4(a.x + b, a.y + b, a.z + b, a.w + b);
+}
+inline __host__ __device__ void operator+=(int4 &a, int b)
+{
+ a.x += b; a.y += b; a.z += b; a.w += b;
+}
+
+inline __host__ __device__ uint4 operator+(uint4 a, uint4 b)
+{
+ return make_uint4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
+}
+inline __host__ __device__ void operator+=(uint4 &a, uint4 b)
+{
+ a.x += b.x; a.y += b.y; a.z += b.z; a.w += b.w;
+}
+inline __host__ __device__ uint4 operator+(uint4 a, uint b)
+{
+ return make_uint4(a.x + b, a.y + b, a.z + b, a.w + b);
+}
+inline __host__ __device__ uint4 operator+(uint b, uint4 a)
+{
+ return make_uint4(a.x + b, a.y + b, a.z + b, a.w + b);
+}
+inline __host__ __device__ void operator+=(uint4 &a, uint b)
+{
+ a.x += b; a.y += b; a.z += b; a.w += b;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// subtract
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator-(float2 a, float2 b)
+{
+ return make_float2(a.x - b.x, a.y - b.y);
+}
+inline __host__ __device__ void operator-=(float2 &a, float2 b)
+{
+ a.x -= b.x; a.y -= b.y;
+}
+inline __host__ __device__ float2 operator-(float2 a, float b)
+{
+ return make_float2(a.x - b, a.y - b);
+}
+inline __host__ __device__ float2 operator-(float b, float2 a)
+{
+ return make_float2(b - a.x, b - a.y);
+}
+inline __host__ __device__ void operator-=(float2 &a, float b)
+{
+ a.x -= b; a.y -= b;
+}
+
+inline __host__ __device__ int2 operator-(int2 a, int2 b)
+{
+ return make_int2(a.x - b.x, a.y - b.y);
+}
+inline __host__ __device__ void operator-=(int2 &a, int2 b)
+{
+ a.x -= b.x; a.y -= b.y;
+}
+inline __host__ __device__ int2 operator-(int2 a, int b)
+{
+ return make_int2(a.x - b, a.y - b);
+}
+inline __host__ __device__ int2 operator-(int b, int2 a)
+{
+ return make_int2(b - a.x, b - a.y);
+}
+inline __host__ __device__ void operator-=(int2 &a, int b)
+{
+ a.x -= b; a.y -= b;
+}
+
+inline __host__ __device__ uint2 operator-(uint2 a, uint2 b)
+{
+ return make_uint2(a.x - b.x, a.y - b.y);
+}
+inline __host__ __device__ void operator-=(uint2 &a, uint2 b)
+{
+ a.x -= b.x; a.y -= b.y;
+}
+inline __host__ __device__ uint2 operator-(uint2 a, uint b)
+{
+ return make_uint2(a.x - b, a.y - b);
+}
+inline __host__ __device__ uint2 operator-(uint b, uint2 a)
+{
+ return make_uint2(b - a.x, b - a.y);
+}
+inline __host__ __device__ void operator-=(uint2 &a, uint b)
+{
+ a.x -= b; a.y -= b;
+}
+
+inline __host__ __device__ float3 operator-(float3 a, float3 b)
+{
+ return make_float3(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+inline __host__ __device__ void operator-=(float3 &a, float3 b)
+{
+ a.x -= b.x; a.y -= b.y; a.z -= b.z;
+}
+inline __host__ __device__ float3 operator-(float3 a, float b)
+{
+ return make_float3(a.x - b, a.y - b, a.z - b);
+}
+inline __host__ __device__ float3 operator-(float b, float3 a)
+{
+ return make_float3(b - a.x, b - a.y, b - a.z);
+}
+inline __host__ __device__ void operator-=(float3 &a, float b)
+{
+ a.x -= b; a.y -= b; a.z -= b;
+}
+
+inline __host__ __device__ int3 operator-(int3 a, int3 b)
+{
+ return make_int3(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+inline __host__ __device__ void operator-=(int3 &a, int3 b)
+{
+ a.x -= b.x; a.y -= b.y; a.z -= b.z;
+}
+inline __host__ __device__ int3 operator-(int3 a, int b)
+{
+ return make_int3(a.x - b, a.y - b, a.z - b);
+}
+inline __host__ __device__ int3 operator-(int b, int3 a)
+{
+ return make_int3(b - a.x, b - a.y, b - a.z);
+}
+inline __host__ __device__ void operator-=(int3 &a, int b)
+{
+ a.x -= b; a.y -= b; a.z -= b;
+}
+
+inline __host__ __device__ uint3 operator-(uint3 a, uint3 b)
+{
+ return make_uint3(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+inline __host__ __device__ void operator-=(uint3 &a, uint3 b)
+{
+ a.x -= b.x; a.y -= b.y; a.z -= b.z;
+}
+inline __host__ __device__ uint3 operator-(uint3 a, uint b)
+{
+ return make_uint3(a.x - b, a.y - b, a.z - b);
+}
+inline __host__ __device__ uint3 operator-(uint b, uint3 a)
+{
+ return make_uint3(b - a.x, b - a.y, b - a.z);
+}
+inline __host__ __device__ void operator-=(uint3 &a, uint b)
+{
+ a.x -= b; a.y -= b; a.z -= b;
+}
+
+inline __host__ __device__ float4 operator-(float4 a, float4 b)
+{
+ return make_float4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
+}
+inline __host__ __device__ void operator-=(float4 &a, float4 b)
+{
+ a.x -= b.x; a.y -= b.y; a.z -= b.z; a.w -= b.w;
+}
+inline __host__ __device__ float4 operator-(float4 a, float b)
+{
+ return make_float4(a.x - b, a.y - b, a.z - b, a.w - b);
+}
+inline __host__ __device__ void operator-=(float4 &a, float b)
+{
+ a.x -= b; a.y -= b; a.z -= b; a.w -= b;
+}
+
+inline __host__ __device__ int4 operator-(int4 a, int4 b)
+{
+ return make_int4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
+}
+inline __host__ __device__ void operator-=(int4 &a, int4 b)
+{
+ a.x -= b.x; a.y -= b.y; a.z -= b.z; a.w -= b.w;
+}
+inline __host__ __device__ int4 operator-(int4 a, int b)
+{
+ return make_int4(a.x - b, a.y - b, a.z - b, a.w - b);
+}
+inline __host__ __device__ int4 operator-(int b, int4 a)
+{
+ return make_int4(b - a.x, b - a.y, b - a.z, b - a.w);
+}
+inline __host__ __device__ void operator-=(int4 &a, int b)
+{
+ a.x -= b; a.y -= b; a.z -= b; a.w -= b;
+}
+
+inline __host__ __device__ uint4 operator-(uint4 a, uint4 b)
+{
+ return make_uint4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
+}
+inline __host__ __device__ void operator-=(uint4 &a, uint4 b)
+{
+ a.x -= b.x; a.y -= b.y; a.z -= b.z; a.w -= b.w;
+}
+inline __host__ __device__ uint4 operator-(uint4 a, uint b)
+{
+ return make_uint4(a.x - b, a.y - b, a.z - b, a.w - b);
+}
+inline __host__ __device__ uint4 operator-(uint b, uint4 a)
+{
+ return make_uint4(b - a.x, b - a.y, b - a.z, b - a.w);
+}
+inline __host__ __device__ void operator-=(uint4 &a, uint b)
+{
+ a.x -= b; a.y -= b; a.z -= b; a.w -= b;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// multiply
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator*(float2 a, float2 b)
+{
+ return make_float2(a.x * b.x, a.y * b.y);
+}
+inline __host__ __device__ void operator*=(float2 &a, float2 b)
+{
+ a.x *= b.x; a.y *= b.y;
+}
+inline __host__ __device__ float2 operator*(float2 a, float b)
+{
+ return make_float2(a.x * b, a.y * b);
+}
+inline __host__ __device__ float2 operator*(float b, float2 a)
+{
+ return make_float2(b * a.x, b * a.y);
+}
+inline __host__ __device__ void operator*=(float2 &a, float b)
+{
+ a.x *= b; a.y *= b;
+}
+
+inline __host__ __device__ int2 operator*(int2 a, int2 b)
+{
+ return make_int2(a.x * b.x, a.y * b.y);
+}
+inline __host__ __device__ void operator*=(int2 &a, int2 b)
+{
+ a.x *= b.x; a.y *= b.y;
+}
+inline __host__ __device__ int2 operator*(int2 a, int b)
+{
+ return make_int2(a.x * b, a.y * b);
+}
+inline __host__ __device__ int2 operator*(int b, int2 a)
+{
+ return make_int2(b * a.x, b * a.y);
+}
+inline __host__ __device__ void operator*=(int2 &a, int b)
+{
+ a.x *= b; a.y *= b;
+}
+
+inline __host__ __device__ uint2 operator*(uint2 a, uint2 b)
+{
+ return make_uint2(a.x * b.x, a.y * b.y);
+}
+inline __host__ __device__ void operator*=(uint2 &a, uint2 b)
+{
+ a.x *= b.x; a.y *= b.y;
+}
+inline __host__ __device__ uint2 operator*(uint2 a, uint b)
+{
+ return make_uint2(a.x * b, a.y * b);
+}
+inline __host__ __device__ uint2 operator*(uint b, uint2 a)
+{
+ return make_uint2(b * a.x, b * a.y);
+}
+inline __host__ __device__ void operator*=(uint2 &a, uint b)
+{
+ a.x *= b; a.y *= b;
+}
+
+inline __host__ __device__ float3 operator*(float3 a, float3 b)
+{
+ return make_float3(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+inline __host__ __device__ void operator*=(float3 &a, float3 b)
+{
+ a.x *= b.x; a.y *= b.y; a.z *= b.z;
+}
+inline __host__ __device__ float3 operator*(float3 a, float b)
+{
+ return make_float3(a.x * b, a.y * b, a.z * b);
+}
+inline __host__ __device__ float3 operator*(float b, float3 a)
+{
+ return make_float3(b * a.x, b * a.y, b * a.z);
+}
+inline __host__ __device__ void operator*=(float3 &a, float b)
+{
+ a.x *= b; a.y *= b; a.z *= b;
+}
+
+inline __host__ __device__ int3 operator*(int3 a, int3 b)
+{
+ return make_int3(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+inline __host__ __device__ void operator*=(int3 &a, int3 b)
+{
+ a.x *= b.x; a.y *= b.y; a.z *= b.z;
+}
+inline __host__ __device__ int3 operator*(int3 a, int b)
+{
+ return make_int3(a.x * b, a.y * b, a.z * b);
+}
+inline __host__ __device__ int3 operator*(int b, int3 a)
+{
+ return make_int3(b * a.x, b * a.y, b * a.z);
+}
+inline __host__ __device__ void operator*=(int3 &a, int b)
+{
+ a.x *= b; a.y *= b; a.z *= b;
+}
+
+inline __host__ __device__ uint3 operator*(uint3 a, uint3 b)
+{
+ return make_uint3(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+inline __host__ __device__ void operator*=(uint3 &a, uint3 b)
+{
+ a.x *= b.x; a.y *= b.y; a.z *= b.z;
+}
+inline __host__ __device__ uint3 operator*(uint3 a, uint b)
+{
+ return make_uint3(a.x * b, a.y * b, a.z * b);
+}
+inline __host__ __device__ uint3 operator*(uint b, uint3 a)
+{
+ return make_uint3(b * a.x, b * a.y, b * a.z);
+}
+inline __host__ __device__ void operator*=(uint3 &a, uint b)
+{
+ a.x *= b; a.y *= b; a.z *= b;
+}
+
+inline __host__ __device__ float4 operator*(float4 a, float4 b)
+{
+ return make_float4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w);
+}
+inline __host__ __device__ void operator*=(float4 &a, float4 b)
+{
+ a.x *= b.x; a.y *= b.y; a.z *= b.z; a.w *= b.w;
+}
+inline __host__ __device__ float4 operator*(float4 a, float b)
+{
+ return make_float4(a.x * b, a.y * b, a.z * b, a.w * b);
+}
+inline __host__ __device__ float4 operator*(float b, float4 a)
+{
+ return make_float4(b * a.x, b * a.y, b * a.z, b * a.w);
+}
+inline __host__ __device__ void operator*=(float4 &a, float b)
+{
+ a.x *= b; a.y *= b; a.z *= b; a.w *= b;
+}
+
+inline __host__ __device__ int4 operator*(int4 a, int4 b)
+{
+ return make_int4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w);
+}
+inline __host__ __device__ void operator*=(int4 &a, int4 b)
+{
+ a.x *= b.x; a.y *= b.y; a.z *= b.z; a.w *= b.w;
+}
+inline __host__ __device__ int4 operator*(int4 a, int b)
+{
+ return make_int4(a.x * b, a.y * b, a.z * b, a.w * b);
+}
+inline __host__ __device__ int4 operator*(int b, int4 a)
+{
+ return make_int4(b * a.x, b * a.y, b * a.z, b * a.w);
+}
+inline __host__ __device__ void operator*=(int4 &a, int b)
+{
+ a.x *= b; a.y *= b; a.z *= b; a.w *= b;
+}
+
+inline __host__ __device__ uint4 operator*(uint4 a, uint4 b)
+{
+ return make_uint4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w);
+}
+inline __host__ __device__ void operator*=(uint4 &a, uint4 b)
+{
+ a.x *= b.x; a.y *= b.y; a.z *= b.z; a.w *= b.w;
+}
+inline __host__ __device__ uint4 operator*(uint4 a, uint b)
+{
+ return make_uint4(a.x * b, a.y * b, a.z * b, a.w * b);
+}
+inline __host__ __device__ uint4 operator*(uint b, uint4 a)
+{
+ return make_uint4(b * a.x, b * a.y, b * a.z, b * a.w);
+}
+inline __host__ __device__ void operator*=(uint4 &a, uint b)
+{
+ a.x *= b; a.y *= b; a.z *= b; a.w *= b;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// divide
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator/(float2 a, float2 b)
+{
+ return make_float2(a.x / b.x, a.y / b.y);
+}
+inline __host__ __device__ void operator/=(float2 &a, float2 b)
+{
+ a.x /= b.x; a.y /= b.y;
+}
+inline __host__ __device__ float2 operator/(float2 a, float b)
+{
+ return make_float2(a.x / b, a.y / b);
+}
+inline __host__ __device__ void operator/=(float2 &a, float b)
+{
+ a.x /= b; a.y /= b;
+}
+inline __host__ __device__ float2 operator/(float b, float2 a)
+{
+ return make_float2(b / a.x, b / a.y);
+}
+
+inline __host__ __device__ float3 operator/(float3 a, float3 b)
+{
+ return make_float3(a.x / b.x, a.y / b.y, a.z / b.z);
+}
+inline __host__ __device__ void operator/=(float3 &a, float3 b)
+{
+ a.x /= b.x; a.y /= b.y; a.z /= b.z;
+}
+inline __host__ __device__ float3 operator/(float3 a, float b)
+{
+ return make_float3(a.x / b, a.y / b, a.z / b);
+}
+inline __host__ __device__ void operator/=(float3 &a, float b)
+{
+ a.x /= b; a.y /= b; a.z /= b;
+}
+inline __host__ __device__ float3 operator/(float b, float3 a)
+{
+ return make_float3(b / a.x, b / a.y, b / a.z);
+}
+
+inline __host__ __device__ float4 operator/(float4 a, float4 b)
+{
+ return make_float4(a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w);
+}
+inline __host__ __device__ void operator/=(float4 &a, float4 b)
+{
+ a.x /= b.x; a.y /= b.y; a.z /= b.z; a.w /= b.w;
+}
+inline __host__ __device__ float4 operator/(float4 a, float b)
+{
+ return make_float4(a.x / b, a.y / b, a.z / b, a.w / b);
+}
+inline __host__ __device__ void operator/=(float4 &a, float b)
+{
+ a.x /= b; a.y /= b; a.z /= b; a.w /= b;
+}
+inline __host__ __device__ float4 operator/(float b, float4 a){
+ return make_float4(b / a.x, b / a.y, b / a.z, b / a.w);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// min
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 fminf(float2 a, float2 b)
+{
+ return make_float2(fminf(a.x,b.x), fminf(a.y,b.y));
+}
+inline __host__ __device__ float3 fminf(float3 a, float3 b)
+{
+ return make_float3(fminf(a.x,b.x), fminf(a.y,b.y), fminf(a.z,b.z));
+}
+inline __host__ __device__ float4 fminf(float4 a, float4 b)
+{
+ return make_float4(fminf(a.x,b.x), fminf(a.y,b.y), fminf(a.z,b.z), fminf(a.w,b.w));
+}
+
+inline __host__ __device__ int2 min(int2 a, int2 b)
+{
+ return make_int2(min(a.x,b.x), min(a.y,b.y));
+}
+inline __host__ __device__ int3 min(int3 a, int3 b)
+{
+ return make_int3(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z));
+}
+inline __host__ __device__ int4 min(int4 a, int4 b)
+{
+ return make_int4(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z), min(a.w,b.w));
+}
+
+inline __host__ __device__ uint2 min(uint2 a, uint2 b)
+{
+ return make_uint2(min(a.x,b.x), min(a.y,b.y));
+}
+inline __host__ __device__ uint3 min(uint3 a, uint3 b)
+{
+ return make_uint3(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z));
+}
+inline __host__ __device__ uint4 min(uint4 a, uint4 b)
+{
+ return make_uint4(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z), min(a.w,b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// max
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 fmaxf(float2 a, float2 b)
+{
+ return make_float2(fmaxf(a.x,b.x), fmaxf(a.y,b.y));
+}
+inline __host__ __device__ float3 fmaxf(float3 a, float3 b)
+{
+ return make_float3(fmaxf(a.x,b.x), fmaxf(a.y,b.y), fmaxf(a.z,b.z));
+}
+inline __host__ __device__ float4 fmaxf(float4 a, float4 b)
+{
+ return make_float4(fmaxf(a.x,b.x), fmaxf(a.y,b.y), fmaxf(a.z,b.z), fmaxf(a.w,b.w));
+}
+
+inline __host__ __device__ int2 max(int2 a, int2 b)
+{
+ return make_int2(max(a.x,b.x), max(a.y,b.y));
+}
+inline __host__ __device__ int3 max(int3 a, int3 b)
+{
+ return make_int3(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z));
+}
+inline __host__ __device__ int4 max(int4 a, int4 b)
+{
+ return make_int4(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z), max(a.w,b.w));
+}
+
+inline __host__ __device__ uint2 max(uint2 a, uint2 b)
+{
+ return make_uint2(max(a.x,b.x), max(a.y,b.y));
+}
+inline __host__ __device__ uint3 max(uint3 a, uint3 b)
+{
+ return make_uint3(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z));
+}
+inline __host__ __device__ uint4 max(uint4 a, uint4 b)
+{
+ return make_uint4(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z), max(a.w,b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// lerp
+// - linear interpolation between a and b, based on value t in [0, 1] range
+////////////////////////////////////////////////////////////////////////////////
+
+inline __device__ __host__ float lerp(float a, float b, float t)
+{
+ return a + t*(b-a);
+}
+inline __device__ __host__ float2 lerp(float2 a, float2 b, float t)
+{
+ return a + t*(b-a);
+}
+inline __device__ __host__ float3 lerp(float3 a, float3 b, float t)
+{
+ return a + t*(b-a);
+}
+inline __device__ __host__ float4 lerp(float4 a, float4 b, float t)
+{
+ return a + t*(b-a);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// clamp
+// - clamp the value v to be in the range [a, b]
+////////////////////////////////////////////////////////////////////////////////
+
+inline __device__ __host__ float clamp(float f, float a, float b)
+{
+ return fmaxf(a, fminf(f, b));
+}
+inline __device__ __host__ int clamp(int f, int a, int b)
+{
+ return max(a, min(f, b));
+}
+inline __device__ __host__ uint clamp(uint f, uint a, uint b)
+{
+ return max(a, min(f, b));
+}
+
+inline __device__ __host__ float2 clamp(float2 v, float a, float b)
+{
+ return make_float2(clamp(v.x, a, b), clamp(v.y, a, b));
+}
+inline __device__ __host__ float2 clamp(float2 v, float2 a, float2 b)
+{
+ return make_float2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y));
+}
+inline __device__ __host__ float3 clamp(float3 v, float a, float b)
+{
+ return make_float3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b));
+}
+inline __device__ __host__ float3 clamp(float3 v, float3 a, float3 b)
+{
+ return make_float3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z));
+}
+inline __device__ __host__ float4 clamp(float4 v, float a, float b)
+{
+ return make_float4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b));
+}
+inline __device__ __host__ float4 clamp(float4 v, float4 a, float4 b)
+{
+ return make_float4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w));
+}
+
+inline __device__ __host__ int2 clamp(int2 v, int a, int b)
+{
+ return make_int2(clamp(v.x, a, b), clamp(v.y, a, b));
+}
+inline __device__ __host__ int2 clamp(int2 v, int2 a, int2 b)
+{
+ return make_int2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y));
+}
+inline __device__ __host__ int3 clamp(int3 v, int a, int b)
+{
+ return make_int3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b));
+}
+inline __device__ __host__ int3 clamp(int3 v, int3 a, int3 b)
+{
+ return make_int3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z));
+}
+inline __device__ __host__ int4 clamp(int4 v, int a, int b)
+{
+ return make_int4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b));
+}
+inline __device__ __host__ int4 clamp(int4 v, int4 a, int4 b)
+{
+ return make_int4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w));
+}
+
+inline __device__ __host__ uint2 clamp(uint2 v, uint a, uint b)
+{
+ return make_uint2(clamp(v.x, a, b), clamp(v.y, a, b));
+}
+inline __device__ __host__ uint2 clamp(uint2 v, uint2 a, uint2 b)
+{
+ return make_uint2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y));
+}
+inline __device__ __host__ uint3 clamp(uint3 v, uint a, uint b)
+{
+ return make_uint3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b));
+}
+inline __device__ __host__ uint3 clamp(uint3 v, uint3 a, uint3 b)
+{
+ return make_uint3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z));
+}
+inline __device__ __host__ uint4 clamp(uint4 v, uint a, uint b)
+{
+ return make_uint4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b));
+}
+inline __device__ __host__ uint4 clamp(uint4 v, uint4 a, uint4 b)
+{
+ return make_uint4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// dot product
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float dot(float2 a, float2 b)
+{
+ return a.x * b.x + a.y * b.y;
+}
+inline __host__ __device__ float dot(float3 a, float3 b)
+{
+ return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+inline __host__ __device__ float dot(float4 a, float4 b)
+{
+ return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+inline __host__ __device__ int dot(int2 a, int2 b)
+{
+ return a.x * b.x + a.y * b.y;
+}
+inline __host__ __device__ int dot(int3 a, int3 b)
+{
+ return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+inline __host__ __device__ int dot(int4 a, int4 b)
+{
+ return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+inline __host__ __device__ uint dot(uint2 a, uint2 b)
+{
+ return a.x * b.x + a.y * b.y;
+}
+inline __host__ __device__ uint dot(uint3 a, uint3 b)
+{
+ return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+inline __host__ __device__ uint dot(uint4 a, uint4 b)
+{
+ return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// length
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float length(float2 v)
+{
+ return sqrtf(dot(v, v));
+}
+inline __host__ __device__ float length(float3 v)
+{
+ return sqrtf(dot(v, v));
+}
+inline __host__ __device__ float length(float4 v)
+{
+ return sqrtf(dot(v, v));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// normalize
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 normalize(float2 v)
+{
+ float invLen = rsqrtf(dot(v, v));
+ return v * invLen;
+}
+inline __host__ __device__ float3 normalize(float3 v)
+{
+ float invLen = rsqrtf(dot(v, v));
+ return v * invLen;
+}
+inline __host__ __device__ float4 normalize(float4 v)
+{
+ float invLen = rsqrtf(dot(v, v));
+ return v * invLen;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// floor
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 floorf(float2 v)
+{
+ return make_float2(floorf(v.x), floorf(v.y));
+}
+inline __host__ __device__ float3 floorf(float3 v)
+{
+ return make_float3(floorf(v.x), floorf(v.y), floorf(v.z));
+}
+inline __host__ __device__ float4 floorf(float4 v)
+{
+ return make_float4(floorf(v.x), floorf(v.y), floorf(v.z), floorf(v.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// frac - returns the fractional portion of a scalar or each vector component
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float fracf(float v)
+{
+ return v - floorf(v);
+}
+inline __host__ __device__ float2 fracf(float2 v)
+{
+ return make_float2(fracf(v.x), fracf(v.y));
+}
+inline __host__ __device__ float3 fracf(float3 v)
+{
+ return make_float3(fracf(v.x), fracf(v.y), fracf(v.z));
+}
+inline __host__ __device__ float4 fracf(float4 v)
+{
+ return make_float4(fracf(v.x), fracf(v.y), fracf(v.z), fracf(v.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// fmod
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 fmodf(float2 a, float2 b)
+{
+ return make_float2(fmodf(a.x, b.x), fmodf(a.y, b.y));
+}
+inline __host__ __device__ float3 fmodf(float3 a, float3 b)
+{
+ return make_float3(fmodf(a.x, b.x), fmodf(a.y, b.y), fmodf(a.z, b.z));
+}
+inline __host__ __device__ float4 fmodf(float4 a, float4 b)
+{
+ return make_float4(fmodf(a.x, b.x), fmodf(a.y, b.y), fmodf(a.z, b.z), fmodf(a.w, b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// absolute value
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 fabs(float2 v)
+{
+ return make_float2(fabs(v.x), fabs(v.y));
+}
+inline __host__ __device__ float3 fabs(float3 v)
+{
+ return make_float3(fabs(v.x), fabs(v.y), fabs(v.z));
+}
+inline __host__ __device__ float4 fabs(float4 v)
+{
+ return make_float4(fabs(v.x), fabs(v.y), fabs(v.z), fabs(v.w));
+}
+
+inline __host__ __device__ int2 abs(int2 v)
+{
+ return make_int2(abs(v.x), abs(v.y));
+}
+inline __host__ __device__ int3 abs(int3 v)
+{
+ return make_int3(abs(v.x), abs(v.y), abs(v.z));
+}
+inline __host__ __device__ int4 abs(int4 v)
+{
+ return make_int4(abs(v.x), abs(v.y), abs(v.z), abs(v.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// reflect
+// - returns reflection of incident ray I around surface normal N
+// - N should be normalized, reflected vector's length is equal to length of I
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float3 reflect(float3 i, float3 n)
+{
+ return i - 2.0f * n * dot(n,i);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// cross product
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float3 cross(float3 a, float3 b)
+{
+ return make_float3(a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// smoothstep
+// - returns 0 if x < a
+// - returns 1 if x > b
+// - otherwise returns smooth interpolation between 0 and 1 based on x
+////////////////////////////////////////////////////////////////////////////////
+
+inline __device__ __host__ float smoothstep(float a, float b, float x)
+{
+ float y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+ return (y*y*(3.0f - (2.0f*y)));
+}
+inline __device__ __host__ float2 smoothstep(float2 a, float2 b, float2 x)
+{
+ float2 y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+ return (y*y*(make_float2(3.0f) - (make_float2(2.0f)*y)));
+}
+inline __device__ __host__ float3 smoothstep(float3 a, float3 b, float3 x)
+{
+ float3 y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+ return (y*y*(make_float3(3.0f) - (make_float3(2.0f)*y)));
+}
+inline __device__ __host__ float4 smoothstep(float4 a, float4 b, float4 x)
+{
+ float4 y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+ return (y*y*(make_float4(3.0f) - (make_float4(2.0f)*y)));
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+#ifndef PCL_CUDA_PCL_CUDA_BASE_H_
+#define PCL_CUDA_PCL_CUDA_BASE_H_
+
+#include <boost/shared_ptr.hpp>
+#include <pcl/cuda/point_cloud.h>
+
+namespace pcl
+{
+namespace cuda
+{
+ ///////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief PCL base class. Implements methods that are used by all PCL objects.
+ */
+ template <typename CloudT>
+ class PCLCUDABase
+ {
+ public:
+ typedef CloudT PointCloud;
+ typedef typename PointCloud::Ptr PointCloudPtr;
+ typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+ /** \brief Empty constructor. */
+ PCLCUDABase () : input_() {};
+
+ /** \brief Provide a pointer to the input dataset
+ * \param cloud the const boost shared pointer to a PointCloud message
+ */
+ virtual inline void
+ setInputCloud (const PointCloudConstPtr &cloud)
+ {
+ input_ = cloud;
+ }
+
+ /** \brief Get a pointer to the input host point cloud dataset. */
+ inline PointCloudConstPtr const
+ getInputCloud ()
+ {
+ return (input_);
+ }
+
+ protected:
+ /** \brief The input point cloud dataset. */
+ PointCloudConstPtr input_;
+
+ /** \brief This method should get called before starting the actual computation. */
+ bool
+ initCompute ()
+ {
+ // Check if input was set
+ if (!input_)
+ return (false);
+ return (true);
+ }
+
+ /** \brief This method should get called after finishing the actual computation. */
+ bool
+ deinitCompute ()
+ {
+ return (true);
+ }
+ };
+} // namespace
+} // namespace
+
+#endif //#ifndef PCL_PCL_BASE_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_CUDA_POINT_CLOUD_H_
+#define PCL_CUDA_POINT_CLOUD_H_
+
+#include <pcl/cuda/point_types.h>
+#include <pcl/cuda/thrust.h>
+#include <boost/shared_ptr.hpp>
+
+namespace pcl
+{
+ namespace cuda
+ {
+ /** \brief misnamed class holding a 3x3 matrix */
+ struct CovarianceMatrix
+ {
+ float3 data[3];
+ };
+
+ /** \brief Simple structure holding RGB data. */
+ struct OpenNIRGB
+ {
+ unsigned char r, g, b;
+ };
+
+ /** \brief Host helper class. Contains several typedefs and some static
+ * functions to help writing portable code (that runs both on host
+ * and device) */
+ template <typename T>
+ struct Host
+ {
+ // vector type
+ typedef typename thrust::host_vector<T> type;
+
+// // iterator type
+// typedef thrust::detail::normal_iterator<T*> type;
+//
+// // pointer type
+// typedef T* pointer_type;
+//
+// // allocator
+// static T* alloc (int size)
+// {
+// return (T*) malloc (size);
+// }
+//
+// // cast to different pointer
+// template <typename U>
+// static U* cast (type ptr)
+// {
+// return (U*)ptr;
+// }
+ };
+
+ /** \brief Device helper class. Contains several typedefs and some static
+ * functions to help writing portable code (that runs both on host
+ * and device) */
+ template <typename T>
+ struct Device
+ {
+ // vector type
+ typedef typename thrust::device_vector<T> type;
+
+// // iterator type
+// typedef thrust::detail::normal_iterator<thrust::device_ptr<T> > iterator_type;
+//
+// // pointer type
+// typedef thrust::device_ptr<T> pointer_type;
+//
+// // allocator
+// static thrust::device_ptr<T> alloc (int size)
+// {
+// return thrust::device_malloc<T> (size);
+// }
+//
+// // cast to different pointer
+// template <typename U>
+// static thrust::device_ptr<U> cast (type ptr)
+// {
+// return thrust::device_ptr<U> ((U*)ptr.get());
+// }
+//
+// // cast raw pointer to different pointer
+// template <typename U>
+// static thrust::device_ptr<U> cast (T* ptr)
+// {
+// return thrust::device_ptr<U> ((U*)ptr);
+// }
+ };
+
+ /** @b PointCloudAOS represents an AOS (Array of Structs) PointCloud
+ * implementation for CUDA processing.
+ *
+ * This is the most efficient way to perform operations on x86 architectures
+ * (using SSE alignment).
+ */
+ template <template <typename> class Storage>
+ class PointCloudAOS
+ {
+ public:
+ PointCloudAOS () : width (0), height (0), is_dense (true)
+ {}
+
+ //////////////////////////////////////////////////////////////////////////////////////
+ inline PointCloudAOS& operator = (const PointCloudAOS& rhs)
+ {
+ points = rhs.points;
+ width = rhs.width;
+ height = rhs.height;
+ is_dense = rhs.is_dense;
+ return (*this);
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////
+ template <typename OtherStorage>
+ inline PointCloudAOS& operator << (const OtherStorage& rhs)
+ {
+ points = rhs.points;
+ // TODO: Test speed on operator () = vs resize+copy
+ //points.resize (rhs.points.size ());
+ //thrust::copy (rhs.points.begin (), rhs.points.end (), points.begin ());
+ width = rhs.width;
+ height = rhs.height;
+ is_dense = rhs.is_dense;
+ return (*this);
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////
+ inline PointXYZRGB
+ at (int u, int v) const
+ {
+ if (this->height > 1)
+ return (points[v * this->width + u]);
+ else
+ return (PointXYZRGB (std::numeric_limits<float>::quiet_NaN (),
+ std::numeric_limits<float>::quiet_NaN (),
+ std::numeric_limits<float>::quiet_NaN (),
+ 0));
+ // throw IsNotDenseException ("Can't use 2D indexing with a sparse point cloud");
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////
+ inline PointXYZRGB& operator () (int u, int v)
+ {
+ return (points[v* this->width +u]);
+ }
+ inline const PointXYZRGB& operator () (int u, int v) const
+ {
+ return (points[v* this->width +u]);
+ }
+
+ /** \brief The point data. */
+ //typename Storage<float3>::type points;
+ typename Storage<PointXYZRGB>::type points;
+
+ typedef typename Storage<PointXYZRGB>::type::iterator iterator;
+
+ /** \brief The point cloud width (if organized as an image-structure). */
+ unsigned int width;
+ /** \brief The point cloud height (if organized as an image-structure). */
+ unsigned int height;
+
+ /** \brief True if no points are invalid (e.g., have NaN or Inf values). */
+ bool is_dense;
+
+ typedef boost::shared_ptr<PointCloudAOS<Storage> > Ptr;
+ typedef boost::shared_ptr<const PointCloudAOS<Storage> > ConstPtr;
+ };
+
+ /** @b PointCloudSOA represents a SOA (Struct of Arrays) PointCloud
+ * implementation for CUDA processing.
+ */
+ template <template <typename> class Storage>
+ class PointCloudSOA
+ {
+ public:
+ PointCloudSOA () : width (0), height (0), is_dense (true)
+ {}
+
+ //////////////////////////////////////////////////////////////////////////////////////
+ inline PointCloudSOA& operator = (const PointCloudSOA& rhs)
+ {
+ points_x = rhs.points_x;
+ points_y = rhs.points_y;
+ points_z = rhs.points_z;
+ width = rhs.width;
+ height = rhs.height;
+ is_dense = rhs.is_dense;
+ return (*this);
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////
+ template <typename OtherStorage>
+ inline PointCloudSOA& operator << (const OtherStorage& rhs)
+ {
+ points_x = rhs.points_x;
+ points_y = rhs.points_y;
+ points_z = rhs.points_z;
+ width = rhs.width;
+ height = rhs.height;
+ is_dense = rhs.is_dense;
+ return (*this);
+ }
+
+ /** \brief Resize the internal point data vectors.
+ * \param newsize the new size
+ */
+ void
+ resize (size_t newsize)
+ {
+ assert (sane ());
+ points_x.resize (newsize);
+ points_y.resize (newsize);
+ points_z.resize (newsize);
+ }
+
+ /** \brief Return the size of the internal vectors */
+ std::size_t
+ size () const
+ {
+ assert (sane ());
+ return (points_x.size ());
+ }
+
+ /** \brief Check if the internal pooint data vectors are valid. */
+ bool
+ sane () const
+ {
+ return (points_x.size () == points_y.size () &&
+ points_x.size () == points_z.size ());
+ }
+
+ /** \brief The point data. */
+ typename Storage<float>::type points_x;
+ typename Storage<float>::type points_y;
+ typename Storage<float>::type points_z;
+ typename Storage<int>::type rgb;
+
+ /** \brief The point cloud width (if organized as an image-structure). */
+ unsigned int width;
+ /** \brief The point cloud height (if organized as an image-structure). */
+ unsigned int height;
+
+ /** \brief True if no points are invalid (e.g., have NaN or Inf values). */
+ bool is_dense;
+
+ typedef boost::shared_ptr<PointCloudSOA<Storage> > Ptr;
+ typedef boost::shared_ptr<const PointCloudSOA<Storage> > ConstPtr;
+
+ //////////////////////////////////////////////////////////////////////////////////////
+ // Extras. Testing ZIP iterators
+ typedef thrust::tuple<float, float, float> tuple_type;
+ typedef typename Storage<float>::type::iterator float_iterator;
+ typedef thrust::tuple<float_iterator, float_iterator, float_iterator> iterator_tuple;
+ typedef thrust::zip_iterator<iterator_tuple> zip_iterator;
+
+ zip_iterator
+ zip_begin ()
+ {
+ return (thrust::make_zip_iterator (thrust::make_tuple (points_x.begin (),
+ points_y.begin (),
+ points_z.begin ())));
+ }
+
+ zip_iterator
+ zip_end ()
+ {
+ return (thrust::make_zip_iterator (thrust::make_tuple (points_x.end (),
+ points_y.end (),
+ points_z.end ())));
+ }
+ };
+
+ template <template <typename> class Storage, typename T>
+ struct PointIterator
+ {
+ typedef void type;
+ };
+
+ template <typename T>
+ struct PointIterator<Device,T>
+ {
+ typedef thrust::detail::normal_iterator<thrust::device_ptr<T> > type;
+ };
+
+ template <typename T>
+ struct PointIterator<Host,T>
+ {
+ typedef thrust::detail::normal_iterator<T*> type;
+ };
+
+ template <template <typename> class Storage, typename T>
+ struct StoragePointer
+ {
+ // typedef void* type;
+ };
+
+ template <typename T>
+ struct StoragePointer<Device,T>
+ {
+ typedef thrust::device_ptr<T> type;
+ template <typename U>
+ static thrust::device_ptr<U> cast (type ptr)
+ {
+ return thrust::device_ptr<U> ((U*)ptr.get());
+ }
+ template <typename U>
+ static thrust::device_ptr<U> cast (T* ptr)
+ {
+ return thrust::device_ptr<U> ((U*)ptr);
+ }
+ };
+
+ template <typename T>
+ struct StoragePointer<Host,T>
+ {
+ typedef T* type;
+ template <typename U>
+ static U* cast (type ptr)
+ {
+ return (U*)ptr;
+ }
+ };
+ template <template <typename> class Storage, typename T>
+ struct StorageAllocator
+ {
+ };
+
+ template <typename T>
+ struct StorageAllocator<Device,T>
+ {
+ static thrust::device_ptr<T> alloc (int size)
+ {
+ return thrust::device_malloc<T> (size);
+ }
+ };
+
+ template <typename T>
+ struct StorageAllocator<Host,T>
+ {
+ static T* alloc (int size)
+ {
+ return (T*) malloc (size);
+ }
+ };
+
+
+ } // namespace
+} // namespace
+
+#endif //#ifndef PCL_CUDA_POINT_CLOUD_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_CUDA_POINT_TYPES_H_
+#define PCL_CUDA_POINT_TYPES_H_
+
+#include <pcl/cuda/common/point_type_rgb.h>
+#include <cuda.h>
+
+namespace pcl
+{
+namespace cuda
+{
+ /** \brief Default point xyz-rgb structure. */
+ struct /*__align__(16)*/ PointXYZRGB
+ {
+ inline __host__ __device__ PointXYZRGB () {}
+ inline __host__ __device__ PointXYZRGB (float _x, float _y, float _z, int _rgb) :
+ x(_x), y(_y), z(_z), rgb(_rgb) {}
+
+ // Declare a union for XYZ
+ union
+ {
+ float3 xyz;
+ struct
+ {
+ float x;
+ float y;
+ float z;
+ };
+ };
+ RGB rgb;
+
+ inline __host__ __device__ bool operator == (const PointXYZRGB &rhs)
+ {
+ return (x == rhs.x && y == rhs.y && z == rhs.z && rgb == rhs.rgb);
+ }
+
+ // this allows direct assignment of a PointXYZRGB to float3...
+ inline __host__ __device__ operator float3 () const
+ {
+ return xyz;
+ }
+
+ const inline __host__ __device__ PointXYZRGB operator - (const PointXYZRGB &rhs) const
+ {
+ PointXYZRGB res = *this;
+ res -= rhs;
+ return (res);
+// xyz = -rhs.xyz;
+// rgb = -rhs.rgb;
+// return (*this -= rhs);
+ }
+
+ inline __host__ __device__ PointXYZRGB& operator += (const PointXYZRGB &rhs)
+ {
+ xyz += rhs.xyz;
+ rgb += rhs.rgb;
+ return (*this);
+ }
+
+ inline __host__ __device__ PointXYZRGB& operator -= (const PointXYZRGB &rhs)
+ {
+ xyz -= rhs.xyz;
+ rgb -= rhs.rgb;
+ return (*this);
+ }
+
+ inline __host__ __device__ PointXYZRGB& operator *= (const PointXYZRGB &rhs)
+ {
+ xyz *= rhs.xyz;
+ rgb *= rhs.rgb;
+ return (*this);
+ }
+
+ inline __host__ __device__ PointXYZRGB& operator /= (const PointXYZRGB &rhs)
+ {
+ xyz /= rhs.xyz;
+ rgb /= rhs.rgb;
+ return (*this);
+ }
+ };
+
+ /** \brief Default point xyz-rgb structure. */
+ struct __align__(16) PointXYZRGBNormal
+ {
+ inline __host__ __device__ PointXYZRGBNormal () {}
+ inline __host__ __device__ PointXYZRGBNormal (float _x, float _y, float _z, int _rgb) :
+ x(_x), y(_y), z(_z), rgb(_rgb) {}
+
+ // Declare a union for XYZ
+ union
+ {
+ float3 xyz;
+ struct
+ {
+ float x;
+ float y;
+ float z;
+ };
+ };
+ RGB rgb;
+ union
+ {
+ float4 normal;
+ struct
+ {
+ float normal_x;
+ float normal_y;
+ float normal_z;
+ float curvature;
+ };
+ };
+
+ inline __host__ __device__ bool operator == (const PointXYZRGBNormal &rhs)
+ {
+ return (x == rhs.x && y == rhs.y && z == rhs.z && rgb == rhs.rgb && normal_x == rhs.normal_x && normal_y == rhs.normal_y && normal_z == rhs.normal_z);
+ }
+
+ // this allows direct assignment of a PointXYZRGBNormal to float3...
+ inline __host__ __device__ operator float3 () const
+ {
+ return xyz;
+ }
+
+ const inline __host__ __device__ PointXYZRGBNormal operator - (const PointXYZRGBNormal &rhs) const
+ {
+ PointXYZRGBNormal res = *this;
+ res -= rhs;
+ return (res);
+// xyz = -rhs.xyz;
+// rgb = -rhs.rgb;
+// return (*this -= rhs);
+ }
+
+ inline __host__ __device__ PointXYZRGBNormal& operator += (const PointXYZRGBNormal &rhs)
+ {
+ xyz += rhs.xyz;
+ rgb += rhs.rgb;
+ normal += rhs.normal;
+ return (*this);
+ }
+
+ inline __host__ __device__ PointXYZRGBNormal& operator -= (const PointXYZRGBNormal &rhs)
+ {
+ xyz -= rhs.xyz;
+ rgb -= rhs.rgb;
+ normal -= rhs.normal;
+ return (*this);
+ }
+
+ inline __host__ __device__ PointXYZRGBNormal& operator *= (const PointXYZRGBNormal &rhs)
+ {
+ xyz *= rhs.xyz;
+ rgb *= rhs.rgb;
+ normal *= rhs.normal;
+ return (*this);
+ }
+
+ inline __host__ __device__ PointXYZRGBNormal& operator /= (const PointXYZRGBNormal &rhs)
+ {
+ xyz /= rhs.xyz;
+ rgb /= rhs.rgb;
+ normal /= rhs.normal;
+ return (*this);
+ }
+ };
+} // namespace
+} // namespace
+
+#endif //#ifndef PCL_CUDA_POINT_TYPES_H_
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: point_cloud.h 4927 2012-03-07 03:35:53Z rusu $
+ *
+ */
+
+#ifndef PCL_CUDA_COMMON_THRUST_H_
+#define PCL_CUDA_COMMON_THRUST_H_
+
+#include <thrust/host_vector.h>
+#include <thrust/device_vector.h>
+#include <thrust/copy.h>
+#include <thrust/device_ptr.h>
+#include <thrust/sequence.h>
+
+#endif // PCL_CUDA_COMMON_THRUST_H_
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_CUDA_TIME_CPU_H_
+#define PCL_CUDA_TIME_CPU_H_
+
+#include <iostream>
+#include <cmath>
+#include <string>
+
+#ifdef WIN32
+# define NOMINMAX
+# define WIN32_LEAN_AND_MEAN
+# include <Windows.h>
+# include <time.h>
+#else
+# include <sys/time.h>
+#endif
+
+/////////////////////////////////////////////////////////////////////
+// Note: this file is here because NVCC can't deal with all boost
+// things, e.g. the new pcl::ScopeTime implementation
+/////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+ namespace cuda
+ {
+ /**
+ * \brief Class to measure the time spent in a scope
+ *
+ * To use this class, e.g. to measure the time spent in a function,
+ * just create an instance at the beginning of the function.
+ * \ingroup common
+ */
+ class ScopeTimeCPU
+ {
+ public:
+ inline ScopeTimeCPU (const char* title);
+ inline ~ScopeTimeCPU ();
+ private:
+ std::string title_;
+ double start_time_;
+ };
+
+
+#ifndef MEASURE_FUNCTION_TIME
+#define MEASURE_FUNCTION_TIME \
+ ScopeTimeCPU scopeTime(__func__)
+#endif
+
+ inline double getTime ();
+
+/// Executes code, only if secs are gone since last exec.
+#ifndef DO_EVERY_TS
+#define DO_EVERY_TS(secs, currentTime, code) \
+if (1) {\
+ static double s_lastDone_ = 0.0; \
+ double s_now_ = (currentTime); \
+ if (s_lastDone_ > s_now_) \
+ s_lastDone_ = s_now_; \
+ if (s_now_ - s_lastDone_ > (secs)) { \
+ code; \
+ s_lastDone_ = s_now_; \
+ }\
+} else \
+ (void)0
+#endif
+
+/// Executes code, only if secs are gone since last exec.
+#ifndef DO_EVERY
+#define DO_EVERY(secs, code) \
+ DO_EVERY_TS(secs, pcl::cuda::getTime(), code)
+#endif
+
+ } // end namespace
+} // end namespace
+
+
+inline double
+ pcl::cuda::getTime ()
+{
+#ifdef WIN32
+ LARGE_INTEGER frequency;
+ LARGE_INTEGER timer_tick;
+ QueryPerformanceFrequency(&frequency);
+ QueryPerformanceCounter(&timer_tick);
+ return (double)(timer_tick.QuadPart)/(double)frequency.QuadPart;
+#else
+ timeval current_time;
+ gettimeofday (¤t_time, NULL);
+ return (current_time.tv_sec + 1e-6 * current_time.tv_usec);
+#endif
+}
+
+inline pcl::cuda::ScopeTimeCPU::ScopeTimeCPU (const char* title) : title_ (title)
+{
+ start_time_ = pcl::cuda::getTime ();
+}
+
+inline pcl::cuda::ScopeTimeCPU::~ScopeTimeCPU ()
+{
+ double end_time = pcl::cuda::getTime ();
+ double duration = end_time - start_time_;
+ std::cerr << title_ << " took " << 1000 * duration << "ms. " << std::endl;
+}
+
+#endif //#ifndef PCL_NORMS_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_CUDA_TIME_GPU_H_
+#define PCL_CUDA_TIME_GPU_H_
+
+#include <cuda.h>
+#include <pcl/cuda/cutil_inline.h>
+
+namespace pcl
+{
+ namespace cuda
+ {
+ /**
+ * \brief Class to measure the time spent in a scope
+ *
+ * To use this class, e.g. to measure the time spent in a function,
+ * just create an instance at the beginning of the function.
+ */
+ class ScopeTimeGPU
+ {
+ public:
+ /** \brief Constructor. */
+ inline ScopeTimeGPU (const char* title) : title_ (title)
+ {
+ start ();
+ }
+
+ /** \brief Destructor. */
+ inline ~ScopeTimeGPU ()
+ {
+ stop ();
+ std::cerr << title_ << " took " << elapsed_time_ << "ms.\n";
+ }
+
+ /** \brief Start the timer. */
+ inline void
+ start ()
+ {
+ CUT_CHECK_ERROR ("dude");
+ cutilSafeCall (cudaEventCreate (&start_));
+ cutilSafeCall (cudaEventCreate (&end_));
+
+ cutilSafeCall (cudaEventRecord (start_, 0));
+ }
+
+ /** \brief Stop the timer. */
+ inline void
+ stop ()
+ {
+ CUT_CHECK_ERROR ("dude");
+ // Measure time needed to copy data
+ cutilSafeCall (cudaThreadSynchronize ());
+ cutilSafeCall (cudaEventRecord (end_, 0));
+ cutilSafeCall (cudaEventSynchronize (end_));
+ cutilSafeCall (cudaEventElapsedTime (&elapsed_time_, start_, end_));
+
+ cutilSafeCall (cudaEventDestroy (end_));
+ cutilSafeCall (cudaEventDestroy (start_));
+ }
+
+ /** \brief Stop and print the timer. */
+ inline void stop_print ()
+ {
+ stop ();
+ std::cerr << title_ << " took " << elapsed_time_ << "ms.\n";
+ }
+
+
+ private:
+ cudaEvent_t start_;
+ cudaEvent_t end_;
+
+ float elapsed_time_;
+
+ std::string title_;
+ };
+ } // namespace
+} // namespace
+
+#endif //#ifndef PCL_CUDA_TIMER_H_
--- /dev/null
+set(SUBSYS_NAME cuda_features)
+set(SUBSYS_PATH cuda/features)
+set(SUBSYS_DESC "Point Cloud CUDA Feature Estimation library")
+set(SUBSYS_DEPS cuda_common io common)
+
+# ---[ Point Cloud Library - pcl/cuda/io
+
+set(build TRUE)
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
+mark_as_advanced("BUILD_${SUBSYS_NAME}")
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS})
+PCL_SET_SUBSYS_INCLUDE_DIR("${SUBSYS_NAME}" "${SUBSYS_PATH}")
+
+if (build)
+ set(srcs
+ src/normal_3d.cu
+ )
+
+ set(incs
+ include/pcl/cuda/features/normal_3d.h
+ )
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
+ PCL_CUDA_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs})
+
+ set(EXT_DEPS "")
+ #set(EXT_DEPS CUDA)
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}"
+ "${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "")
+
+ # Install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}" ${incs})
+
+endif()
+
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_CUDA_NORMAL_3D_H_
+#define PCL_CUDA_NORMAL_3D_H_
+
+#include <pcl/pcl_macros.h>
+
+#include <pcl/cuda/common/eigen.h>
+
+namespace pcl
+{
+ namespace cuda
+ {
+
+ // normal estimation using PCA on neighborhood. DANGER: neighborhood is sampled with a bias!! contact Nico for details :P
+ template <typename InputIteratorT, typename OutputIteratorT, template <typename> class Storage>
+ void computePointNormals (InputIteratorT begin, InputIteratorT end, OutputIteratorT output, float focallength, const boost::shared_ptr <const PointCloudAOS <Storage> > &input, float radius, int desired_number_neighbors);
+
+ template <template <typename> class Storage, typename InputIteratorT>
+ boost::shared_ptr<typename Storage<float4>::type> computePointNormals (InputIteratorT begin, InputIteratorT end, float focallength, const boost::shared_ptr <const PointCloudAOS <Storage> > &input, float radius, int desired_number_neighbors);
+
+ // fast normal computations
+ template <typename OutputIteratorT, template <typename> class Storage>
+ void computeFastPointNormals (OutputIteratorT output, const boost::shared_ptr <const PointCloudAOS <Storage> > &input);
+
+ template <template <typename> class Storage>
+ boost::shared_ptr<typename Storage<float4>::type> computeFastPointNormals (const boost::shared_ptr <const PointCloudAOS <Storage> > &input);
+
+ // Weird normal estimation (normal deviations - more of an art project..)
+ template <typename InputIteratorT, typename OutputIteratorT, template <typename> class Storage>
+ void computeWeirdPointNormals (InputIteratorT begin, InputIteratorT end, OutputIteratorT output, float focallength, const boost::shared_ptr <const PointCloudAOS <Storage> > &input, float radius, int desired_number_neighbors);
+
+ template <template <typename> class Storage, typename InputIteratorT>
+ boost::shared_ptr<typename Storage<float4>::type> computeWeirdPointNormals (InputIteratorT begin, InputIteratorT end, float focallength, const boost::shared_ptr <const PointCloudAOS <Storage> > &input, float radius, int desired_number_neighbors);
+ } // namespace
+} // namespace
+
+#endif
+
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: normal_3d.h 1370 2011-06-19 01:06:01Z jspricke $
+ *
+ */
+
+#ifndef PCL_CUDA_NORMAL_3D_H_
+#define PCL_CUDA_NORMAL_3D_H_
+
+#include <pcl/pcl_exports.h>
+
+#include <pcl/cuda/common/eigen.h>
+
+namespace pcl
+{
+ namespace cuda
+ {
+
+ template <template <typename> class Storage>
+ struct NormalEstimationKernel
+ {
+ typedef boost::shared_ptr <const PointCloudAOS <Storage> > CloudConstPtr;
+ NormalEstimationKernel (const boost::shared_ptr <const PointCloudAOS <Storage> > &input, float focallength, float sqr_radius, float sqrt_desired_nr_neighbors)
+ : points_ (thrust::raw_pointer_cast(&input->points[0]))
+ , focallength_ (focallength)
+ , search_ (input, focallength, sqr_radius)
+ , sqr_radius_(sqr_radius)
+ , sqrt_desired_nr_neighbors_ (sqrt_desired_nr_neighbors)
+ {}
+
+ inline __host__ __device__
+ float4 operator () (float3 query_pt)
+ {
+ CovarianceMatrix cov;
+ int nnn = 0;
+ if (!isnan (query_pt.x))
+ nnn =
+ search_.computeCovarianceOnline (query_pt, cov, sqrt_desired_nr_neighbors_);
+ else
+ return make_float4(query_pt.x);
+
+ CovarianceMatrix evecs;
+ float3 evals;
+ // compute eigenvalues and -vectors
+ if (nnn <= 1)
+ return make_float4(0);
+
+ eigen33 (cov, evecs, evals);
+ //float curvature = evals.x / (evals.x + evals.y + evals.z);
+ float curvature = evals.x / (query_pt.z * (0.2f / 4.0f) * query_pt.z * (0.2f / 4.0f));
+
+ float3 mc = normalize (evecs.data[0]);
+ // TODO: this should be an optional step, as it slows down eveything
+ // btw, this flips the normals to face the origin (assumed to be the view point)
+ if ( dot (query_pt, mc) > 0 )
+ mc = -mc;
+ return make_float4 (mc.x, mc.y, mc.z, curvature);
+ }
+
+ const PointXYZRGB *points_;
+ float focallength_;
+ OrganizedRadiusSearch<CloudConstPtr> search_;
+ float sqr_radius_;
+ float sqrt_desired_nr_neighbors_;
+ };
+
+ template <template <typename> class Storage>
+ struct FastNormalEstimationKernel
+ {
+ FastNormalEstimationKernel (const boost::shared_ptr <const PointCloudAOS <Storage> > &input, int width, int height)
+ : points_ (thrust::raw_pointer_cast(&input->points[0])), width_(width), height_(height)
+ {}
+
+ inline __host__ __device__
+ float4 operator () (int idx)
+ {
+ float3 query_pt = points_[idx];
+ if (isnan(query_pt.z))
+ return make_float4 (0.0f,0.0f,0.0f,0.0f);
+
+ int xIdx = idx % width_;
+ int yIdx = idx / width_;
+
+ // are we at a border? are our neighbor valid points?
+ bool west_valid = (xIdx > 1) && !isnan (points_[idx-1].z) && fabs (points_[idx-1].z - query_pt.z) < 200;
+ bool east_valid = (xIdx < width_-1) && !isnan (points_[idx+1].z) && fabs (points_[idx+1].z - query_pt.z) < 200;
+ bool north_valid = (yIdx > 1) && !isnan (points_[idx-width_].z) && fabs (points_[idx-width_].z - query_pt.z) < 200;
+ bool south_valid = (yIdx < height_-1) && !isnan (points_[idx+width_].z) && fabs (points_[idx+width_].z - query_pt.z) < 200;
+
+ float3 horiz, vert;
+ if (west_valid & east_valid)
+ horiz = points_[idx+1] - points_[idx-1];
+ if (west_valid & !east_valid)
+ horiz = points_[idx] - points_[idx-1];
+ if (!west_valid & east_valid)
+ horiz = points_[idx+1] - points_[idx];
+ if (!west_valid & !east_valid)
+ return make_float4 (0.0f,0.0f,0.0f,1.0f);
+
+ if (south_valid & north_valid)
+ vert = points_[idx-width_] - points_[idx+width_];
+ if (south_valid & !north_valid)
+ vert = points_[idx] - points_[idx+width_];
+ if (!south_valid & north_valid)
+ vert = points_[idx-width_] - points_[idx];
+ if (!south_valid & !north_valid)
+ return make_float4 (0.0f,0.0f,0.0f,1.0f);
+
+ float3 normal = cross (horiz, vert);
+
+ float curvature = length (normal);
+ curvature = fabs(horiz.z) > 0.04 | fabs(vert.z) > 0.04 | !west_valid | !east_valid | !north_valid | !south_valid;
+
+ float3 mc = normalize (normal);
+ if ( dot (query_pt, mc) > 0 )
+ mc = -mc;
+ return make_float4 (mc.x, mc.y, mc.z, curvature);
+ }
+
+ const PointXYZRGB *points_;
+ int width_;
+ int height_;
+ };
+
+ template <template <typename> class Storage>
+ struct NormalDeviationKernel
+ {
+ typedef boost::shared_ptr <const PointCloudAOS <Storage> > CloudConstPtr;
+ NormalDeviationKernel (const boost::shared_ptr <const PointCloudAOS <Storage> > &input, float focallength, float sqr_radius, float sqrt_desired_nr_neighbors)
+ : points_ (thrust::raw_pointer_cast(&input->points[0]))
+ , focallength_ (focallength)
+ , search_ (input, focallength, sqr_radius)
+ , sqr_radius_(sqr_radius)
+ , sqrt_desired_nr_neighbors_ (sqrt_desired_nr_neighbors)
+ {}
+
+ template <typename Tuple>
+ inline __host__ __device__
+ float4 operator () (Tuple &t)
+ {
+ float3 query_pt = thrust::get<0>(t);
+ float4 normal = thrust::get<1>(t);
+ CovarianceMatrix cov;
+ float3 centroid;
+ if (!isnan (query_pt.x))
+ centroid = search_.computeCentroid (query_pt, cov, sqrt_desired_nr_neighbors_);
+ else
+ return make_float4(query_pt.x);
+
+ float proj = normal.x * (query_pt.x - centroid.x) / sqrt(sqr_radius_) +
+ normal.y * (query_pt.y - centroid.y) / sqrt(sqr_radius_) +
+ normal.z * (query_pt.z - centroid.z) / sqrt(sqr_radius_) ;
+
+
+ //return make_float4 (normal.x*proj, normal.y*proj, normal.z*proj, clamp (fabs (proj), 0.0f, 1.0f));
+ return make_float4 (
+ (centroid.x - query_pt.x) / sqrt(sqr_radius_) ,
+ (centroid.y - query_pt.y) / sqrt(sqr_radius_) ,
+ (centroid.z - query_pt.z) / sqrt(sqr_radius_) ,
+ 0);
+ }
+
+ const PointXYZRGB *points_;
+ float focallength_;
+ OrganizedRadiusSearch<CloudConstPtr> search_;
+ float sqr_radius_;
+ float sqrt_desired_nr_neighbors_;
+ };
+
+ } // namespace
+} // namespace
+
+#endif
+
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include "pcl/cuda/features/normal_3d_kernels.h"
+
+namespace pcl
+{
+ namespace cuda
+ {
+
+ template <typename InputIteratorT, typename OutputIteratorT, template <typename> class Storage>
+ void computePointNormals (InputIteratorT begin, InputIteratorT end, OutputIteratorT output, float focallength, const boost::shared_ptr <const PointCloudAOS <Storage> > &input, float radius, int desired_number_neighbors)
+ {
+ NormalEstimationKernel<Storage> ne = NormalEstimationKernel<Storage> (input, focallength, radius*radius, sqrt ((float)desired_number_neighbors));
+ thrust::transform (begin, end, output, ne);
+ }
+
+ template <template <typename> class Storage, typename InputIteratorT>
+ boost::shared_ptr<typename Storage<float4>::type> computePointNormals (InputIteratorT begin, InputIteratorT end,
+ float focallength, const boost::shared_ptr <const PointCloudAOS <Storage> > &input, float radius, int desired_number_neighbors)
+ {
+ boost::shared_ptr<typename Storage<float4>::type> normals (new typename Storage<float4>::type);
+ normals->resize (end - begin);
+ computePointNormals (begin, end, normals->begin(), focallength, input, radius, desired_number_neighbors);
+ return normals;
+ }
+
+ template <typename OutputIteratorT, template <typename> class Storage>
+ void computeFastPointNormals (OutputIteratorT output, const boost::shared_ptr <const PointCloudAOS <Storage> > &input)
+ {
+ FastNormalEstimationKernel<Storage> ne = FastNormalEstimationKernel<Storage> (input, input->width, input->height);
+ thrust::transform (thrust::counting_iterator<int>(0), thrust::counting_iterator<int>(0) + input->width * input->height, output, ne);
+ }
+
+ template <template <typename> class Storage>
+ boost::shared_ptr<typename Storage<float4>::type> computeFastPointNormals (const boost::shared_ptr <const PointCloudAOS <Storage> > &input)
+ {
+ boost::shared_ptr<typename Storage<float4>::type> normals (new typename Storage<float4>::type);
+ normals->resize (input->points.size());
+ computeFastPointNormals (normals->begin(), input);
+ return normals;
+ }
+
+ template <typename InputIteratorT, typename OutputIteratorT, template <typename> class Storage>
+ void computeWeirdPointNormals (InputIteratorT begin, InputIteratorT end, OutputIteratorT output, float focallength, const boost::shared_ptr <const PointCloudAOS <Storage> > &input, float radius, int desired_number_neighbors)
+ {
+ NormalEstimationKernel<Storage> ne = NormalEstimationKernel<Storage> (input, focallength, radius*radius, sqrt ((float)desired_number_neighbors));
+ thrust::transform (begin, end, output, ne);
+
+ // we have normals now.
+
+ NormalDeviationKernel<Storage> nd = NormalDeviationKernel<Storage> (input, focallength, radius*radius, sqrt ((float)desired_number_neighbors));
+ thrust::transform (thrust::make_zip_iterator (thrust::make_tuple (begin, output)), thrust::make_zip_iterator (thrust::make_tuple (begin, output)) + (end-begin), output, nd);
+ }
+
+ template <template <typename> class Storage, typename InputIteratorT>
+ boost::shared_ptr<typename Storage<float4>::type> computeWeirdPointNormals (InputIteratorT begin, InputIteratorT end, float focallength, const boost::shared_ptr <const PointCloudAOS <Storage> > &input, float radius, int desired_number_neighbors)
+ {
+ boost::shared_ptr<typename Storage<float4>::type> normals (new typename Storage<float4>::type);
+ normals->resize (end - begin);
+ computeWeirdPointNormals (begin, end, normals->begin(), focallength, input, radius, desired_number_neighbors);
+ return normals;
+ }
+
+
+ // Aaaand, a couple of instantiations
+ template PCL_EXPORTS void computePointNormals<typename Device<PointXYZRGB>::type::const_iterator, typename Device<float4>::type::iterator, Device>
+ (Device<PointXYZRGB>::type::const_iterator begin,
+ Device<PointXYZRGB>::type::const_iterator end,
+ Device<float4>::type::iterator output,
+ float focallength,
+ const boost::shared_ptr <const PointCloudAOS <Device> > &input,
+ float radius,
+ int desired_number_neighbors);
+
+ template PCL_EXPORTS void computePointNormals<typename Host<PointXYZRGB>::type::const_iterator, typename Host<float4>::type::iterator, Host>
+ (Host<PointXYZRGB>::type::const_iterator begin,
+ Host<PointXYZRGB>::type::const_iterator end,
+ Host<float4>::type::iterator output,
+ float focallength,
+ const boost::shared_ptr <const PointCloudAOS <Host> > &input,
+ float radius,
+ int desired_number_neighbors);
+
+ template PCL_EXPORTS boost::shared_ptr<typename Device<float4>::type> computePointNormals<Device, typename PointIterator<Device,PointXYZRGB>::type >
+ (PointIterator<Device,PointXYZRGB>::type begin,
+ PointIterator<Device,PointXYZRGB>::type end,
+ float focallength,
+ const boost::shared_ptr <const PointCloudAOS <Device> > &input,
+ float radius,
+ int desired_number_neighbors);
+
+ template PCL_EXPORTS boost::shared_ptr<typename Host<float4>::type> computePointNormals<Host, typename PointIterator<Host,PointXYZRGB>::type >
+ (PointIterator<Host,PointXYZRGB>::type begin,
+ PointIterator<Host,PointXYZRGB>::type end,
+ float focallength,
+ const boost::shared_ptr <const PointCloudAOS <Host> > &input,
+ float radius,
+ int desired_number_neighbors);
+
+ // Aaaand, a couple of instantiations
+ template PCL_EXPORTS void computeFastPointNormals<typename Device<float4>::type::iterator, Device>
+ (Device<float4>::type::iterator output,
+ const boost::shared_ptr <const PointCloudAOS <Device> > &input);
+
+ template PCL_EXPORTS void computeFastPointNormals<typename Host<float4>::type::iterator, Host>
+ (Host<float4>::type::iterator output,
+ const boost::shared_ptr <const PointCloudAOS <Host> > &input);
+
+ template PCL_EXPORTS boost::shared_ptr<typename Device<float4>::type> computeFastPointNormals<Device>
+ (const boost::shared_ptr <const PointCloudAOS <Device> > &input);
+
+ template PCL_EXPORTS boost::shared_ptr<typename Host<float4>::type> computeFastPointNormals<Host>
+ (const boost::shared_ptr <const PointCloudAOS <Host> > &input);
+
+ // Aaaand, a couple of instantiations
+ template PCL_EXPORTS void computeWeirdPointNormals<typename Device<PointXYZRGB>::type::const_iterator, typename Device<float4>::type::iterator, Device>
+ (Device<PointXYZRGB>::type::const_iterator begin,
+ Device<PointXYZRGB>::type::const_iterator end,
+ Device<float4>::type::iterator output,
+ float focallength,
+ const boost::shared_ptr <const PointCloudAOS <Device> > &input,
+ float radius,
+ int desired_number_neighbors);
+
+ template PCL_EXPORTS void computeWeirdPointNormals<typename Host<PointXYZRGB>::type::const_iterator, typename Host<float4>::type::iterator, Host>
+ (Host<PointXYZRGB>::type::const_iterator begin,
+ Host<PointXYZRGB>::type::const_iterator end,
+ Host<float4>::type::iterator output,
+ float focallength,
+ const boost::shared_ptr <const PointCloudAOS <Host> > &input,
+ float radius,
+ int desired_number_neighbors);
+
+ template PCL_EXPORTS boost::shared_ptr<typename Device<float4>::type> computeWeirdPointNormals<Device, typename PointIterator<Device,PointXYZRGB>::type >
+ (PointIterator<Device,PointXYZRGB>::type begin,
+ PointIterator<Device,PointXYZRGB>::type end,
+ float focallength,
+ const boost::shared_ptr <const PointCloudAOS <Device> > &input,
+ float radius,
+ int desired_number_neighbors);
+
+ template PCL_EXPORTS boost::shared_ptr<typename Host<float4>::type> computeWeirdPointNormals<Host, typename PointIterator<Host,PointXYZRGB>::type >
+ (PointIterator<Host,PointXYZRGB>::type begin,
+ PointIterator<Host,PointXYZRGB>::type end,
+ float focallength,
+ const boost::shared_ptr <const PointCloudAOS <Host> > &input,
+ float radius,
+ int desired_number_neighbors);
+
+ } // namespace
+} // namespace
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_CUDA_FILTER_H_
+#define PCL_CUDA_FILTER_H_
+
+#include <pcl_cuda/pcl_cuda_base.h>
+#include <float.h>
+
+namespace pcl_cuda
+{
+ /** \brief Removes points with x, y, or z equal to NaN
+ * \param cloud_in the input point cloud
+ * \param cloud_out the input point cloud
+ * \param index the mapping (ordered): cloud_out.points[i] = cloud_in.points[index[i]]
+ * \note The density of the point cloud is lost.
+ * \note Can be called with cloud_in == cloud_out
+ */
+// template <typename PointT> void removeNaNFromPointCloud (const pcl::PointCloud<PointT> &cloud_in, pcl::PointCloud<PointT> &cloud_out, std::vector<int> &index);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b Filter represents the base filter class. Some generic 3D
+ * operations that are applicable to all filters are defined here as static
+ * methods.
+ */
+ template <typename CloudT>
+ class Filter : public PCLCUDABase<CloudT>
+ {
+ using PCLCUDABase<CloudT>::initCompute;
+ using PCLCUDABase<CloudT>::deinitCompute;
+
+ public:
+ using PCLCUDABase<CloudT>::input_;
+
+ typedef typename PCLCUDABase<CloudT>::PointCloud PointCloud;
+ typedef typename PointCloud::Ptr PointCloudPtr;
+ typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+ /** \brief Empty constructor. */
+ Filter () : filter_field_name_ (""),
+ filter_limit_min_ (-FLT_MAX), filter_limit_max_ (FLT_MAX),
+ filter_limit_negative_ (false)
+ {};
+
+ /** \brief Provide the name of the field to be used for filtering data.
+ * In conjunction with \a setFilterLimits, points having values outside
+ * this interval will be discarded.
+ * \param field_name the name of the field that contains values used for filtering
+ */
+ inline void
+ setFilterFieldName (const std::string &field_name) { filter_field_name_ = field_name; }
+
+ /** \brief Get the name of the field used for filtering. */
+ inline std::string const
+ getFilterFieldName () { return (filter_field_name_); }
+
+ /** \brief Set the field filter limits. All points having field values
+ * outside this interval will be discarded.
+ * \param limit_min the minimum allowed field value
+ * \param limit_max the maximum allowed field value
+ */
+ inline void
+ setFilterLimits (const double &limit_min, const double &limit_max)
+ {
+ filter_limit_min_ = limit_min;
+ filter_limit_max_ = limit_max;
+ }
+
+ /** \brief Get the field filter limits (min/max) set by the user.
+ * The default values are -FLT_MAX, FLT_MAX.
+ * \param limit_min the minimum limit
+ * \param limit_max the maximum limit
+ */
+ inline void
+ getFilterLimits (double &limit_min, double &limit_max)
+ {
+ limit_min = filter_limit_min_;
+ limit_max = filter_limit_max_;
+ }
+
+ /** \brief Set to true if we want to return the data outside the interval
+ * specified by setFilterLimits (min, max). Default: false.
+ * \param limit_negative return data inside the interval (false) or outside (true)
+ */
+ inline void
+ setFilterLimitsNegative (const bool limit_negative)
+ {
+ filter_limit_negative_ = limit_negative;
+ }
+
+ /** \brief Get whether the data outside the interval (min/max) is to be
+ * returned (true) or inside (false).
+ * \param limit_negative the limit_negative flag
+ */
+ inline void
+ getFilterLimitsNegative (bool &limit_negative) { limit_negative = filter_limit_negative_; }
+ inline bool
+ getFilterLimitsNegative () { return (filter_limit_negative_); }
+
+ /** \brief Calls the filtering method and returns the filtered dataset on the device
+ * \param output the resultant filtered point cloud dataset on the device
+ */
+ inline void
+ filter (PointCloud &output)
+ {
+ if (!initCompute ()) return;
+
+ // Copy header at a minimum
+ //output.header = input_->header;
+ //output.sensor_origin_ = input_->sensor_origin_;
+ //output.sensor_orientation_ = input_->sensor_orientation_;
+
+ // Apply the actual filter
+ applyFilter (output);
+
+ deinitCompute ();
+ }
+
+ protected:
+ /** \brief The filter name. */
+ std::string filter_name_;
+
+ /** \brief The desired user filter field name. */
+ std::string filter_field_name_;
+
+ /** \brief The minimum allowed filter value a point will be considered from. */
+ double filter_limit_min_;
+
+ /** \brief The maximum allowed filter value a point will be considered from. */
+ double filter_limit_max_;
+
+ /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */
+ bool filter_limit_negative_;
+
+ /** \brief Abstract filter method.
+ *
+ * The implementation needs to set output.{points, width, height, is_dense}.
+ */
+ virtual void
+ applyFilter (PointCloud &output) = 0;
+
+ /** \brief Get a string representation of the name of this class. */
+ inline const std::string&
+ getClassName () const { return (filter_name_); }
+ };
+}
+
+#endif //#ifndef PCL_FILTER_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_CUDA_FILTERS_PASSTHROUGH_H_
+#define PCL_CUDA_FILTERS_PASSTHROUGH_H_
+
+#include <pcl_cuda/filters/filter.h>
+#include <thrust/count.h>
+#include <thrust/remove.h>
+#include <vector_types.h>
+
+namespace pcl_cuda
+{
+
+ /** \brief Check if a specific point is valid or not. Applicable to AOS structures. */
+ struct isFiniteAOS
+ {
+ __inline__ __device__ bool
+ operator () (const PointXYZRGB &pt)
+ //operator () (const float3 &pt)
+ {
+ return (isfinite (pt.x) && isfinite (pt.y) && isfinite (pt.z));
+ }
+ };
+
+ /** \brief Check if a specific point is valid or not. Applicable to SOA structures. */
+ struct isFiniteSOA
+ {
+ __inline__ __device__ bool
+ operator () (const float &pt)
+ {
+ return (isfinite (pt));
+ }
+ };
+
+ /** \brief Check if a specific point is valid or not. Applicable to SOA structures. */
+ struct isFiniteZIPSOA
+ {
+ __inline__ __device__ bool
+ operator () (const PointCloudSOA<Device>::tuple_type& tuple)
+ {
+ using thrust::get;
+ return (!isfinite (get<0> (tuple)) ||
+ !isfinite (get<1> (tuple)) ||
+ !isfinite (get<2> (tuple)));
+ }
+ };
+
+ ///////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b PassThrough uses the base Filter class methods to pass through
+ * all data that satisfies the user given constraints.
+ */
+ template <typename CloudT>
+ class PassThrough: public Filter<CloudT>
+ {
+ public:
+ using Filter<CloudT>::filter_name_;
+
+ typedef typename PCLCUDABase<CloudT>::PointCloud PointCloud;
+ typedef typename PointCloud::Ptr PointCloudPtr;
+ typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+ /** \brief Empty constructor. */
+ PassThrough ()
+ {
+ filter_name_ = "PassThrough";
+ };
+
+ protected:
+ /** \brief Filter a Point Cloud.
+ * \param output the resultant point cloud message
+ */
+ void
+ applyFilter (PointCloud &output)
+ {
+ std::cerr << "applyFilter" << std::endl;
+ }
+ };
+
+ ///////////////////////////////////////////////////////////////////////////////////////////
+ template <>
+ class PassThrough<PointCloudAOS<Device> >: public Filter<PointCloudAOS<Device> >
+ {
+ public:
+ /** \brief Empty constructor. */
+ PassThrough ()
+ {
+ filter_name_ = "PassThroughAOS";
+ };
+
+ protected:
+ /** \brief Filter a Point Cloud.
+ * \param output the resultant point cloud message
+ */
+ void
+ applyFilter (PointCloud &output)
+ {
+ // Allocate enough space
+ output.points.resize (input_->points.size ());
+ // Copy data
+ Device<PointXYZRGB>::type::iterator nr_points = thrust::copy_if (input_->points.begin (), input_->points.end (), output.points.begin (), isFiniteAOS ());
+ //Device<float3>::type::iterator nr_points = thrust::copy_if (input_->points.begin (), input_->points.end (), output.points.begin (), isFiniteAOS ());
+ output.points.resize (nr_points - output.points.begin ());
+
+ //std::cerr << "[applyFilterAOS]: ";
+ //std::cerr << input_->points.size () << " " << output.points.size () << std::endl;
+ }
+ };
+
+ //////////////////////////////////////////////////////////////////////////////////////////
+ template <>
+ class PassThrough<PointCloudSOA<Device> >: public Filter<PointCloudSOA<Device> >
+ {
+ public:
+ /** \brief Empty constructor. */
+ PassThrough () : zip_(false)
+ {
+ filter_name_ = "PassThroughSOA";
+ };
+
+ inline void
+ setZip (bool zip)
+ {
+ zip_ = zip;
+ }
+
+
+ protected:
+ /** \brief Filter a Point Cloud.
+ * \param output the resultant point cloud message
+ */
+ void
+ applyFilter (PointCloud &output)
+ {
+ if (!zip_)
+ {
+ // Allocate enough space
+ output.resize (input_->size ());
+ // Copy data
+ Device<float>::type::iterator nr_points = thrust::copy_if (input_->points_x.begin (), input_->points_x.end (), output.points_x.begin (), isFiniteSOA ());
+ nr_points = thrust::copy_if (input_->points_y.begin (), input_->points_y.end (), output.points_y.begin (), isFiniteSOA ());
+ nr_points = thrust::copy_if (input_->points_z.begin (), input_->points_z.end (), output.points_z.begin (), isFiniteSOA ());
+ output.resize (nr_points - output.points_z.begin ());
+
+ //std::cerr << "[applyFilterSOA]: ";
+ //std::cerr << input_->size () << " " << output.size () << std::endl;
+ }
+
+ else
+ {
+ output = *input_;
+ PointCloud::zip_iterator result = thrust::remove_if (output.zip_begin (), output.zip_end (), isFiniteZIPSOA ());
+ PointCloud::iterator_tuple result_tuple = result.get_iterator_tuple ();
+ PointCloud::float_iterator xiter = thrust::get<0> (result_tuple),
+ yiter = thrust::get<1> (result_tuple),
+ ziter = thrust::get<2> (result_tuple);
+
+ unsigned badpoints = distance (xiter, output.points_x.end ());
+ unsigned goodpoints = distance (output.points_x.begin (), xiter);
+
+ output.resize (goodpoints);
+
+ //std::cerr << "[applyFilterSOA-ZIP]: ";
+ //std::cerr << input_->size () << " " << output.size () << std::endl;
+ }
+ }
+
+ private:
+ bool zip_;
+ };
+}
+
+#endif //#ifndef PCL_FILTERS_PASSTHROUGH_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_CUDA_FILTERS_VOXELGRID_H_
+#define PCL_CUDA_FILTERS_VOXELGRID_H_
+
+#include <pcl_cuda/filters/filter.h>
+#include <pcl_cuda/filters/passthrough.h>
+#include <thrust/count.h>
+#include <thrust/remove.h>
+#include <vector_types.h>
+
+namespace pcl_cuda
+{
+ ///////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
+ */
+ template <typename CloudT>
+ class VoxelGrid: public Filter<CloudT>
+ {
+ public:
+ using Filter<CloudT>::filter_name_;
+
+ typedef typename PCLCUDABase<CloudT>::PointCloud PointCloud;
+ typedef typename PointCloud::Ptr PointCloudPtr;
+ typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+ /** \brief Empty constructor. */
+ VoxelGrid ()
+ {
+ filter_name_ = "VoxelGrid";
+ };
+
+ protected:
+ /** \brief Filter a Point Cloud.
+ * \param output the resultant point cloud message
+ */
+ void
+ applyFilter (PointCloud &output)
+ {
+ std::cerr << "applyFilter" << std::endl;
+ }
+ };
+
+ ///////////////////////////////////////////////////////////////////////////////////////////
+ template <>
+ class VoxelGrid<PointCloudAOS<Device> >: public Filter<PointCloudAOS<Device> >
+ {
+ public:
+ /** \brief Empty constructor. */
+ VoxelGrid ()
+ {
+ filter_name_ = "VoxelGridAOS";
+ };
+
+ protected:
+ /** \brief Filter a Point Cloud.
+ * \param output the resultant point cloud message
+ */
+ void
+ applyFilter (PointCloud &output)
+ {
+ // Allocate enough space
+ output.points.resize (input_->points.size ());
+ // Copy data
+ Device<PointXYZRGB>::type::iterator nr_points = thrust::copy_if (input_->points.begin (), input_->points.end (), output.points.begin (), isFiniteAOS ());
+ output.points.resize (nr_points - output.points.begin ());
+
+ //std::cerr << "[applyFilterAOS]: ";
+ //std::cerr << input_->points.size () << " " << output.points.size () << std::endl;
+ }
+ };
+
+ //////////////////////////////////////////////////////////////////////////////////////////
+ template <>
+ class VoxelGrid<PointCloudSOA<Device> >: public Filter<PointCloudSOA<Device> >
+ {
+ public:
+ /** \brief Empty constructor. */
+ VoxelGrid () : zip_(false)
+ {
+ filter_name_ = "VoxelGridSOA";
+ };
+
+ inline void
+ setZip (bool zip)
+ {
+ zip_ = zip;
+ }
+
+
+ protected:
+ /** \brief Filter a Point Cloud.
+ * \param output the resultant point cloud message
+ */
+ void
+ applyFilter (PointCloud &output)
+ {
+ if (!zip_)
+ {
+ // Allocate enough space
+ output.resize (input_->size ());
+ // Copy data
+ Device<float>::type::iterator nr_points = thrust::copy_if (input_->points_x.begin (), input_->points_x.end (), output.points_x.begin (), isFiniteSOA ());
+ nr_points = thrust::copy_if (input_->points_y.begin (), input_->points_y.end (), output.points_y.begin (), isFiniteSOA ());
+ nr_points = thrust::copy_if (input_->points_z.begin (), input_->points_z.end (), output.points_z.begin (), isFiniteSOA ());
+ output.resize (nr_points - output.points_z.begin ());
+
+ //std::cerr << "[applyFilterSOA]: ";
+ //std::cerr << input_->size () << " " << output.size () << std::endl;
+ }
+
+ else
+ {
+ output = *input_;
+ PointCloud::zip_iterator result = thrust::remove_if (output.zip_begin (), output.zip_end (), isFiniteZIPSOA ());
+ PointCloud::iterator_tuple result_tuple = result.get_iterator_tuple ();
+ PointCloud::float_iterator xiter = thrust::get<0> (result_tuple),
+ yiter = thrust::get<1> (result_tuple),
+ ziter = thrust::get<2> (result_tuple);
+
+ unsigned badpoints = distance (xiter, output.points_x.end ());
+ unsigned goodpoints = distance (output.points_x.begin (), xiter);
+
+ output.resize (goodpoints);
+
+ //std::cerr << "[applyFilterSOA-ZIP]: ";
+ //std::cerr << input_->size () << " " << output.size () << std::endl;
+ }
+ }
+
+ private:
+ bool zip_;
+ };
+}
+
+#endif //#ifndef PCL_FILTERS_VOXELGRID_H_
--- /dev/null
+set(SUBSYS_NAME cuda_io)
+set(SUBSYS_PATH cuda/io)
+set(SUBSYS_DESC "Point cloud CUDA IO library")
+set(SUBSYS_DEPS cuda_common io common)
+
+# ---[ Point Cloud Library - pcl/cuda/io
+
+set(build TRUE)
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" OFF)
+mark_as_advanced("BUILD_${SUBSYS_NAME}")
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS})
+PCL_SET_SUBSYS_INCLUDE_DIR("${SUBSYS_NAME}" "${SUBSYS_PATH}")
+
+if (build AND HAVE_OPENNI)
+ set(srcs
+ src/disparity_to_cloud.cu
+ src/cloud_to_pcl.cpp
+ src/host_device.cu
+ src/extract_indices.cu
+ src/debayering.cu
+ )
+
+ set(incs
+ include/pcl/cuda/io/cloud_from_pcl.h
+ include/pcl/cuda/io/cloud_to_pcl.h
+ include/pcl/cuda/io/debayering.h
+ include/pcl/cuda/io/disparity_to_cloud.h
+ include/pcl/cuda/io/extract_indices.h
+ include/pcl/cuda/io/host_device.h
+ include/pcl/cuda/io/predicate.h
+ )
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
+ PCL_CUDA_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs})
+
+ set(EXT_DEPS "")
+ #set(EXT_DEPS CUDA)
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}"
+ "${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "")
+
+
+ # Install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}" ${incs})
+
+endif()
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_CUDA_CLOUD_TO_PCL_H_
+#define PCL_CUDA_CLOUD_TO_PCL_H_
+
+#include <pcl/cuda/point_cloud.h>
+
+#include <pcl/pcl_exports.h>
+
+namespace pcl
+{
+ template <typename T>
+ class PointCloud;
+
+ struct PointXYZRGB;
+ struct PointXYZRGBNormal;
+
+ namespace cuda
+ {
+ // convert point cloud with color and normals
+ PCL_EXPORTS void
+ toPCL (const PointCloudAOS<Host> &input, const thrust::host_vector<float4> &normals, pcl::PointCloud<pcl::PointXYZRGBNormal> &output);
+ PCL_EXPORTS void
+ toPCL (const PointCloudAOS<Device> &input, const thrust::device_vector<float4> &normals, pcl::PointCloud<pcl::PointXYZRGBNormal> &output);
+
+ // convert point cloud with color
+ PCL_EXPORTS void
+ toPCL (const PointCloudAOS<Host> &input, pcl::PointCloud<pcl::PointXYZRGB> &output);
+ PCL_EXPORTS void
+ toPCL (const PointCloudAOS<Device> &input, pcl::PointCloud<pcl::PointXYZRGB> &output);
+
+ // convert pcl point cloud with color to pcl::cuda cloud
+ PCL_EXPORTS void
+ fromPCL (const pcl::PointCloud<pcl::PointXYZRGB> &input, PointCloudAOS<Host> &output);
+ PCL_EXPORTS void
+ fromPCL (const pcl::PointCloud<pcl::PointXYZRGB> &input, PointCloudAOS<Device> &output);
+ } // namespace
+} // namespace
+
+#endif //#ifndef PCL_CUDA_CLOUD_TO_PCL_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_CUDA_DEBAYERING_H_
+#define PCL_CUDA_DEBAYERING_H_
+
+#include <pcl/cuda/point_cloud.h>
+#include <pcl/io/openni_camera/openni_image.h>
+
+namespace pcl
+{
+ namespace cuda
+ {
+
+ struct downsampleIndices
+ {
+ downsampleIndices (int width, int height, int stride)
+ : width (width), height (height), stride (stride)
+ {}
+
+ int width, height, stride;
+
+ __host__ __device__
+ bool operator () (int i)
+ {
+ int xIdx = i % width;
+ int yIdx = i / width;
+
+ return ((xIdx % stride == 0) & (yIdx % stride == 0));
+ }
+ };
+
+ template <template <typename> class Storage>
+ struct DebayerBilinear
+ {
+ unsigned width;
+ unsigned height;
+ //static unsigned dataSize;
+ //static unsigned char* global_data; // has to be initialized only once!
+ //unsigned char* data;
+ unsigned char *data;
+ DebayerBilinear (unsigned char *bayer_image, unsigned width, unsigned height);
+ //DebayerBilinear (const boost::shared_ptr<openni_wrapper::Image>& bayer_image);
+
+ __inline__ __host__ __device__ OpenNIRGB operator () (int index) const;
+ };
+ /*
+ struct DebayerEdgeAware
+ {
+ unsigned width;
+ unsigned height;
+ static unsigned dataSize;
+ static unsigned char* global_data; // has to be initialized only once!
+ unsigned char* data;
+ DebayerEdgeAware (const boost::shared_ptr<openni_wrapper::Image>& bayer_image);
+ ~DebayerEdgeAware ();
+
+ __inline__ __host__ __device__ OpenNIRGB operator () (int index) const;
+ };
+ */
+ template<template <typename> class Storage>
+ class DebayeringDownsampling
+ {
+ public:
+ typedef typename Storage<OpenNIRGB>::type RGBImageType;
+ void
+ compute (const boost::shared_ptr<openni_wrapper::Image>& bayer_image, RGBImageType& rgb_image) const;
+ };
+
+ template <template <typename> class Storage>
+ struct YUV2RGBKernel
+ {
+ unsigned width;
+ unsigned height;
+ unsigned char *data;
+ YUV2RGBKernel (unsigned char *yuv_image, unsigned width, unsigned height);
+
+ __inline__ __host__ __device__ OpenNIRGB operator () (int index) const;
+ };
+
+ template<template <typename> class Storage>
+ class YUV2RGB
+ {
+ public:
+ typedef typename Storage<OpenNIRGB>::type RGBImageType;
+ void
+ compute (const boost::shared_ptr<openni_wrapper::Image>& yuv_image, RGBImageType& rgb_image) const;
+ };
+
+ template<template <typename> class Storage>
+ class Debayering
+ {
+ public:
+ typedef typename Storage<OpenNIRGB>::type RGBImageType;
+ void
+ computeBilinear (const boost::shared_ptr<openni_wrapper::Image>& bayer_image, RGBImageType& rgb_image) const;
+
+ //void
+ //computeEdgeAware (const boost::shared_ptr<openni_wrapper::Image>& bayer_image, thrust::host_vector<OpenNIRGB>& rgb_image) const;
+
+ //void
+ //computeEdgeAware (const boost::shared_ptr<openni_wrapper::Image>& bayer_image, thrust::device_vector<OpenNIRGB>& rgb_image) const;
+ };
+
+ } // namespace
+} // namespace
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_CUDA_DISPARITY_TO_CLOUD_H_
+#define PCL_CUDA_DISPARITY_TO_CLOUD_H_
+
+#include <pcl/cuda/point_cloud.h>
+#include <pcl/cuda/io/cloud_to_pcl.h>
+#include <pcl/io/openni_camera/openni_image.h>
+#include <pcl/io/openni_camera/openni_depth_image.h>
+//#include <pcl/CameraInfo.h>
+//#include <pcl/PCLImage.h>
+
+#include <boost/cstdint.hpp>
+
+namespace pcl
+{
+namespace cuda
+{
+ /** \brief Compute the XYZ values for a point based on disparity information. */
+ struct ComputeXYZ
+ {
+ int width, height;
+ int center_x, center_y;
+ float constant;
+ float bad_point;
+
+ ComputeXYZ (int w, int h, int cx, int cy, float con) :
+ width(w), height(h), center_x(cx), center_y(cy), constant(con)
+ {
+ bad_point = std::numeric_limits<float>::quiet_NaN ();
+ }
+
+ template <typename Tuple> __inline__ __host__ __device__ PointXYZRGB
+ operator () (const Tuple &t);
+ };
+
+ /** \brief Compute the XYZ and RGB values for a point based on disparity information. */
+ struct ComputeXYZRGB
+ {
+ int width, height;
+ int center_x, center_y;
+ float constant;
+ float bad_point;
+
+ ComputeXYZRGB (int w, int h, int cx, int cy, float con) :
+ width(w), height(h), center_x(cx), center_y(cy), constant(con)
+ {
+ bad_point = std::numeric_limits<float>::quiet_NaN ();
+ }
+
+ template <typename Tuple> __inline__ __host__ __device__ PointXYZRGB
+ operator () (const Tuple &t);
+ };
+
+ /** \brief Disparity to PointCloudAOS generator.
+ */
+ class PCL_EXPORTS DisparityToCloud
+ {
+ public:
+// // compute using ROS images, Device output
+// void
+// compute (const pcl::PCLImage::ConstPtr &depth_image,
+// const pcl::PCLImage::ConstPtr &rgb_image,
+// const pcl::CameraInfo::ConstPtr &info,
+// PointCloudAOS<Device>::Ptr &output);
+//
+// // compute using ROS images, Host output
+// void
+// compute (const pcl::PCLImage::ConstPtr &depth_image,
+// const pcl::PCLImage::ConstPtr &rgb_image,
+// const pcl::CameraInfo::ConstPtr &info,
+// PointCloudAOS<Host>::Ptr &output);
+
+ // compute using OpenNI images, Device output
+ template <template <typename> class Storage> void
+ compute (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ const boost::shared_ptr<openni_wrapper::Image>& image,
+ float constant,
+ typename PointCloudAOS<Storage>::Ptr &output,
+ bool downsample = false, int stride = 2, int smoothing_nr_iterations = 0, int smoothing_filter_size = 2);
+
+ template <template <typename> class Storage> void
+ compute (const boost::uint16_t* depth_image,
+ const OpenNIRGB* rgb_image,
+ int width, int height,
+ float constant,
+ typename PointCloudAOS<Storage>::Ptr &output,
+ int smoothing_nr_iterations = 0, int smoothing_filter_size = 2);
+
+ // compute using OpenNI images, Host output
+/* void
+ compute (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ const boost::shared_ptr<openni_wrapper::Image>& image,
+ float constant,
+ PointCloudAOS<Host>::Ptr &output);*/
+
+ // ...
+// void
+// compute (const pcl::PCLImage::ConstPtr &depth_image,
+// const pcl::CameraInfo::ConstPtr &info,
+// PointCloudAOS<Device>::Ptr &output);
+//
+// void
+// compute (const pcl::PCLImage::ConstPtr &depth_image,
+// const pcl::CameraInfo::ConstPtr &info,
+// PointCloudAOS<Host>::Ptr &output);
+
+ void
+ compute (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ float constant,
+ PointCloudAOS<Device>::Ptr &output);
+
+ void
+ compute (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ float constant,
+ PointCloudAOS<Host>::Ptr &output);
+ };
+
+} // namespace
+} // namespace
+
+#endif //#ifndef PCL_CUDA_DISPARITY_TO_CLOUD_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_CUDA_EXTRACT_INDICES_H_
+#define PCL_CUDA_EXTRACT_INDICES_H_
+
+#include <pcl/cuda/point_cloud.h>
+
+namespace pcl
+{
+namespace cuda
+{
+ template <template <typename> class Storage, class DataT, class MaskT>
+ void extractMask (const boost::shared_ptr<typename Storage<DataT>::type> &input,
+ MaskT* mask,
+ boost::shared_ptr<typename Storage<DataT>::type> &output);
+ template <template <typename> class Storage, class T>
+ void extractMask (const typename PointCloudAOS<Storage>::Ptr &input,
+ T* mask,
+ typename PointCloudAOS<Storage>::Ptr &output);
+
+ template <template <typename> class Storage>
+ void extractIndices (const typename PointCloudAOS<Storage>::Ptr &input,
+ typename Storage<int>::type& indices,
+ typename PointCloudAOS<Storage>::Ptr &output);
+
+ template <template <typename> class Storage>
+ void removeIndices (const typename PointCloudAOS<Storage>::Ptr &input,
+ typename Storage<int>::type& indices,
+ typename PointCloudAOS<Storage>::Ptr &output);
+
+ template <template <typename> class Storage>
+ void extractIndices (const typename PointCloudAOS<Storage>::Ptr &input,
+ typename Storage<int>::type& indices,
+ typename PointCloudAOS<Storage>::Ptr &output, const OpenNIRGB& color);
+
+ template <template <typename> class Storage>
+ void removeIndices (const typename PointCloudAOS<Storage>::Ptr &input,
+ typename Storage<int>::type& indices,
+ typename PointCloudAOS<Storage>::Ptr &output, const OpenNIRGB& color);
+ template <template <typename> class Storage>
+ void colorIndices (typename PointCloudAOS<Storage>::Ptr &input,
+ boost::shared_ptr<typename Storage<int>::type> indices,
+ const OpenNIRGB& color);
+ template <template <typename> class Storage>
+ void colorCloud (typename PointCloudAOS<Storage>::Ptr &input,
+ typename Storage<char4>::type &colors);
+} // namespace
+} // namespace
+
+#endif //#ifndef PCL_CUDA_EXTRACT_INDICES_H_
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_CUDA_HOST_DEVICE_H_
+#define PCL_CUDA_HOST_DEVICE_H_
+
+#include <pcl/cuda/point_cloud.h>
+
+namespace pcl
+{
+ template <typename T>
+ class PointCloud;
+
+ struct PointXYZRGB;
+
+ namespace cuda
+ {
+
+ template <template <typename> class Storage, template <typename> class OtherStorage>
+ typename PointCloudAOS<OtherStorage>::Ptr toStorage (const PointCloudAOS<Storage> &input);
+
+ template <template <typename> class Storage>
+ void toHost (const PointCloudAOS<Storage> &input, PointCloudAOS<Host> &output);
+
+ template <template <typename> class Storage>
+ void toDevice (const PointCloudAOS<Storage> &input, PointCloudAOS<Device> &output);
+
+ } // namespace
+} // namespace
+
+#endif //#ifndef PCL_CUDA_HOST_DEVICE_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_CUDA_KINECT_SMOOTHING_H_
+#define PCL_CUDA_KINECT_SMOOTHING_H_
+
+#include <pcl/io/openni_camera/openni_image.h>
+#include <thrust/tuple.h>
+#include <pcl/pcl_exports.h>
+
+namespace pcl
+{
+ namespace cuda
+ {
+
+ struct DisparityBoundSmoothing
+ {
+ DisparityBoundSmoothing (int width, int height, int window_size, float focal_length, float baseline, float disparity_threshold, float *data, float *raw_data)
+ : width_ (width), height_ (height), window_size_ (window_size)
+ , focal_length_(focal_length), baseline_(baseline), disparity_threshold_(disparity_threshold)
+ , data_(data), raw_data_(raw_data)
+ {}
+
+ int width_, height_;
+ int window_size_;
+ float focal_length_;
+ float baseline_;
+ float disparity_threshold_;
+ float *data_;
+ float *raw_data_;
+
+ // helper function
+ inline __host__ __device__
+ float disparity2depth (float disparity)
+ {
+ return baseline_ * focal_length_ / disparity;
+ }
+
+ // helper function
+ inline __host__ __device__
+ float depth2disparity (float depth)
+ {
+ return baseline_ * focal_length_ / depth;
+ }
+
+ // clampToDisparityBounds
+ inline __host__ __device__
+ float clampToDisparityBounds (float avg_depth, float depth)
+ {
+ float disparity = depth2disparity (depth);
+ float avg_disparity = depth2disparity (avg_depth);
+ float min_disparity = disparity - disparity_threshold_;
+ float max_disparity = disparity + disparity_threshold_;
+
+ if (avg_disparity > max_disparity)
+ return disparity2depth (max_disparity);
+ if (avg_disparity < min_disparity)
+ return disparity2depth (min_disparity);
+
+ return avg_depth;
+ }
+
+ // actual kernel operator
+ inline __host__ __device__
+ float operator () (int idx)
+ {
+ float depth = data_ [idx];
+#ifdef __CUDACC__
+ if (depth == 0 | isnan(depth) | isinf(depth))
+ return 0;
+#else
+ if (depth == 0 | pcl_isnan(depth) | pcl_isinf(depth))
+ return 0;
+#endif
+ int xIdx = idx % width_;
+ int yIdx = idx / width_;
+ // TODO: test median
+ // This implements a fixed window size in image coordinates (pixels)
+ int4 bounds = make_int4 (
+ xIdx - window_size_,
+ xIdx + window_size_,
+ yIdx - window_size_,
+ yIdx + window_size_
+ );
+
+ // clamp the coordinates to fit to depth image size
+ bounds.x = clamp (bounds.x, 0, width_-1);
+ bounds.y = clamp (bounds.y, 0, width_-1);
+ bounds.z = clamp (bounds.z, 0, height_-1);
+ bounds.w = clamp (bounds.w, 0, height_-1);
+
+ float average_depth = depth;
+ int counter = 1;
+ // iterate over all pixels in the rectangular region
+ for (int y = bounds.z; y <= bounds.w; ++y)
+ {
+ for (int x = bounds.x; x <= bounds.y; ++x)
+ {
+ // find index in point cloud from x,y pixel positions
+ int otherIdx = ((int)y) * width_ + ((int)x);
+ float otherDepth = data_[otherIdx];
+
+ // ignore invalid points
+ if (otherDepth == 0)
+ continue;
+ if (fabs(otherDepth - depth) > 200)
+ continue;
+
+ ++counter;
+ average_depth += otherDepth;
+ }
+ }
+
+ return clampToDisparityBounds (average_depth / counter, raw_data_[idx]);
+ }
+ };
+
+
+ // This version requires a pre-computed map of float3 (nr_valid_points, min_allowable_depth, max_allowable_depth);
+ struct DisparityClampedSmoothing
+ {
+ DisparityClampedSmoothing (float* data, float3* disparity_helper_map, int width, int height, int window_size)
+ : data_(data), disparity_helper_map_(disparity_helper_map), width_(width), height_(height), window_size_(window_size)
+ {}
+
+ float* data_;
+ float3* disparity_helper_map_;
+ int width_;
+ int height_;
+ int window_size_;
+
+ template <typename Tuple>
+ inline __host__ __device__
+ float operator () (Tuple t)
+ {
+ float depth = thrust::get<0> (t);
+ int idx = thrust::get<1> (t);
+ float3 dhel = disparity_helper_map_[idx];
+ int nr = (int) dhel.x;
+ float min_d = dhel.y;
+ float max_d = dhel.z;
+#ifdef __CUDACC__
+ if (depth == 0 | isnan(depth) | isinf(depth))
+ return 0.0f;
+#else
+ if (depth == 0 | pcl_isnan(depth) | pcl_isinf(depth))
+ return 0.0f;
+#endif
+ int xIdx = idx % width_;
+ int yIdx = idx / width_;
+
+ // This implements a fixed window size in image coordinates (pixels)
+ int4 bounds = make_int4 (
+ xIdx - window_size_,
+ xIdx + window_size_,
+ yIdx - window_size_,
+ yIdx + window_size_
+ );
+
+ // clamp the coordinates to fit to disparity image size
+ bounds.x = clamp (bounds.x, 0, width_-1);
+ bounds.y = clamp (bounds.y, 0, width_-1);
+ bounds.z = clamp (bounds.z, 0, height_-1);
+ bounds.w = clamp (bounds.w, 0, height_-1);
+
+ // iterate over all pixels in the rectangular region
+ for (int y = bounds.z; y <= bounds.w; ++y)
+ {
+ for (int x = bounds.x; x <= bounds.y; ++x)
+ {
+ // find index in point cloud from x,y pixel positions
+ int otherIdx = ((int)y) * width_ + ((int)x);
+ depth += data_[otherIdx];
+ }
+ }
+
+ return clamp (depth / nr, min_d, max_d);
+ }
+ };
+
+ struct DisparityHelperMap
+ {
+ DisparityHelperMap (float* data, int width, int height, int window_size, float baseline, float focal_length, float disp_thresh)
+ : data_(data), width_(width), height_(height), window_size_(window_size), baseline_(baseline), focal_length_(focal_length), disp_thresh_(disp_thresh)
+ {}
+
+ float* data_;
+ int width_;
+ int height_;
+ int window_size_;
+ float baseline_;
+ float focal_length_;
+ float disp_thresh_;
+
+ // helper function
+ inline __host__ __device__
+ float disparity2depth (float disparity)
+ {
+ return baseline_ * focal_length_ / disparity;
+ }
+
+ // helper function
+ inline __host__ __device__
+ float depth2disparity (float depth)
+ {
+ return baseline_ * focal_length_ / depth;
+ }
+
+ inline __host__ __device__
+ float3 operator () (int idx)
+ {
+ float disparity = depth2disparity (data_ [idx]);
+#ifdef __CUDACC__
+ if (disparity == 0 | isnan(disparity) | isinf(disparity))
+ return make_float3 (0,0,0);
+#else
+ if (disparity == 0 | pcl_isnan(disparity) | pcl_isinf(disparity))
+ return make_float3 (0,0,0);
+#endif
+ int xIdx = idx % width_;
+ int yIdx = idx / width_;
+
+ // This implements a fixed window size in image coordinates (pixels)
+ int4 bounds = make_int4 (
+ xIdx - window_size_,
+ xIdx + window_size_,
+ yIdx - window_size_,
+ yIdx + window_size_
+ );
+
+ // clamp the coordinates to fit to disparity image size
+ bounds.x = clamp (bounds.x, 0, width_-1);
+ bounds.y = clamp (bounds.y, 0, width_-1);
+ bounds.z = clamp (bounds.z, 0, height_-1);
+ bounds.w = clamp (bounds.w, 0, height_-1);
+
+ int counter = 1;
+ // iterate over all pixels in the rectangular region
+ for (int y = bounds.z; y <= bounds.w; ++y)
+ {
+ for (int x = bounds.x; x <= bounds.y; ++x)
+ {
+ // find index in point cloud from x,y pixel positions
+ int otherIdx = ((int)y) * width_ + ((int)x);
+ float otherDepth = data_[otherIdx];
+
+ // ignore invalid points
+ if (otherDepth > 0)
+ ++counter;
+ }
+ }
+
+ return make_float3 ((float) counter,
+ disparity2depth (disparity + disp_thresh_),
+ disparity2depth (disparity - disp_thresh_));
+ }
+ };
+
+
+ } // namespace
+} // namespace
+
+#endif
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_CUDA_PREDICATE_H_
+#define PCL_CUDA_PREDICATE_H_
+
+//#if THRUST_DEVICE_COMPILER == THRUST_DEVICE_COMPILER_NVCC
+//#undef __MMX__
+#include <pcl/cuda/point_cloud.h>
+#include <pcl/cuda/point_types.h>
+//#else
+//#endif
+
+
+namespace pcl
+{
+namespace cuda
+{
+ template <class T>
+ struct isNotZero
+ {
+ __inline__ __host__ __device__ bool
+ operator()(T x) { return (x != 0); }
+ };
+
+ struct isInlier
+ {
+ __inline__ __host__ __device__ bool
+ operator()(int x) { return (x != -1); }
+ };
+
+ struct isNotInlier
+ {
+ __inline__ __host__ __device__ bool
+ operator()(int x) { return (x == -1); }
+ };
+
+ struct SetColor
+ {
+ SetColor (const OpenNIRGB& color) : color_(color) {}
+ __inline__ __host__ __device__ void
+ operator()(PointXYZRGB& point) { point.rgb.r = color_.r; point.rgb.g = color_.g; point.rgb.b = color_.b;}
+ OpenNIRGB color_;
+ };
+
+ struct ChangeColor
+ {
+ ChangeColor (const OpenNIRGB& color) : color_(color) {}
+ __inline__ __host__ __device__ PointXYZRGB&
+ operator()(PointXYZRGB& point)
+ {
+ point.rgb.r = color_.r; point.rgb.g = color_.g; point.rgb.b = color_.b;
+ return point;
+ }
+ OpenNIRGB color_;
+ };
+
+} // namespace
+} // namespace
+
+#endif //#ifndef PCL_CUDA_PREDICATE_H_
+
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include "pcl/cuda/io/cloud_to_pcl.h"
+
+
+namespace pcl
+{
+namespace cuda
+{
+
+void
+fromPCL (const pcl::PointCloud<pcl::PointXYZRGB> &input, PointCloudAOS<Host> &output)
+{
+// output.points.resize (input.points.size());
+// for (size_t i = 0; i < input.points.size (); ++i)
+// {
+// output.points[i].x = input.points[i].x;
+// output.points[i].y = input.points[i].y;
+// output.points[i].z = input.points[i].z;
+// // Pack RGB into a float
+// output.points[i].rgb = *(float*)(&input.points[i].rgb);
+// }
+// thrust::copy (output.points.begin(), output.points.end (), input.points.begin());
+// output.width = input.width;
+// output.height = input.height;
+// output.is_dense = input.is_dense;
+}
+
+void
+fromPCL (const pcl::PointCloud<pcl::PointXYZRGB> &input, PointCloudAOS<Device> &output)
+{
+// PointCloudAOS<Host> output_host;
+// fromPCL (input, output_host);
+// output << output_host;
+}
+
+} // namespace
+} // namespace
+
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/cuda/io/cloud_to_pcl.h>
+
+namespace pcl
+{
+namespace cuda
+{
+
+//////////////////////////////////////////////////////////////////////////
+void
+toPCL (const PointCloudAOS<Host> &input,
+ const thrust::host_vector<float4> &normals,
+ pcl::PointCloud<pcl::PointXYZRGBNormal> &output)
+{
+ output.points.resize (input.points.size ());
+ for (size_t i = 0; i < input.points.size (); ++i)
+ {
+ output.points[i].x = input.points[i].x;
+ output.points[i].y = input.points[i].y;
+ output.points[i].z = input.points[i].z;
+ // Pack RGB into a float
+ output.points[i].rgb = *(float*)(&input.points[i].rgb);
+
+ output.points[i].normal_x = normals[i].x;
+ output.points[i].normal_y = normals[i].y;
+ output.points[i].normal_z = normals[i].z;
+ output.points[i].curvature = normals[i].w;
+ }
+
+ output.width = input.width;
+ output.height = input.height;
+ output.is_dense = input.is_dense;
+}
+
+//////////////////////////////////////////////////////////////////////////
+void
+toPCL (const PointCloudAOS<Device> &d_input,
+ const thrust::device_vector<float4> &d_normals,
+ pcl::PointCloud<pcl::PointXYZRGBNormal> &output)
+{
+ PointCloudAOS<Host> input;
+ input << d_input;
+ thrust::host_vector<float4> normals = d_normals;
+
+ output.points.resize (input.points.size ());
+ for (size_t i = 0; i < input.points.size (); ++i)
+ {
+ output.points[i].x = input.points[i].x;
+ output.points[i].y = input.points[i].y;
+ output.points[i].z = input.points[i].z;
+ // Pack RGB into a float
+ output.points[i].rgb = *(float*)(&input.points[i].rgb);
+
+ output.points[i].normal_x = normals[i].x;
+ output.points[i].normal_y = normals[i].y;
+ output.points[i].normal_z = normals[i].z;
+ output.points[i].curvature = normals[i].w;
+ }
+ output.width = input.width;
+ output.height = input.height;
+ output.is_dense = input.is_dense;
+}
+
+//////////////////////////////////////////////////////////////////////////
+void
+toPCL (const PointCloudAOS<Host> &input,
+ pcl::PointCloud<pcl::PointXYZRGB> &output)
+{
+ output.points.resize (input.points.size ());
+ for (size_t i = 0; i < input.points.size (); ++i)
+ {
+ output.points[i].x = input.points[i].x;
+ output.points[i].y = input.points[i].y;
+ output.points[i].z = input.points[i].z;
+ // Pack RGB into a float
+ output.points[i].rgb = *(float*)(&input.points[i].rgb);
+ }
+
+ output.width = input.width;
+ output.height = input.height;
+ output.is_dense = input.is_dense;
+
+/* for (size_t i = 0; i < output.points.size (); ++i)
+ std::cerr <<
+ output.points[i].x << " " <<
+ output.points[i].y << " " <<
+ output.points[i].z << " " << std::endl;*/
+}
+
+//////////////////////////////////////////////////////////////////////////
+void
+toPCL (const PointCloudAOS<Device> &input,
+ pcl::PointCloud<pcl::PointXYZRGB> &output)
+{
+ PointCloudAOS<Host> cloud;
+ cloud << input;
+
+ output.points.resize (cloud.points.size ());
+ for (size_t i = 0; i < cloud.points.size (); ++i)
+ {
+ output.points[i].x = cloud.points[i].x;
+ output.points[i].y = cloud.points[i].y;
+ output.points[i].z = cloud.points[i].z;
+ // Pack RGB into a float
+ output.points[i].rgb = *(float*)(&cloud.points[i].rgb);
+ }
+
+ output.width = cloud.width;
+ output.height = cloud.height;
+ output.is_dense = cloud.is_dense;
+
+/* for (size_t i = 0; i < output.points.size (); ++i)
+ std::cerr <<
+ output.points[i].x << " " <<
+ output.points[i].y << " " <<
+ output.points[i].z << " " << std::endl;*/
+}
+
+} // namespace
+} // namespace
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <pcl/pcl_exports.h>
+
+#include <pcl/cuda/io/debayering.h>
+#include <iostream>
+#include <pcl/cuda/time_cpu.h>
+#include <pcl/cuda/thrust.h>
+
+#define ABS(x) ((x)>0)?(x):(-(x))
+namespace pcl
+{
+ namespace cuda
+ {
+ template <template <typename> class Storage>
+ YUV2RGBKernel<Storage>::YUV2RGBKernel
+ (unsigned char *yuv_image, unsigned width, unsigned height)
+ : width(width), height(height), data(yuv_image)
+ {
+ }
+
+ template <template <typename> class Storage>
+ OpenNIRGB YUV2RGBKernel<Storage>::operator () (int index) const
+ {
+ //TODO: assert width/height is modulo 2
+
+ int yuv_index = index * 2; // actual index in byte array
+
+ int u, v;
+ if (index % 2)
+ {
+ u = data[yuv_index + 0] - 128; // current pixel has u value
+ v = data[yuv_index + 2] - 128; // get v from next pixel
+ }
+ else
+ {
+ u = data[yuv_index - 2] - 128; // get u from last pixel
+ v = data[yuv_index + 0] - 128; // current pixel has v value
+ }
+
+ int y = data[yuv_index + 1];
+
+#define CLIP_CHAR(c) ((c)>255?255:(c)<0?0:(c))
+ OpenNIRGB result;
+ result.b = CLIP_CHAR (y + ((v * 18678 + 8192 ) >> 14));
+ result.g = CLIP_CHAR (y + ((v * -9519 - u * 6472 + 8192 ) >> 14));
+ result.r = CLIP_CHAR (y + ((u * 33292 + 8192 ) >> 14));
+
+ return result;
+ }
+
+ template <template <typename> class Storage>
+ DebayerBilinear<Storage>::DebayerBilinear
+ (unsigned char *bayer_image, unsigned width, unsigned height)
+ : width(width), height(height), data(bayer_image)
+ {/*
+ if (dataSize < bayer_image->getWidth () * bayer_image->getHeight ())
+ {
+ if (!global_data)
+ cudaFree (global_data);
+ dataSize = bayer_image->getWidth () * bayer_image->getHeight ();
+ cudaError_t status = cudaMalloc( &data, dataSize );
+ if (status != cudaSuccess ) std::cout << "malloc failed!" << std::endl;
+
+ data = global_data;
+ }
+ */
+
+ /* thrust::device_vector<unsigned char>* data;
+ cudaError_t status = cudaMemcpy(data, (void*)(bayer_image->getMetaData().Data()), bayer_image->getMetaData().DataSize(), cudaMemcpyHostToDevice);
+ if (status != cudaSuccess )
+ {
+ std::cout << "memcpy failed! : " << status << std::endl;
+ //std::cout << cudaErrorInvalidValue << " , " << cudaErrorInvalidDevicePointer << " , " << cudaErrorInvalidMemcpyDirection << std::endl;
+ }*/
+ }
+
+ template <template <typename> class Storage>
+ OpenNIRGB DebayerBilinear<Storage>::operator () (int index) const
+ {
+ // get position
+ int xIdx = index % width;
+ int yIdx = index / width;
+
+ OpenNIRGB result;
+
+ // result.r = result.g = result.b = 0;
+ // return result;
+
+ // ToDo: need to be handled later on!
+ if (xIdx == 0 || xIdx == width-1 || yIdx == 0 || yIdx == height-1)
+ {
+ result.r = result.g = result.b = 0;
+ return result;
+ }
+
+ if (yIdx & 1) // We are in the Blue line
+ {
+ if (xIdx & 1) // Odd -> We start with GBGBGBGBG
+ {
+ result.r = (unsigned char) ((data [index - width] + data [index + width]) >> 1);
+ result.g = data [index];
+ result.b = (data [index - 1] + data [index + 1]) >> 1;
+ }
+ else // BGBGBGBG
+ {
+ result.r = (data [index - width - 1] + data [index - width + 1] + data [index + width - 1] + data [index + width + 1] ) >> 2;
+ result.g = (data [index - 1] + data [index + 1] + data [index - width] + data [index + width]) >> 2;
+ result.b = data [index];
+ }
+ }
+ else // We are in the Red line
+ {
+ if (xIdx & 1) // Odd -> We start with RGRGRG
+ {
+ result.r = data [index];
+ result.g = (data [index - 1] + data [index + 1] + data [index - width] + data [index + width]) >> 2;
+ result.b = (data [index - width - 1] + data [index - width + 1] + data [index + width - 1] + data [index + width + 1] ) >> 2;
+ }
+ else // GRGRGR
+ {
+ result.r = (data [index - 1] + data [index + 1]) >> 1;
+ result.g = data [index];
+ result.b = (data [index - width] + data [index + width]) >> 1;
+ }
+ }
+
+ return result;
+ }
+
+ /*unsigned char* DebayerEdgeAware::global_data = 0;
+ unsigned DebayerEdgeAware::dataSize = 0;
+
+ DebayerEdgeAware::DebayerEdgeAware (const boost::shared_ptr<openni_wrapper::Image>& bayer_image)
+ {
+ if (dataSize < bayer_image->getWidth () * bayer_image->getHeight ())
+ {
+ if (!global_data)
+ cudaFree (global_data);
+ dataSize = bayer_image->getWidth () * bayer_image->getHeight ();
+ cudaError_t status = cudaMalloc( &data, dataSize );
+ if (status != cudaSuccess ) std::cout << "malloc failed!" << std::endl;
+
+ data = global_data;
+ }
+
+ cudaError_t status = cudaMemcpy(data, (void*)(bayer_image->getMetaData().Data()), bayer_image->getMetaData().DataSize(), cudaMemcpyHostToDevice);
+ if (status != cudaSuccess )
+ {
+ std::cout << "memcpy failed! : " << status << std::endl;
+ //std::cout << cudaErrorInvalidValue << " , " << cudaErrorInvalidDevicePointer << " , " << cudaErrorInvalidMemcpyDirection << std::endl;
+ }
+ }
+
+ DebayerEdgeAware::~DebayerEdgeAware ()
+ {
+ //cudaFree (data);
+ }
+
+ OpenNIRGB DebayerEdgeAware::operator () (int index) const
+ {
+ // get position
+ int xIdx = index % width;
+ int yIdx = index / width;
+
+ OpenNIRGB result;
+
+ // ToDo: need to be handled later on!
+ if (xIdx == 0 || xIdx == width-1 || yIdx == 0 || yIdx == height-1)
+ {
+ result.r = result.g = result.b = 0;
+ return result;
+ }
+
+ if (yIdx & 1) // We are in the Blue line
+ {
+ if (xIdx & 1) // Odd -> We start with GBGBGBGBG
+ {
+ result.r = (data [index - width] + data [index + width]) >> 1;
+ result.g = data [index];
+ result.b = (data [index - 1] + data [index + 1]) >> 1;
+ }
+ else // BGBGBGBG
+ {
+ result.r = (data [index - width - 1] + data [index - width + 1] + data [index + width - 1] + data [index + width + 1] ) >> 2;
+
+ if ( ABS(data [index - 1] - data [index + 1]) > ABS(data [index - width] - data [index + width] ) )
+ result.g = (data [index - width] + data [index + width]) >> 1;
+ else
+ result.g = (data [index - 1] + data [index + 1]) >> 1;
+ result.b = data [index];
+ }
+ }
+ else // We are in the Red line
+ {
+ if (xIdx & 1) // Odd -> We start with RGRGRG
+ {
+ result.r = data [index];
+ if ( ABS(data [index - 1] - data [index + 1]) > ABS(data [index - width] - data [index + width]) )
+ result.g = (data [index - width] + data [index + width]) >> 1;
+ else
+ result.g = (data [index - 1] + data [index + 1]) >> 1;
+ result.b = (data [index - width - 1] + data [index - width + 1] + data [index + width - 1] + data [index + width + 1] ) >> 2;
+ }
+ else // GRGRGR
+ {
+ result.r = (data [index - 1] + data [index + 1]) >> 1;
+ result.g = data [index];
+ result.b = (data [index - width] + data [index + width]) >> 1;
+ }
+ }
+
+ return result;
+ }*/
+
+ template<template <typename> class Storage>
+ void Debayering<Storage>::computeBilinear (const boost::shared_ptr<openni_wrapper::Image>& bayer_image, RGBImageType& rgb_image) const
+ {
+ //pcl::ScopeTime t ("computeBilinear");
+ typename Storage<unsigned char>::type bayer_data (bayer_image->getWidth () * bayer_image->getHeight ());
+ // thrust::device_vector<unsigned char> bayer_data (bayer_image->getWidth () * bayer_image->getHeight ());
+ thrust::copy ((unsigned char*)(bayer_image->getMetaData().Data()),
+ (unsigned char*)(bayer_image->getMetaData().Data() + bayer_image->getMetaData().DataSize()),
+ bayer_data.begin ());
+ // thrust::device_vector<int> indices(bayer_image->getWidth () * bayer_image->getHeight ());
+ // thrust::sequence (indices.begin(), indices.end() );
+ // thrust::transform (indices.begin(),
+ // indices.end(),
+ // rgb_image.begin(),
+ // DebayerBilinear<Device> (bayer_data, bayer_image->getWidth (), bayer_image->getHeight ()));
+ thrust::counting_iterator<int> first (0);
+ thrust::transform (first,
+ first + (int)(bayer_image->getWidth () * bayer_image->getHeight ()),
+ rgb_image.begin(),
+ DebayerBilinear<Storage> (thrust::raw_pointer_cast (&bayer_data[0]), bayer_image->getWidth (), bayer_image->getHeight ()) );
+ }
+
+ template<template <typename> class Storage>
+ void YUV2RGB<Storage>::compute (const boost::shared_ptr<openni_wrapper::Image>& yuv_image, RGBImageType& rgb_image) const
+ {
+ typename Storage<unsigned char>::type yuv_data (yuv_image->getMetaData().DataSize());
+ thrust::copy ((unsigned char*)(yuv_image->getMetaData().Data()),
+ (unsigned char*)(yuv_image->getMetaData().Data() + yuv_image->getMetaData().DataSize()),
+ yuv_data.begin ());
+ thrust::counting_iterator<int> first (0);
+ thrust::transform (first,
+ first + (int)(yuv_image->getWidth () * yuv_image->getHeight ()),
+ rgb_image.begin(),
+ YUV2RGBKernel<Storage> (thrust::raw_pointer_cast (&yuv_data[0]), yuv_image->getWidth (), yuv_image->getHeight ()) );
+ }
+
+ template <template <typename> class Storage>
+ struct DebayerDownsample
+ {
+ DebayerDownsample (unsigned char *bayer_image, unsigned width, unsigned stride)
+ : width(width), stride(stride), data(bayer_image) {}
+
+ int width, height, stride;
+ unsigned char *data;
+
+ __host__ __device__
+ OpenNIRGB operator() (int i)
+ {
+ OpenNIRGB result;
+ result.r = data [i+1];
+ result.g = (data [i] + data[i + width + 1]) / 2;
+ result.b = data [i + width];
+ return result;
+ }
+
+ };
+
+
+ template<template <typename> class Storage>
+ void DebayeringDownsampling<Storage>::compute (const boost::shared_ptr<openni_wrapper::Image>& bayer_image, RGBImageType& rgb_image) const
+ {
+ //pcl::ScopeTime t ("computeBilinear");
+ typename Storage<unsigned char>::type bayer_data (bayer_image->getWidth () * bayer_image->getHeight ());
+ // thrust::device_vector<unsigned char> bayer_data (bayer_image->getWidth () * bayer_image->getHeight ());
+ thrust::copy ((unsigned char*)(bayer_image->getMetaData().Data()),
+ (unsigned char*)(bayer_image->getMetaData().Data() + bayer_image->getMetaData().DataSize()),
+ bayer_data.begin ());
+
+ thrust::counting_iterator<int> first (0);
+ typename Storage<int>::type downsampled_indices;
+ downsampled_indices.resize ((bayer_image->getWidth()/2) * (bayer_image->getHeight()/2));
+ thrust::copy_if (first,
+ first + (int)(bayer_image->getWidth () * bayer_image->getHeight ()),
+ downsampled_indices.begin(),
+ downsampleIndices(bayer_image->getWidth (), bayer_image->getHeight (), 2));
+
+ thrust::transform (downsampled_indices.begin (),
+ downsampled_indices.end (),
+ rgb_image.begin(),
+ DebayerDownsample<Storage> (thrust::raw_pointer_cast (&bayer_data[0]), bayer_image->getWidth (), 2) );
+ }
+ /*
+ void Debayering::computeEdgeAware (const boost::shared_ptr<openni_wrapper::Image>& bayer_image, thrust::host_vector<OpenNIRGB>& rgb_image) const
+ {
+ thrust::device_vector<OpenNIRGB> image (bayer_image->getWidth () * bayer_image->getHeight ()) ;
+ computeEdgeAware (bayer_image, image );
+ rgb_image = image;
+ }
+
+ void Debayering::computeEdgeAware (const boost::shared_ptr<openni_wrapper::Image>& bayer_image, thrust::device_vector<OpenNIRGB>& rgb_image) const
+ {
+ //pcl::ScopeTime t ("computeEdgeAware");
+ thrust::device_vector<int> indices(bayer_image->getWidth () * bayer_image->getHeight ());
+ thrust::sequence( indices.begin(), indices.end() );
+ thrust::transform ( indices.begin(), indices.end(), rgb_image.begin(), DebayerEdgeAware (bayer_image) );
+ }*/
+
+ template class PCL_EXPORTS YUV2RGB<Device>;
+ template class PCL_EXPORTS YUV2RGB<Host>;
+ template class PCL_EXPORTS Debayering<Device>;
+ template class PCL_EXPORTS Debayering<Host>;
+ template class PCL_EXPORTS DebayeringDownsampling<Device>;
+ template class PCL_EXPORTS DebayeringDownsampling<Host>;
+
+ } // namespace
+} // namespace
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include "pcl/cuda/io/disparity_to_cloud.h"
+#include "pcl/cuda/io/debayering.h"
+#include "pcl/cuda/io/cloud_to_pcl.h"
+#include "pcl/cuda/io/kinect_smoothing.h"
+
+#include <pcl/io/openni_camera/openni_image.h>
+#include <pcl/io/openni_camera/openni_depth_image.h>
+
+#include <thrust/iterator/constant_iterator.h>
+#include <thrust/copy.h>
+#include <string>
+#include <iostream>
+#include <fstream>
+
+namespace pcl
+{
+namespace cuda
+{
+
+
+//////////////////////////////////////////////////////////////////////////
+template <typename Tuple> PointXYZRGB
+ComputeXYZ::operator () (const Tuple &t)
+{
+ PointXYZRGB pt;
+
+ if (thrust::get<0>(t) == 0) // assume no_sample_value and shadow_value are also 0
+ pt.x = pt.y = pt.z = bad_point;
+ else
+ {
+ // Fill in XYZ (and copy NaNs with it)
+ pt.x = ((thrust::get<1>(t) % width) - center_x) * thrust::get<0>(t) * constant * 0.001f;
+ pt.y = ((thrust::get<1>(t) / width) - center_y) * thrust::get<0>(t) * constant * 0.001f;
+ pt.z = thrust::get<0>(t) * 0.001f;
+ }
+ return (pt);
+}
+
+//////////////////////////////////////////////////////////////////////////
+template <typename Tuple> PointXYZRGB
+ComputeXYZRGB::operator () (const Tuple &t)
+{
+ PointXYZRGB pt;
+
+ if (thrust::get<0>(t) == 0) // assume no_sample_value and shadow_value are also 0
+ pt.x = pt.y = pt.z = bad_point;
+ else
+ {
+ // Fill in XYZ (and copy NaNs with it)
+ pt.x = ((thrust::get<2>(t) % width) - center_x) * thrust::get<0>(t) * constant * 0.001f;
+ pt.y = ((thrust::get<2>(t) / width) - center_y) * thrust::get<0>(t) * constant * 0.001f;
+ pt.z = thrust::get<0>(t) * 0.001f;
+ }
+ pt.rgb = thrust::get<1>(t).r << 16 | thrust::get<1>(t).g << 8 | thrust::get<1>(t).b;
+ return (pt);
+}
+
+//////////////////////////////////////////////////////////////////////////
+//void
+//DisparityToCloud::compute (const pcl::PCLImage::ConstPtr &depth_image,
+// const pcl::PCLImage::ConstPtr &rgb_image,
+// const pcl::CameraInfo::ConstPtr &info,
+// PointCloudAOS<Device>::Ptr &output)
+//{
+// if (!output)
+// output.reset (new PointCloudAOS<Device>);
+//
+// using namespace thrust;
+//
+// // Prepare the output
+// output->height = depth_image->height;
+// output->width = depth_image->width;
+// output->is_dense = false;
+// output->points.resize (output->width * output->height);
+//
+// // Copy the depth data and the RGB data on the card
+// device_vector<float> depth (depth_image->data.size () / sizeof (float));
+// thrust::copy ((float*)(&depth_image->data[0]), (float*)(&depth_image->data[0]) + depth.size (), depth.begin ());
+//
+// // Prepare constants
+// if (rgb_image)
+// {
+// device_vector<OpenNIRGB> rgb (rgb_image->width * rgb_image->height);
+// thrust::copy ((OpenNIRGB*)(&rgb_image->data[0]),
+// (OpenNIRGB*)(&rgb_image->data[rgb_image->width * rgb_image->height * 3]), rgb.begin ());
+//
+// // Suat: implement the rgb/depth stepping
+// assert (rgb.size () == depth.size ());
+//
+// // Send the data to the device
+// transform (
+// make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), counting_iterator<int>(0))),
+// make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), counting_iterator<int>(0))) +
+// depth_image->width * depth_image->height,
+// output->points.begin (),
+// ComputeXYZRGB (depth_image->width, depth_image->height,
+// depth_image->width >> 1, depth_image->height >> 1, 1.0 / info->P[0]));
+// }
+// else
+// {
+// // Send the data to the device
+// transform (
+// make_zip_iterator (make_tuple (depth.begin (), counting_iterator<int>(0))),
+// make_zip_iterator (make_tuple (depth.begin (), counting_iterator<int>(0))) +
+// depth_image->width * depth_image->height,
+// output->points.begin (),
+// ComputeXYZ (depth_image->width, depth_image->height,
+// depth_image->width >> 1, depth_image->height >> 1, 1.0 / info->P[0]));
+// }
+//
+//}
+//
+////////////////////////////////////////////////////////////////////////////
+//void
+//DisparityToCloud::compute (const pcl::PCLImage::ConstPtr &depth_image,
+// const pcl::PCLImage::ConstPtr &rgb_image,
+// const pcl::CameraInfo::ConstPtr &info,
+// PointCloudAOS<Host>::Ptr &output)
+//{
+// if (!output)
+// output.reset (new PointCloudAOS<Host>);
+//
+// PointCloudAOS<Device>::Ptr data;
+// compute (depth_image, rgb_image, info, data);
+// *output << *data;
+//}
+
+ template <typename T>
+ void save_pgm (const T* depth, std::string filename, int w, int h)
+ {
+ std::ofstream ofs (filename.c_str());
+ ofs << "P2" << std::endl << w << " " << h << std::endl;
+ T max_el = 0;
+ for (int depthIdx = 0; depthIdx < h * w; ++depthIdx)
+ if (depth[depthIdx] > max_el)
+ max_el = depth[depthIdx];
+ ofs << (int)max_el << std::endl;
+ for (int yIdx = 0; yIdx < h; ++yIdx)
+ {
+ for (int xIdx = 0; xIdx < w; ++xIdx)
+ {
+ ofs << (int)depth[w*yIdx+xIdx] << " ";
+ }
+ ofs << std::endl;
+ }
+ }
+
+//////////////////////////////////////////////////////////////////////////
+template <template <typename> class Storage> void
+DisparityToCloud::compute (const boost::uint16_t* depth_image,
+ const OpenNIRGB* rgb_image,
+ int width, int height,
+ float constant,
+ typename PointCloudAOS<Storage>::Ptr &output,
+ int smoothing_nr_iterations, int smoothing_filter_size)
+{
+ using namespace thrust;
+ if (!output)
+ output.reset (new PointCloudAOS<Storage>);
+
+ int output_size = width * height;
+ float baseline = 0.075f;
+ float disp_thresh = 0.001f/8.0f;
+
+ output->height = height;
+ output->width = width;
+
+ output->is_dense = false;
+ output->points.resize (output_size);
+
+ // copy to float
+ typename Storage<float>::type depth (output_size);
+ copy (depth_image, (const unsigned short*)(&depth_image[output_size]), depth.begin ());
+
+ typename Storage<OpenNIRGB>::type rgb (output_size);
+ copy (rgb_image, (const OpenNIRGB*)(&rgb_image[output_size]), rgb.begin());
+
+ if (smoothing_nr_iterations > 0)
+ {
+ typename Storage<float3>::type disp_helper_map (output_size);
+
+ float* depth_ptr = raw_pointer_cast(&depth[0]);
+
+ transform (counting_iterator<int>(0),
+ counting_iterator<int>(0) + output_size,
+ disp_helper_map.begin (),
+ DisparityHelperMap (depth_ptr, width, height, smoothing_filter_size, baseline, 1.0f/constant, disp_thresh));
+
+ for (int iter = 0; iter < smoothing_nr_iterations; iter++)
+ {
+ transform (
+ make_zip_iterator (make_tuple (depth.begin (), counting_iterator<int>(0))),
+ make_zip_iterator (make_tuple (depth.begin (), counting_iterator<int>(0))) + output_size,
+ depth.begin (), DisparityClampedSmoothing (raw_pointer_cast(&depth[0]), raw_pointer_cast(&disp_helper_map[0]), width, height, smoothing_filter_size));
+ }
+
+ // Send the data to the device
+ transform (
+ make_zip_iterator (make_tuple (depth.begin (), rgb.begin(), counting_iterator<int>(0))),
+ make_zip_iterator (make_tuple (depth.begin (), rgb.begin(), counting_iterator<int>(0))) + output_size,
+ output->points.begin (),
+ ComputeXYZRGB (width, height,
+ width >> 1, height >> 1, constant));
+ }
+ else
+ {
+ transform (
+ make_zip_iterator (make_tuple (depth.begin(), rgb.begin(), counting_iterator<int>(0))),
+ make_zip_iterator (make_tuple (depth.begin(), rgb.begin(), counting_iterator<int>(0))) + output_size,
+ output->points.begin (),
+ ComputeXYZRGB (width, height,
+ width >> 1, height >> 1, constant));
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////
+template <template <typename> class Storage> void
+DisparityToCloud::compute (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ const boost::shared_ptr<openni_wrapper::Image>& rgb_image,
+ float constant,
+ typename PointCloudAOS<Storage>::Ptr &output,
+ bool downsample, int stride, int smoothing_nr_iterations, int smoothing_filter_size)
+{
+ if (!output)
+ output.reset (new PointCloudAOS<Storage>);
+
+ using namespace thrust;
+
+ int depth_width = depth_image->getWidth ();
+ int depth_height = depth_image->getHeight ();
+
+ // Prepare the output
+ if (downsample)
+ {
+ output->height = depth_height / stride;
+ output->width = depth_width / stride;
+ constant *= stride;
+ }
+ else
+ {
+ output->height = depth_height;
+ output->width = depth_width;
+ }
+
+ output->is_dense = false;
+ output->points.resize (output->width * output->height);
+
+ // Copy the depth data and the RGB data on the card
+ typename Storage<float>::type depth (output->width * output->height);
+ unsigned short* depth_buffer = (unsigned short*)depth_image->getDepthMetaData ().Data ();
+
+ if (downsample)
+ {
+ typename Storage<float>::type depth_device (depth_width * depth_height);
+ // first, copy from host to Storage
+ thrust::copy (depth_buffer, (unsigned short*)(&depth_buffer[depth_width * depth_height]), depth_device.begin ());
+
+ thrust::counting_iterator<int> counter (0);
+ //typename Storage<int>::type downsampled_indices;
+ //downsampled_indices.resize ((output->width/2) * (output->height/2));
+ //thrust::copy_if (counting_iterator<int>(0), counting_iterator<int>(0)+depth_width*depth_height, downsampled_indices.begin (), downsampleIndices (output->width, output->height, 2));
+ thrust::copy_if (depth_device.begin (),
+ depth_device.end (),
+ //thrust::make_constant_iterator (12), thrust::make_constant_iterator (12) + depth_width * depth_height,
+ counter,
+ depth.begin (),
+ downsampleIndices (depth_width, depth_height, stride));
+ //host_vector<float> h_depth = depth;
+ //save_pgm<float> ((float*)&(h_depth[0]), "outpoutbla", output->width, output->height);
+ }
+ else
+ thrust::copy (depth_buffer, (unsigned short*)(&depth_buffer[output->width * output->height]), depth.begin ());
+
+ // Prepare constants
+ if (rgb_image)
+ {
+ int image_width = rgb_image->getWidth ();
+ int image_height = rgb_image->getHeight ();
+ if (downsample)
+ {
+ image_width /= stride;
+ image_height /= stride;
+ }
+
+ typename Storage<OpenNIRGB>::type rgb (image_width * image_height);
+
+
+ if (rgb_image->getEncoding () == openni_wrapper::Image::BAYER_GRBG)
+ {
+ if (downsample)
+ {
+ DebayeringDownsampling<Storage> debayering;
+ debayering.compute (rgb_image, rgb);
+ }
+ else
+ {
+ Debayering<Storage> debayering;
+ debayering.computeBilinear (rgb_image, rgb);
+ }
+ }
+ else if (rgb_image->getEncoding () == openni_wrapper::Image::YUV422)
+ {
+ YUV2RGB<Storage> yuv;
+ yuv.compute (rgb_image, rgb);
+// OpenNIRGB c;
+// c.r = c.g = c.b = (unsigned char) 128;
+// thrust::fill (rgb.begin (), rgb.end (), c);
+ }
+
+
+ int output_size = output->width * output->height;
+ float baseline = 0.075f;
+ float disp_thresh = 0.001f/8.0f;
+
+ if (smoothing_nr_iterations > 0)
+ {
+#if 1
+ typename Storage<float3>::type disp_helper_map (output_size);
+
+ transform (counting_iterator<int>(0),
+ counting_iterator<int>(0) + output_size,
+ disp_helper_map.begin (),
+ DisparityHelperMap (thrust::raw_pointer_cast(&depth[0]), output->width, output->height, smoothing_filter_size, baseline, 1.0f/constant, disp_thresh));
+
+ for (int iter = 0; iter < smoothing_nr_iterations; iter++)
+ {
+ transform (
+ make_zip_iterator (make_tuple (depth.begin (), counting_iterator<int>(0))),
+ make_zip_iterator (make_tuple (depth.begin (), counting_iterator<int>(0))) + output_size,
+ depth.begin (), DisparityClampedSmoothing (raw_pointer_cast(&depth[0]), raw_pointer_cast(&disp_helper_map[0]), output->width, output->height, smoothing_filter_size));
+ }
+
+ // Send the data to the device
+ transform (
+ make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), counting_iterator<int>(0))),
+ make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), counting_iterator<int>(0))) + output_size,
+ output->points.begin (),
+ ComputeXYZRGB (output->width, output->height,
+ output->width >> 1, output->height >> 1, constant));
+#else
+// typename Storage<float>::type smooth_depth1 (output_size);
+// typename Storage<float>::type smooth_depth2 (output_size);
+//
+// transform (counting_iterator<int>(0),
+// counting_iterator<int>(0) + output_size,
+// smooth_depth1.begin (),
+// DisparityBoundSmoothing (output->width, output->height, smoothing_filter_size, 1.0f/constant, baseline, disp_thresh, thrust::raw_pointer_cast<float>(&depth[0]), thrust::raw_pointer_cast<float>(&depth[0])));
+//
+// for (int iter = 0; iter < (smoothing_nr_iterations-1)/2; iter++)
+// {
+// transform (counting_iterator<int>(0),
+// counting_iterator<int>(0) + output_size,
+// smooth_depth2.begin (),
+// DisparityBoundSmoothing (output->width, output->height, smoothing_filter_size, 1.0f/constant, baseline, disp_thresh, thrust::raw_pointer_cast<float>(&smooth_depth1[0]), thrust::raw_pointer_cast<float>(&depth[0])));
+// transform (counting_iterator<int>(0),
+// counting_iterator<int>(0) + output_size,
+// smooth_depth1.begin (),
+// DisparityBoundSmoothing (output->width, output->height, smoothing_filter_size, 1.0f/constant, baseline, disp_thresh, thrust::raw_pointer_cast<float>(&smooth_depth2[0]), thrust::raw_pointer_cast<float>(&depth[0])));
+// }
+// // Suat: implement the rgb/depth stepping
+// //assert (rgb.size () == depth.size ());
+//
+// // Send the data to the device
+// transform (
+// make_zip_iterator (make_tuple (smooth_depth1.begin (), rgb.begin (), counting_iterator<int>(0))),
+// make_zip_iterator (make_tuple (smooth_depth1.begin (), rgb.begin (), counting_iterator<int>(0))) + output_size,
+// output->points.begin (),
+// ComputeXYZRGB (output->width, output->height,
+// output->width >> 1, output->height >> 1, constant));
+#endif
+ }
+ else
+ {
+ // Send the data to the device
+ transform (
+ make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), counting_iterator<int>(0))),
+ make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), counting_iterator<int>(0))) + output_size,
+ output->points.begin (),
+ ComputeXYZRGB (output->width, output->height,
+ output->width >> 1, output->height >> 1, constant));
+ }
+ }
+ else
+ {
+ // Send the data to the device
+ transform (
+ make_zip_iterator (make_tuple (depth.begin (), counting_iterator<int>(0))),
+ make_zip_iterator (make_tuple (depth.begin (), counting_iterator<int>(0))) +
+ output->width * output->height,
+ output->points.begin (),
+ ComputeXYZ (output->width, output->height,
+ output->width >> 1, output->height >> 1, constant));
+ }
+
+}
+
+//////////////////////////////////////////////////////////////////////////
+/*void
+DisparityToCloud::compute (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ const boost::shared_ptr<openni_wrapper::Image>& rgb_image,
+ float constant,
+ PointCloudAOS<Host>::Ptr &output)
+{
+ if (!output)
+ output.reset (new PointCloudAOS<Host>);
+
+ PointCloudAOS<Device>::Ptr data;
+ compute (depth_image, rgb_image, constant, data);
+ *output << *data;
+}*/
+
+//////////////////////////////////////////////////////////////////////////
+//void
+//DisparityToCloud::compute (const pcl::PCLImage::ConstPtr &depth_image,
+// const pcl::CameraInfo::ConstPtr &info,
+// PointCloudAOS<Device>::Ptr &output)
+//{
+// if (!output)
+// output.reset (new PointCloudAOS<Device>);
+//
+// compute (depth_image, pcl::PCLImage::ConstPtr(), info, output);
+//}
+//
+////////////////////////////////////////////////////////////////////////////
+//void
+//DisparityToCloud::compute (const pcl::PCLImage::ConstPtr &depth_image,
+// const pcl::CameraInfo::ConstPtr &info,
+// PointCloudAOS<Host>::Ptr &output)
+//{
+// if (!output)
+// output.reset (new PointCloudAOS<Host>);
+//
+// PointCloudAOS<Device>::Ptr data;
+// compute (depth_image, info, data);
+// *output << *data;
+//}
+
+//////////////////////////////////////////////////////////////////////////
+void
+DisparityToCloud::compute (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ float constant,
+ PointCloudAOS<Device>::Ptr &output)
+{
+ if (!output)
+ output.reset (new PointCloudAOS<Device>);
+
+ compute<Device> (depth_image, boost::shared_ptr<openni_wrapper::Image>(), constant, output, false);
+}
+
+//////////////////////////////////////////////////////////////////////////
+void
+DisparityToCloud::compute (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ float constant,
+ PointCloudAOS<Host>::Ptr &output)
+{
+ if (!output)
+ output.reset (new PointCloudAOS<Host>);
+
+ compute<Host> (depth_image, boost::shared_ptr<openni_wrapper::Image>(), constant, output, false);
+ //PointCloudAOS<Device>::Ptr data;
+ //compute (depth_image, constant, data);
+ //*output << *data;
+}
+
+template PCL_EXPORTS void
+DisparityToCloud::compute<Host> (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ const boost::shared_ptr<openni_wrapper::Image>& rgb_image,
+ float constant,
+ PointCloudAOS<Host>::Ptr &output,
+ bool downsample, int stride, int, int);
+template PCL_EXPORTS void
+DisparityToCloud::compute<Device> (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ const boost::shared_ptr<openni_wrapper::Image>& rgb_image,
+ float constant,
+ PointCloudAOS<Device>::Ptr &output,
+ bool downsample, int stridem, int, int);
+template PCL_EXPORTS void
+DisparityToCloud::compute<Host> (const boost::uint16_t* depth_image,
+ const OpenNIRGB* rgb_image,
+ int width, int height,
+ float constant,
+ typename PointCloudAOS<Host>::Ptr &output,
+ int smoothing_nr_iterations, int smoothing_filter_size);
+template PCL_EXPORTS void
+DisparityToCloud::compute<Device> (const boost::uint16_t* depth_image,
+ const OpenNIRGB* rgb_image,
+ int width, int height,
+ float constant,
+ typename PointCloudAOS<Device>::Ptr &output,
+ int smoothing_nr_iterations, int smoothing_filter_size);
+} // namespace
+} // namespace
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <pcl/pcl_exports.h>
+
+#include <pcl/cuda/point_cloud.h>
+//#include <pcl/cuda/io/extract_indices.h>
+#include <pcl/cuda/io/predicate.h>
+#include <pcl/cuda/thrust.h>
+
+namespace pcl
+{
+namespace cuda
+{
+
+template <template <typename> class Storage, class T>
+void extractMask (const typename PointCloudAOS<Storage>::Ptr &input,
+ T* mask,
+ typename PointCloudAOS<Storage>::Ptr &output)
+{
+ if (!output)
+ output.reset (new PointCloudAOS<Storage>);
+
+ output->points.resize (input->points.size ());
+
+ typename Storage<T>::type mask_device (input->points.size());
+ thrust::copy (mask, (T*)(&mask[input->points.size()]), mask_device.begin ());
+
+ typename PointCloudAOS<Storage>::iterator it = thrust::copy_if (input->points.begin (), input->points.end (), mask_device.begin (), output->points.begin (), isNotZero<T> ());
+ output->points.resize (it - output->points.begin ());
+
+ output->width = (unsigned int) output->points.size();
+ output->height = 1;
+ output->is_dense = false;
+}
+
+template <template <typename> class Storage, class DataT, class MaskT>
+void extractMask (const boost::shared_ptr<typename Storage<DataT>::type> &input,
+ MaskT* mask,
+ boost::shared_ptr<typename Storage<DataT>::type> &output)
+{
+ if (!output)
+ output.reset (new typename Storage<DataT>::type);
+ output->resize (input->size ());
+
+ typename Storage<MaskT>::type mask_device (input->size());
+ thrust::copy (mask, (MaskT*)(&mask[input->size()]), mask_device.begin ());
+
+ typename Storage<DataT>::type::iterator it =
+ thrust::copy_if (input->begin (), input->end (), mask_device.begin (), output->begin (), isNotZero<MaskT> ());
+ output->resize (it - output->begin ());
+}
+
+
+template <template <typename> class Storage>
+void extractIndices (const typename PointCloudAOS<Storage>::Ptr &input,
+ typename Storage<int>::type& indices,
+ typename PointCloudAOS<Storage>::Ptr &output)
+{
+ if (!output)
+ output.reset (new PointCloudAOS<Storage>);
+
+ output->points.resize (input->points.size ());
+
+ typename PointCloudAOS<Storage>::iterator it = thrust::copy_if (input->points.begin (), input->points.end (), indices.begin (), output->points.begin (), isInlier ());
+ output->points.resize (it - output->points.begin ());
+
+ output->width = (unsigned int) output->points.size();
+ output->height = 1;
+ output->is_dense = false;
+}
+
+template <template <typename> class Storage>
+void removeIndices (const typename PointCloudAOS<Storage>::Ptr &input,
+ typename Storage<int>::type& indices,
+ typename PointCloudAOS<Storage>::Ptr &output)
+{
+ if (!output)
+ output.reset (new PointCloudAOS<Storage>);
+
+ output->points.resize (input->points.size ());
+
+ typename PointCloudAOS<Storage>::iterator it = thrust::copy_if (input->points.begin (), input->points.end (), indices.begin (), output->points.begin (), isNotInlier ());
+ output->points.resize (it - output->points.begin ());
+
+ output->width = (unsigned int) output->points.size();
+ output->height = 1;
+ output->is_dense = false;
+}
+
+template <template <typename> class Storage>
+void extractIndices (const typename PointCloudAOS<Storage>::Ptr &input,
+ typename Storage<int>::type& indices,
+ typename PointCloudAOS<Storage>::Ptr &output, const OpenNIRGB& color)
+{
+ extractIndices<Storage> (input, indices, output);
+ thrust::for_each ( output->points.begin(), output->points.end(), SetColor (color) );
+}
+
+template <template <typename> class Storage>
+void removeIndices (const typename PointCloudAOS<Storage>::Ptr &input,
+ typename Storage<int>::type& indices,
+ typename PointCloudAOS<Storage>::Ptr &output, const OpenNIRGB& color)
+{
+ removeIndices<Storage> (input, indices, output);
+ thrust::for_each ( output->points.begin(), output->points.end(), SetColor (color) );
+}
+
+template <template <typename> class Storage>
+void colorIndices (typename PointCloudAOS<Storage>::Ptr &input,
+ boost::shared_ptr<typename Storage<int>::type> indices,
+ const OpenNIRGB& color)
+{
+ thrust::transform_if (input->points.begin (), input->points.end (), indices->begin (), input->points.begin (), ChangeColor (color), isInlier());
+}
+
+struct ColorCloudFromImage
+{
+ ColorCloudFromImage (char4* colors) : colors_(colors)
+ {}
+ char4 * colors_;
+
+ template <typename Tuple>
+ inline __host__ __device__
+ PointXYZRGB operator () (Tuple &t)
+ {
+ PointXYZRGB &pt = thrust::get<0>(t);
+ char4 rgb = colors_[thrust::get<1>(t)];
+ pt.rgb = ((unsigned char)rgb.x << 16) + ((unsigned char)rgb.y << 8) + (unsigned char)rgb.z;
+ return pt;
+ }
+};
+
+
+template <template <typename> class Storage>
+void colorCloud (typename PointCloudAOS<Storage>::Ptr &input,
+ typename Storage<char4>::type &colors)
+{
+ thrust::transform (thrust::make_zip_iterator(thrust::make_tuple (input->points.begin(), thrust::counting_iterator<int>(0))),
+ thrust::make_zip_iterator(thrust::make_tuple (input->points.begin(), thrust::counting_iterator<int>(0))) + input->width * input->height,
+ input->points.begin (), ColorCloudFromImage (thrust::raw_pointer_cast(&colors[0])));
+}
+
+
+
+template PCL_EXPORTS void extractIndices<Host>(const PointCloudAOS<Host>::Ptr &input,
+ Host<int>::type& indices,
+ PointCloudAOS<Host>::Ptr &output);
+template PCL_EXPORTS void extractIndices<Device> (const PointCloudAOS<Device>::Ptr &input,
+ Device<int>::type& indices,
+ PointCloudAOS<Device>::Ptr &output);
+
+template PCL_EXPORTS void removeIndices<Host>(const PointCloudAOS<Host>::Ptr &input,
+ Host<int>::type& indices,
+ PointCloudAOS<Host>::Ptr &output);
+template PCL_EXPORTS void removeIndices<Device> (const PointCloudAOS<Device>::Ptr &input,
+ Device<int>::type& indices,
+ PointCloudAOS<Device>::Ptr &output);
+
+template PCL_EXPORTS void extractIndices<Host>(const PointCloudAOS<Host>::Ptr &input,
+ Host<int>::type& indices,
+ PointCloudAOS<Host>::Ptr &output, const OpenNIRGB& color);
+template PCL_EXPORTS void extractIndices<Device> (const PointCloudAOS<Device>::Ptr &input,
+ Device<int>::type& indices,
+ PointCloudAOS<Device>::Ptr &output, const OpenNIRGB& color);
+
+template PCL_EXPORTS void removeIndices<Host>(const PointCloudAOS<Host>::Ptr &input,
+ Host<int>::type& indices,
+ PointCloudAOS<Host>::Ptr &output, const OpenNIRGB& color);
+template PCL_EXPORTS void removeIndices<Device> (const PointCloudAOS<Device>::Ptr &input,
+ Device<int>::type& indices,
+ PointCloudAOS<Device>::Ptr &output, const OpenNIRGB& color);
+
+template PCL_EXPORTS void colorIndices<Host> (PointCloudAOS<Host>::Ptr &input,
+ boost::shared_ptr<Host<int>::type> indices,
+ const OpenNIRGB& color);
+template PCL_EXPORTS void colorIndices<Device> (PointCloudAOS<Device>::Ptr &input,
+ boost::shared_ptr<Device<int>::type> indices,
+ const OpenNIRGB& color);
+template PCL_EXPORTS void colorCloud<Host> (PointCloudAOS<Host>::Ptr &input, Host<char4>::type &colors);
+template PCL_EXPORTS void colorCloud<Device>(PointCloudAOS<Device>::Ptr &input, Device<char4>::type &colors);
+
+template PCL_EXPORTS
+void extractMask<Device,unsigned char> (const PointCloudAOS<Device>::Ptr &input, unsigned char* mask, PointCloudAOS<Device>::Ptr &output);
+template PCL_EXPORTS
+void extractMask<Host,unsigned char> (const PointCloudAOS<Host>::Ptr &input, unsigned char* mask, PointCloudAOS<Host>::Ptr &output);
+template PCL_EXPORTS
+void extractMask<Device,float4,unsigned char> (const boost::shared_ptr<Device<float4>::type> &input,
+ unsigned char* mask,
+ boost::shared_ptr<Device<float4>::type> &output);
+template PCL_EXPORTS
+void extractMask<Host,float4,unsigned char> (const boost::shared_ptr<Host<float4>::type> &input,
+ unsigned char* mask,
+ boost::shared_ptr<Host<float4>::type> &output);
+
+} // namespace
+} // namespace
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <pcl/cuda/point_cloud.h>
+#include "pcl/cuda/io/host_device.h"
+
+#include <pcl/pcl_exports.h>
+
+namespace pcl
+{
+ namespace cuda
+ {
+
+ template <template <typename> class Storage, template <typename> class OtherStorage>
+ typename PointCloudAOS<OtherStorage>::Ptr toStorage (const PointCloudAOS<Storage> &input)
+ {
+ typename PointCloudAOS<OtherStorage>::Ptr out (new PointCloudAOS<OtherStorage>);
+ *out << input;
+ return out;
+ }
+
+ template <template <typename> class Storage>
+ void
+ toHost (const PointCloudAOS<Storage> &input, PointCloudAOS<Host> &output)
+ {
+ output << input;
+ }
+
+ template <template <typename> class Storage>
+ void
+ toDevice (const PointCloudAOS<Storage> &input, PointCloudAOS<Device> &output)
+ {
+ output << input;
+ }
+
+ template PCL_EXPORTS PointCloudAOS<Device>::Ptr toStorage <Host, Device> (const PointCloudAOS<Host> &input);
+ template PCL_EXPORTS PointCloudAOS<Device>::Ptr toStorage <Device, Device> (const PointCloudAOS<Device> &input);
+ template PCL_EXPORTS PointCloudAOS<Host>::Ptr toStorage <Host, Host> (const PointCloudAOS<Host> &input);
+ template PCL_EXPORTS PointCloudAOS<Host>::Ptr toStorage <Device, Host> (const PointCloudAOS<Device> &input);
+
+ } // namespace
+} // namespace
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include "pcl/cuda/io/kinect_smoothing.h"
+#include <thrust/iterator/constant_iterator.h>
+#include <string>
+#include <iostream>
+#include <fstream>
+
+namespace pcl
+{
+ namespace cuda
+ {
+
+ //////////////////////////////////////////////////////////////////////////
+ template <typename Tuple> PointXYZRGB
+ ComputeXYZ::operator () (const Tuple &t)
+ {
+ PointXYZRGB pt;
+
+ if (thrust::get<0>(t) == 0) // assume no_sample_value and shadow_value are also 0
+ pt.x = pt.y = pt.z = bad_point;
+ else
+ {
+ // Fill in XYZ (and copy NaNs with it)
+ pt.x = ((thrust::get<1>(t) % width) - center_x) * thrust::get<0>(t) * constant * 0.001f;
+ pt.y = ((thrust::get<1>(t) / width) - center_y) * thrust::get<0>(t) * constant * 0.001f;
+ pt.z = thrust::get<0>(t) * 0.001f;
+ }
+ return (pt);
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <typename Tuple> PointXYZRGB
+ ComputeXYZRGB::operator () (const Tuple &t)
+ {
+ PointXYZRGB pt;
+
+ if (thrust::get<0>(t) == 0) // assume no_sample_value and shadow_value are also 0
+ pt.x = pt.y = pt.z = bad_point;
+ else
+ {
+ // Fill in XYZ (and copy NaNs with it)
+ pt.x = ((thrust::get<2>(t) % width) - center_x) * thrust::get<0>(t) * constant * 0.001f;
+ pt.y = ((thrust::get<2>(t) / width) - center_y) * thrust::get<0>(t) * constant * 0.001f;
+ pt.z = thrust::get<0>(t) * 0.001f;
+ }
+ pt.rgb = thrust::get<1>(t).r << 16 | thrust::get<1>(t).g << 8 | thrust::get<1>(t).b;
+ return (pt);
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ //void
+ //DisparityToCloud::compute (const pcl::PCLImage::ConstPtr &depth_image,
+ // const pcl::PCLImage::ConstPtr &rgb_image,
+ // const pcl::CameraInfo::ConstPtr &info,
+ // PointCloudAOS<Device>::Ptr &output)
+ //{
+ // if (!output)
+ // output.reset (new PointCloudAOS<Device>);
+ //
+ // using namespace thrust;
+ //
+ // // Prepare the output
+ // output->height = depth_image->height;
+ // output->width = depth_image->width;
+ // output->is_dense = false;
+ // output->points.resize (output->width * output->height);
+ //
+ // // Copy the depth data and the RGB data on the card
+ // device_vector<float> depth (depth_image->data.size () / sizeof (float));
+ // thrust::copy ((float*)(&depth_image->data[0]), (float*)(&depth_image->data[0]) + depth.size (), depth.begin ());
+ //
+ // // Prepare constants
+ // if (rgb_image)
+ // {
+ // device_vector<OpenNIRGB> rgb (rgb_image->width * rgb_image->height);
+ // thrust::copy ((OpenNIRGB*)(&rgb_image->data[0]),
+ // (OpenNIRGB*)(&rgb_image->data[rgb_image->width * rgb_image->height * 3]), rgb.begin ());
+ //
+ // // Suat: implement the rgb/depth stepping
+ // assert (rgb.size () == depth.size ());
+ //
+ // // Send the data to the device
+ // transform (
+ // make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), counting_iterator<int>(0))),
+ // make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), counting_iterator<int>(0))) +
+ // depth_image->width * depth_image->height,
+ // output->points.begin (),
+ // ComputeXYZRGB (depth_image->width, depth_image->height,
+ // depth_image->width >> 1, depth_image->height >> 1, 1.0 / info->P[0]));
+ // }
+ // else
+ // {
+ // // Send the data to the device
+ // transform (
+ // make_zip_iterator (make_tuple (depth.begin (), counting_iterator<int>(0))),
+ // make_zip_iterator (make_tuple (depth.begin (), counting_iterator<int>(0))) +
+ // depth_image->width * depth_image->height,
+ // output->points.begin (),
+ // ComputeXYZ (depth_image->width, depth_image->height,
+ // depth_image->width >> 1, depth_image->height >> 1, 1.0 / info->P[0]));
+ // }
+ //
+ //}
+ //
+ ////////////////////////////////////////////////////////////////////////////
+ //void
+ //DisparityToCloud::compute (const pcl::PCLImage::ConstPtr &depth_image,
+ // const pcl::PCLImage::ConstPtr &rgb_image,
+ // const pcl::CameraInfo::ConstPtr &info,
+ // PointCloudAOS<Host>::Ptr &output)
+ //{
+ // if (!output)
+ // output.reset (new PointCloudAOS<Host>);
+ //
+ // PointCloudAOS<Device>::Ptr data;
+ // compute (depth_image, rgb_image, info, data);
+ // *output << *data;
+ //}
+
+ // TODO: This function is a little misplaced here..
+ template <typename T>
+ void save_pgm (const T* depth, std::string filename, int w, int h)
+ {
+ std::ofstream ofs (filename.c_str());
+ ofs << "P2" << std::endl << w << " " << h << std::endl;
+ T max_el = 0;
+ for (int depthIdx = 0; depthIdx < h * w; ++depthIdx)
+ if (depth[depthIdx] > max_el)
+ max_el = depth[depthIdx];
+ ofs << (int)max_el << std::endl;
+ for (int yIdx = 0; yIdx < h; ++yIdx)
+ {
+ for (int xIdx = 0; xIdx < w; ++xIdx)
+ {
+ ofs << (int)depth[w*yIdx+xIdx] << " ";
+ }
+ ofs << std::endl;
+ }
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> void
+ DisparityToCloud::compute (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ const boost::shared_ptr<openni_wrapper::Image>& rgb_image,
+ float constant,
+ typename PointCloudAOS<Storage>::Ptr &output,
+ bool downsample, int stride)
+ {
+ if (!output)
+ output.reset (new PointCloudAOS<Storage>);
+
+ using namespace thrust;
+
+ int depth_width = depth_image->getWidth ();
+ int depth_height = depth_image->getHeight ();
+
+ // Prepare the output
+ if (downsample)
+ {
+ output->height = depth_height / stride;
+ output->width = depth_width / stride;
+ constant *= stride;
+ }
+ else
+ {
+ output->height = depth_height;
+ output->width = depth_width;
+ }
+
+ output->is_dense = false;
+ output->points.resize (output->width * output->height);
+
+ // Copy the depth data and the RGB data on the card
+ typename Storage<float>::type depth (output->width * output->height);
+ unsigned short* depth_buffer = (unsigned short*)depth_image->getDepthMetaData ().Data ();
+
+ if (downsample)
+ {
+ typename Storage<float>::type depth_device (depth_width * depth_height);
+ // first, copy from host to Storage
+ thrust::copy (depth_buffer, (unsigned short*)(&depth_buffer[depth_width * depth_height]), depth_device.begin ());
+
+ thrust::counting_iterator<int> counter (0);
+ //typename Storage<int>::type downsampled_indices;
+ //downsampled_indices.resize ((output->width/2) * (output->height/2));
+ //thrust::copy_if (counting_iterator<int>(0), counting_iterator<int>(0)+depth_width*depth_height, downsampled_indices.begin (), downsampleIndices (output->width, output->height, 2));
+ thrust::copy_if (depth_device.begin (),
+ depth_device.end (),
+ //thrust::make_constant_iterator (12), thrust::make_constant_iterator (12) + depth_width * depth_height,
+ counter,
+ depth.begin (),
+ downsampleIndices (depth_width, depth_height, stride));
+ //host_vector<float> h_depth = depth;
+ //save_pgm<float> ((float*)&(h_depth[0]), "outpoutbla", output->width, output->height);
+ }
+ else
+ thrust::copy (depth_buffer, (unsigned short*)(&depth_buffer[output->width * output->height]), depth.begin ());
+
+ // Prepare constants
+ if (rgb_image)
+ {
+ int image_width = rgb_image->getWidth ();
+ int image_height = rgb_image->getHeight ();
+ if (downsample)
+ {
+ image_width /= stride;
+ image_height /= stride;
+ }
+
+ typename Storage<OpenNIRGB>::type rgb (image_width * image_height);
+
+ if (downsample)
+ {
+ DebayeringDownsampling<Storage> debayering;
+ debayering.compute (rgb_image, rgb);
+ }
+ else
+ {
+ Debayering<Storage> debayering;
+ debayering.computeBilinear (rgb_image, rgb);
+ }
+
+ // Suat: implement the rgb/depth stepping
+ //assert (rgb.size () == depth.size ());
+
+ // Send the data to the device
+ transform (
+ make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), counting_iterator<int>(0))),
+ make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), counting_iterator<int>(0))) +
+ output->width * output->height,
+ output->points.begin (),
+ ComputeXYZRGB (output->width, output->height,
+ output->width >> 1, output->height >> 1, constant));
+ }
+ else
+ {
+ // Send the data to the device
+ transform (
+ make_zip_iterator (make_tuple (depth.begin (), counting_iterator<int>(0))),
+ make_zip_iterator (make_tuple (depth.begin (), counting_iterator<int>(0))) +
+ output->width * output->height,
+ output->points.begin (),
+ ComputeXYZ (output->width, output->height,
+ output->width >> 1, output->height >> 1, constant));
+ }
+
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ /*void
+ DisparityToCloud::compute (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ const boost::shared_ptr<openni_wrapper::Image>& rgb_image,
+ float constant,
+ PointCloudAOS<Host>::Ptr &output)
+ {
+ if (!output)
+ output.reset (new PointCloudAOS<Host>);
+
+ PointCloudAOS<Device>::Ptr data;
+ compute (depth_image, rgb_image, constant, data);
+ *output << *data;
+ }*/
+
+ //////////////////////////////////////////////////////////////////////////
+ //void
+ //DisparityToCloud::compute (const pcl::PCLImage::ConstPtr &depth_image,
+ // const pcl::CameraInfo::ConstPtr &info,
+ // PointCloudAOS<Device>::Ptr &output)
+ //{
+ // if (!output)
+ // output.reset (new PointCloudAOS<Device>);
+ //
+ // compute (depth_image, pcl::PCLImage::ConstPtr(), info, output);
+ //}
+ //
+ ////////////////////////////////////////////////////////////////////////////
+ //void
+ //DisparityToCloud::compute (const pcl::PCLImage::ConstPtr &depth_image,
+ // const pcl::CameraInfo::ConstPtr &info,
+ // PointCloudAOS<Host>::Ptr &output)
+ //{
+ // if (!output)
+ // output.reset (new PointCloudAOS<Host>);
+ //
+ // PointCloudAOS<Device>::Ptr data;
+ // compute (depth_image, info, data);
+ // *output << *data;
+ //}
+
+ //////////////////////////////////////////////////////////////////////////
+ void
+ DisparityToCloud::compute (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ float constant,
+ PointCloudAOS<Device>::Ptr &output)
+ {
+ if (!output)
+ output.reset (new PointCloudAOS<Device>);
+
+ compute<Device> (depth_image, boost::shared_ptr<openni_wrapper::Image>(), constant, output, false);
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ void
+ DisparityToCloud::compute (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ float constant,
+ PointCloudAOS<Host>::Ptr &output)
+ {
+ if (!output)
+ output.reset (new PointCloudAOS<Host>);
+
+ compute<Host> (depth_image, boost::shared_ptr<openni_wrapper::Image>(), constant, output, false);
+ //PointCloudAOS<Device>::Ptr data;
+ //compute (depth_image, constant, data);
+ //*output << *data;
+ }
+
+ template void
+ DisparityToCloud::compute<Host> (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ const boost::shared_ptr<openni_wrapper::Image>& rgb_image,
+ float constant,
+ typename PointCloudAOS<Host>::Ptr &output,
+ bool downsample, int stride);
+ template void
+ DisparityToCloud::compute<Device> (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
+ const boost::shared_ptr<openni_wrapper::Image>& rgb_image,
+ float constant,
+ typename PointCloudAOS<Device>::Ptr &output,
+ bool downsample, int stride);
+
+ } // namespace
+} // namespace
+
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Julius Kammerl (julius@kammerl.de)
+ */
+
+#ifndef POINTCLOUD_DEPTH_NEIGHBOR_SEARCH_H
+#define POINTCLOUD_DEPTH_NEIGHBOR_SEARCH_H
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <algorithm>
+#include <queue>
+#include <vector>
+
+namespace pcl
+{
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b OrganizedNeighborSearch class
+ * \note This class provides neighbor search routines for organized point clouds.
+ * \note
+ * \note typename: PointT: type of point used in pointcloud
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ template<typename PointT>
+ class OrganizedNeighborSearch
+
+ {
+ public:
+
+ /** \brief OrganizedNeighborSearch constructor.
+ * */
+ OrganizedNeighborSearch ():
+ radiusLookupTableWidth_(-1),
+ radiusLookupTableHeight_(-1)
+ {
+ max_distance_ = std::numeric_limits<double>::max ();
+
+ focalLength_ = 1.0f;
+ }
+
+ /** \brief Empty deconstructor. */
+ virtual
+ ~OrganizedNeighborSearch ()
+ {
+ }
+
+ // public typedefs
+ typedef pcl::PointCloud<PointT> PointCloud;
+ typedef boost::shared_ptr<PointCloud> PointCloudPtr;
+ typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
+
+
+ /** \brief Provide a pointer to the input data set.
+ * \param cloud_arg the const boost shared pointer to a PointCloud message
+ */
+ inline void
+ setInputCloud (const PointCloudConstPtr &cloud_arg)
+ {
+
+ if (input_ != cloud_arg)
+ {
+ input_ = cloud_arg;
+
+ estimateFocalLengthFromInputCloud ();
+ generateRadiusLookupTable (input_->width, input_->height);
+ }
+ }
+
+ /** \brief Search for all neighbors of query point that are within a given radius.
+ * \param cloud_arg the point cloud data
+ * \param index_arg the index in \a cloud representing the query point
+ * \param radius_arg the radius of the sphere bounding all of p_q's neighbors
+ * \param k_indices_arg the resultant indices of the neighboring points
+ * \param k_sqr_distances_arg the resultant squared distances to the neighboring points
+ * \param max_nn_arg if given, bounds the maximum returned neighbors to this value
+ * \return number of neighbors found in radius
+ */
+ int
+ radiusSearch (const PointCloudConstPtr &cloud_arg, int index_arg, double radius_arg,
+ std::vector<int> &k_indices_arg, std::vector<float> &k_sqr_distances_arg,
+ int max_nn_arg = INT_MAX);
+
+ /** \brief Search for all neighbors of query point that are within a given radius.
+ * \param index_arg index representing the query point in the dataset given by \a setInputCloud.
+ * If indices were given in setInputCloud, index will be the position in the indices vector
+ * \param radius_arg radius of the sphere bounding all of p_q's neighbors
+ * \param k_indices_arg the resultant indices of the neighboring points
+ * \param k_sqr_distances_arg the resultant squared distances to the neighboring points
+ * \param max_nn_arg if given, bounds the maximum returned neighbors to this value
+ * \return number of neighbors found in radius
+ */
+ int
+ radiusSearch (int index_arg, const double radius_arg, std::vector<int> &k_indices_arg,
+ std::vector<float> &k_sqr_distances_arg, int max_nn_arg = INT_MAX) const;
+
+ /** \brief Search for all neighbors of query point that are within a given radius.
+ * \param p_q_arg the given query point
+ * \param radius_arg the radius of the sphere bounding all of p_q's neighbors
+ * \param k_indices_arg the resultant indices of the neighboring points
+ * \param k_sqr_distances_arg the resultant squared distances to the neighboring points
+ * \param max_nn_arg if given, bounds the maximum returned neighbors to this value
+ * \return number of neighbors found in radius
+ */
+ int
+ radiusSearch (const PointT &p_q_arg, const double radius_arg, std::vector<int> &k_indices_arg,
+ std::vector<float> &k_sqr_distances_arg, int max_nn_arg = INT_MAX) const;
+
+ /** \brief Search for k-nearest neighbors at the query point.
+ * \param cloud_arg the point cloud data
+ * \param index_arg the index in \a cloud representing the query point
+ * \param k_arg the number of neighbors to search for
+ * \param k_indices_arg the resultant indices of the neighboring points (must be resized to \a k a priori!)
+ * \param k_sqr_distances_arg the resultant squared distances to the neighboring points (must be resized to \a k
+ * a priori!)
+ * \return number of neighbors found
+ */
+ int
+ nearestKSearch (const PointCloudConstPtr &cloud_arg, int index_arg, int k_arg, std::vector<int> &k_indices_arg,
+ std::vector<float> &k_sqr_distances_arg);
+
+ /** \brief Search for k-nearest neighbors at query point
+ * \param index_arg index representing the query point in the dataset given by \a setInputCloud.
+ * If indices were given in setInputCloud, index will be the position in the indices vector.
+ * \param k_arg the number of neighbors to search for
+ * \param k_indices_arg the resultant indices of the neighboring points (must be resized to \a k a priori!)
+ * \param k_sqr_distances_arg the resultant squared distances to the neighboring points (must be resized to \a k
+ * a priori!)
+ * \return number of neighbors found
+ */
+ int
+ nearestKSearch (int index_arg, int k_arg, std::vector<int> &k_indices_arg,
+ std::vector<float> &k_sqr_distances_arg);
+
+ /** \brief Search for k-nearest neighbors at given query point.
+ * @param p_q_arg the given query point
+ * @param k_arg the number of neighbors to search for
+ * @param k_indices_arg the resultant indices of the neighboring points (must be resized to k a priori!)
+ * @param k_sqr_distances_arg the resultant squared distances to the neighboring points (must be resized to k a priori!)
+ * @return number of neighbors found
+ */
+ int
+ nearestKSearch (const PointT &p_q_arg, int k_arg, std::vector<int> &k_indices_arg,
+ std::vector<float> &k_sqr_distances_arg);
+
+ /** \brief Get the maximum allowed distance between the query point and its nearest neighbors. */
+ inline double
+ getMaxDistance () const
+ {
+ return (max_distance_);
+ }
+
+ /** \brief Set the maximum allowed distance between the query point and its nearest neighbors. */
+ inline void
+ setMaxDistance (double max_dist)
+ {
+ max_distance_ = max_dist;
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Protected methods
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ protected:
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b radiusSearchLoopkupEntry entry for radius search lookup vector
+ * \note This class defines entries for the radius search lookup vector
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ class radiusSearchLoopkupEntry
+ {
+ public:
+
+ /** \brief Empty constructor */
+ radiusSearchLoopkupEntry ()
+ {
+ }
+
+ /** \brief Empty deconstructor */
+ ~radiusSearchLoopkupEntry ()
+ {
+ }
+
+ /** \brief Define search point and calculate squared distance
+ * @param x_shift shift in x dimension
+ * @param y_shift shift in y dimension
+ */
+ void
+ defineShiftedSearchPoint(int x_shift, int y_shift)
+ {
+ x_diff_ =x_shift;
+ y_diff_ =y_shift;
+
+ squared_distance_ = x_diff_ * x_diff_ + y_diff_ * y_diff_;
+ }
+
+ /** \brief Operator< for comparing radiusSearchLoopkupEntry instances with each other. */
+ bool
+ operator< (const radiusSearchLoopkupEntry& rhs_arg) const
+ {
+ return (this->squared_distance_ < rhs_arg.squared_distance_);
+ }
+
+ // Public globals
+ int x_diff_;
+ int y_diff_;
+ int squared_distance_;
+
+ };
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b nearestNeighborCandidate entry for the nearest neighbor candidate queue
+ * \note This class defines entries for the nearest neighbor candidate queue
+ * \author Julius Kammerl (julius@kammerl.de)
+ */
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ class nearestNeighborCandidate
+ {
+ public:
+
+ /** \brief Empty constructor */
+ nearestNeighborCandidate ()
+ {
+ }
+
+ /** \brief Empty deconstructor */
+ ~nearestNeighborCandidate ()
+ {
+ }
+
+ /** \brief Operator< for comparing nearestNeighborCandidate instances with each other. */
+ bool
+ operator< (const nearestNeighborCandidate& rhs_arg) const
+ {
+ return (this->squared_distance_ < rhs_arg.squared_distance_);
+ }
+
+ // Public globals
+ int index_;
+ double squared_distance_;
+
+ };
+
+ /** \brief Get point at index from input pointcloud dataset
+ * \param index_arg index representing the point in the dataset given by \a setInputCloud
+ * \return PointT from input pointcloud dataset
+ */
+ const PointT&
+ getPointByIndex (const unsigned int index_arg) const;
+
+ /** \brief Generate radius lookup table. It is used to subsequentially iterate over points
+ * which are close to the search point
+ * \param width of organized point cloud
+ * \param height of organized point cloud
+ */
+ void
+ generateRadiusLookupTable (unsigned int width, unsigned int height);
+
+ inline void
+ pointPlaneProjection (const PointT& point, int& xpos, int& ypos) const
+ {
+ xpos = (int) pcl_round(point.x / (point.z * focalLength_));
+ ypos = (int) pcl_round(point.y / (point.z * focalLength_));
+ }
+
+ void
+ getProjectedRadiusSearchBox (const PointT& point_arg, double squared_radius_arg, int& minX_arg, int& minY_arg, int& maxX_arg, int& maxY_arg ) const;
+
+
+ /** \brief Estimate focal length parameter that was used during point cloud generation
+ */
+ void
+ estimateFocalLengthFromInputCloud ();
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Class getName method. */
+ virtual std::string
+ getName () const
+ {
+ return ("Organized_Neighbor_Search");
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Globals
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ /** \brief Pointer to input point cloud dataset. */
+ PointCloudConstPtr input_;
+
+ /** \brief Maximum allowed distance between the query point and its k-neighbors. */
+ double max_distance_;
+
+ /** \brief Global focal length parameter */
+ double focalLength_;
+
+ /** \brief Precalculated radius search lookup vector */
+ std::vector<radiusSearchLoopkupEntry> radiusSearchLookup_;
+ int radiusLookupTableWidth_;
+ int radiusLookupTableHeight_;
+
+ };
+
+}
+
+//#include "organized_neighbor_search.hpp"
+
+#endif
+
--- /dev/null
+#ifndef POINTCLOUD_DEPTH_NEIGHBOR_SEARCH_HPP
+#define POINTCLOUD_DEPTH_NEIGHBOR_SEARCH_HPP
+
+#ifndef PI
+ #define PI 3.14159
+#endif
+
+namespace pcl
+{
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ template<typename PointT>
+ int
+ OrganizedNeighborSearch<PointT>::radiusSearch (const PointCloudConstPtr &cloud_arg, int index_arg,
+ double radius_arg, std::vector<int> &k_indices_arg,
+ std::vector<float> &k_sqr_distances_arg, int max_nn_arg)
+ {
+ this->setInputCloud (cloud_arg);
+
+ return radiusSearch (index_arg, radius_arg, k_indices_arg, k_sqr_distances_arg, max_nn_arg);
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ template<typename PointT>
+ int
+ OrganizedNeighborSearch<PointT>::radiusSearch (int index_arg, const double radius_arg,
+ std::vector<int> &k_indices_arg,
+ std::vector<float> &k_sqr_distances_arg, int max_nn_arg) const
+ {
+
+ const PointT searchPoint = getPointByIndex (index_arg);
+
+ return radiusSearch (searchPoint, radius_arg, k_indices_arg, k_sqr_distances_arg, max_nn_arg);
+
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ template<typename PointT>
+ int
+ OrganizedNeighborSearch<PointT>::radiusSearch (const PointT &p_q_arg, const double radius_arg,
+ std::vector<int> &k_indices_arg,
+ std::vector<float> &k_sqr_distances_arg, int max_nn_arg) const
+ {
+ if (input_->height == 1)
+ {
+ PCL_ERROR ("[pcl::%s::radiusSearch] Input dataset is not organized!", getName ().c_str ());
+ return 0;
+ }
+
+ // search window
+ int leftX, rightX, leftY, rightY;
+ int x, y, idx;
+ double squared_distance, squared_radius;
+ int nnn;
+
+ k_indices_arg.clear ();
+ k_sqr_distances_arg.clear ();
+
+ squared_radius = radius_arg*radius_arg;
+
+ this->getProjectedRadiusSearchBox(p_q_arg, squared_radius, leftX, rightX, leftY, rightY);
+
+
+
+
+ // iterate over all children
+ nnn = 0;
+ for (x = leftX; (x <= rightX) && (nnn < max_nn_arg); x++)
+ for (y = leftY; (y <= rightY) && (nnn < max_nn_arg); y++)
+ {
+ idx = y * input_->width + x;
+ const PointT& point = input_->points[idx];
+
+ const double point_dist_x = point.x - p_q_arg.x;
+ const double point_dist_y = point.y - p_q_arg.y;
+ const double point_dist_z = point.z - p_q_arg.z;
+
+ // calculate squared distance
+ squared_distance = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z * point_dist_z);
+
+ // check distance and add to results
+ if (squared_distance <= squared_radius)
+ {
+ k_indices_arg.push_back (idx);
+ k_sqr_distances_arg.push_back (squared_distance);
+ nnn++;
+ }
+ }
+
+ return nnn;
+
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ template<typename PointT>
+ void
+ OrganizedNeighborSearch<PointT>::getProjectedRadiusSearchBox (const PointT& point_arg, double squared_radius_arg, int& minX_arg, int& maxX_arg, int& minY_arg, int& maxY_arg ) const
+ {
+ double r_sqr, r_quadr, z_sqr;
+ double sqrt_term_y, sqrt_term_x, norm;
+ double x_times_z, y_times_z;
+ double x1, x2, y1, y2;
+
+ // see http://www.wolframalpha.com/input/?i=solve+%5By%2Fsqrt%28f^2%2By^2%29*c-f%2Fsqrt%28f^2%2By^2%29*b%2Br%3D%3D0%2C+f%3D1%2C+y%5D
+ // where b = p_q_arg.y, c = p_q_arg.z, r = radius_arg, f = focalLength_
+
+ r_sqr = squared_radius_arg;
+ r_quadr = r_sqr * r_sqr;
+ z_sqr = point_arg.z * point_arg.z;
+
+ sqrt_term_y = sqrt (point_arg.y * point_arg.y * r_sqr + z_sqr * r_sqr - r_quadr);
+ sqrt_term_x = sqrt (point_arg.x * point_arg.x * r_sqr + z_sqr * r_sqr - r_quadr);
+ norm = 1.0 / (z_sqr - r_sqr);
+
+ x_times_z = point_arg.x * point_arg.z;
+ y_times_z = point_arg.y * point_arg.z;
+
+ y1 = (y_times_z - sqrt_term_y) * norm;
+ y2 = (y_times_z + sqrt_term_y) * norm;
+ x1 = (x_times_z - sqrt_term_x) * norm;
+ x2 = (x_times_z + sqrt_term_x) * norm;
+
+ // determine 2-D search window
+ minX_arg = (int)floor((double)input_->width / 2 + (x1 / focalLength_));
+ maxX_arg = (int)ceil((double)input_->width / 2 + (x2 / focalLength_));
+
+ minY_arg = (int)floor((double)input_->height / 2 + (y1 / focalLength_));
+ maxY_arg = (int)ceil((double)input_->height / 2 + (y2 / focalLength_));
+
+ // make sure the coordinates fit to point cloud resolution
+ minX_arg = std::max<int> (0, minX_arg);
+ maxX_arg = std::min<int> ((int)input_->width - 1, maxX_arg);
+
+ minY_arg = std::max<int> (0, minY_arg);
+ maxY_arg = std::min<int> ((int)input_->height - 1, maxY_arg);
+ }
+
+
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ template<typename PointT>
+ int
+ OrganizedNeighborSearch<PointT>::nearestKSearch (int index_arg, int k_arg, std::vector<int> &k_indices_arg,
+ std::vector<float> &k_sqr_distances_arg)
+ {
+
+ const PointT searchPoint = getPointByIndex (index_arg);
+
+ return nearestKSearch (searchPoint, k_arg, k_indices_arg, k_sqr_distances_arg);
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ template<typename PointT>
+ int
+ OrganizedNeighborSearch<PointT>::nearestKSearch (const PointCloudConstPtr &cloud_arg, int index_arg, int k_arg,
+ std::vector<int> &k_indices_arg,
+ std::vector<float> &k_sqr_distances_arg)
+ {
+ this->setInputCloud (cloud_arg);
+
+ return nearestKSearch (index_arg, k_arg, k_indices_arg, k_sqr_distances_arg);
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ template<typename PointT>
+ int
+ OrganizedNeighborSearch<PointT>::nearestKSearch (const PointT &p_q_arg, int k_arg, std::vector<int> &k_indices_arg,
+ std::vector<float> &k_sqr_distances_arg)
+ {
+ int x_pos, y_pos, x, y, idx;
+ std::size_t i;
+
+ int leftX, rightX, leftY, rightY;
+
+ int radiusSearchPointCount;
+
+ int maxSearchDistance;
+ double squaredMaxSearchRadius;
+
+ assert (k_arg>0);
+
+ if (input_->height == 1)
+ {
+ PCL_ERROR ("[pcl::%s::nearestKSearch] Input dataset is not organized!", getName ().c_str ());
+ return 0;
+ }
+
+ squaredMaxSearchRadius = max_distance_*max_distance_;
+
+ // vector for nearest neighbor candidates
+ std::vector<nearestNeighborCandidate> nearestNeighbors;
+
+ // iterator for radius seach lookup table
+ typename std::vector<radiusSearchLoopkupEntry>::const_iterator radiusSearchLookup_Iterator;
+ radiusSearchLookup_Iterator = radiusSearchLookup_.begin ();
+
+ nearestNeighbors.reserve (k_arg * 2);
+
+ // project search point to plane
+ pointPlaneProjection (p_q_arg, x_pos, y_pos);
+ x_pos += (int)input_->width/2;
+ y_pos += (int)input_->height/2;
+
+ // initialize result vectors
+ k_indices_arg.clear ();
+ k_sqr_distances_arg.clear ();
+
+
+ radiusSearchPointCount = 0;
+ // search for k_arg nearest neighbor candidates using the radius lookup table
+ while ((radiusSearchLookup_Iterator != radiusSearchLookup_.end ()) && ((int)nearestNeighbors.size () < k_arg))
+ {
+ // select point from organized pointcloud
+ x = x_pos + (*radiusSearchLookup_Iterator).x_diff_;
+ y = y_pos + (*radiusSearchLookup_Iterator).y_diff_;
+ radiusSearchLookup_Iterator++;
+ radiusSearchPointCount++;
+
+ if ((x >= 0) && (y >= 0) && (x < (int)input_->width) && (y < (int)input_->height))
+ {
+ idx = y * (int)input_->width + x;
+ const PointT& point = input_->points[idx];
+
+ if ((point.x == point.x) && // check for NaNs
+ (point.y == point.y) &&
+ (point.z == point.z))
+ {
+ double squared_distance;
+
+ const double point_dist_x = point.x - p_q_arg.x;
+ const double point_dist_y = point.y - p_q_arg.y;
+ const double point_dist_z = point.z - p_q_arg.z;
+
+ // calculate squared distance
+ squared_distance
+ = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z * point_dist_z);
+
+ if (squared_distance <= squaredMaxSearchRadius)
+ {
+ // we have a new candidate -> add it
+ nearestNeighborCandidate newCandidate;
+ newCandidate.index_ = idx;
+ newCandidate.squared_distance_ = squared_distance;
+
+ nearestNeighbors.push_back (newCandidate);
+ }
+
+ }
+ }
+ }
+
+ // sort candidate list
+ std::sort (nearestNeighbors.begin (), nearestNeighbors.end ());
+
+ // we found k_arg candidates -> do radius search
+ if ((int)nearestNeighbors.size () == k_arg)
+ {
+ double squared_radius;
+ unsigned int pointCountRadiusSearch;
+ unsigned int pointCountCircleSearch;
+
+ squared_radius = std::min<double>(nearestNeighbors.back ().squared_distance_, squaredMaxSearchRadius);
+
+ this->getProjectedRadiusSearchBox(p_q_arg, squared_radius, leftX, rightX, leftY, rightY);
+
+ leftX *=leftX;
+ rightX *= rightX;
+ leftY *=leftY;
+ rightY *= rightY;
+
+ pointCountRadiusSearch = (rightX-leftX)*(rightY-leftY);
+
+ // search for maximum distance between search point to window borders in 2-D search window
+ maxSearchDistance = 0;
+ maxSearchDistance = std::max<int> (maxSearchDistance, leftX + leftY);
+ maxSearchDistance = std::max<int> (maxSearchDistance, leftX + rightY);
+ maxSearchDistance = std::max<int> (maxSearchDistance, rightX + leftY);
+ maxSearchDistance = std::max<int> (maxSearchDistance, rightX + rightY);
+
+ maxSearchDistance +=1;
+ maxSearchDistance *=maxSearchDistance;
+
+ pointCountCircleSearch= (int)(PI*(double)(maxSearchDistance*maxSearchDistance));
+
+ if (1){//(pointCountCircleSearch<pointCountRadiusSearch) {
+
+ // check for nearest neighbors within window
+ while ((radiusSearchLookup_Iterator != radiusSearchLookup_.end ())
+ && ((*radiusSearchLookup_Iterator).squared_distance_ <= maxSearchDistance))
+ {
+ // select point from organized point cloud
+ x = x_pos + (*radiusSearchLookup_Iterator).x_diff_;
+ y = y_pos + (*radiusSearchLookup_Iterator).y_diff_;
+ radiusSearchLookup_Iterator++;
+
+ if ((x >= 0) && (y >= 0) && (x < (int)input_->width) && (y < (int)input_->height))
+ {
+ idx = y * (int)input_->width + x;
+ const PointT& point = input_->points[idx];
+
+ if ((point.x == point.x) && // check for NaNs
+ (point.y == point.y) && (point.z == point.z))
+ {
+ double squared_distance;
+
+ const double point_dist_x = point.x - p_q_arg.x;
+ const double point_dist_y = point.y - p_q_arg.y;
+ const double point_dist_z = point.z - p_q_arg.z;
+
+ // calculate squared distance
+ squared_distance = (point_dist_x * point_dist_x + point_dist_y * point_dist_y + point_dist_z
+ * point_dist_z);
+
+ if ( squared_distance<=squared_radius )
+ {
+ // add candidate
+ nearestNeighborCandidate newCandidate;
+ newCandidate.index_ = idx;
+ newCandidate.squared_distance_ = squared_distance;
+
+ nearestNeighbors.push_back (newCandidate);
+ }
+ }
+ }
+ }
+ } else {
+ std::vector<int> k_radius_indices;
+ std::vector<float> k_radius_distances;
+
+ nearestNeighbors.clear();
+
+ k_radius_indices.reserve (k_arg*2);
+ k_radius_distances.reserve (k_arg*2);
+
+ radiusSearch (p_q_arg, sqrt(squared_radius),k_radius_indices , k_radius_distances);
+
+ std::cout << k_radius_indices.size () <<std::endl;
+
+ for (i = 0; i < k_radius_indices.size (); i++)
+ {
+ nearestNeighborCandidate newCandidate;
+ newCandidate.index_ = k_radius_indices[i];
+ newCandidate.squared_distance_ = k_radius_distances[i];
+
+ nearestNeighbors.push_back (newCandidate);
+ }
+
+
+ }
+
+ std::sort (nearestNeighbors.begin (), nearestNeighbors.end ());
+
+ // truncate sorted nearest neighbor vector if we found more than k_arg candidates
+ if (nearestNeighbors.size () > (size_t)k_arg)
+ {
+ nearestNeighbors.resize (k_arg);
+ }
+
+ }
+
+ // copy results from nearestNeighbors vector to separate indices and distance vector
+ k_indices_arg.resize (nearestNeighbors.size ());
+ k_sqr_distances_arg.resize (nearestNeighbors.size ());
+
+ for (i = 0; i < nearestNeighbors.size (); i++)
+ {
+ k_indices_arg[i] = nearestNeighbors[i].index_;
+ k_sqr_distances_arg[i] = nearestNeighbors[i].squared_distance_;
+ }
+
+ return k_indices_arg.size ();
+
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ template<typename PointT>
+ void
+ OrganizedNeighborSearch<PointT>::estimateFocalLengthFromInputCloud ()
+ {
+ size_t i, count;
+ int x, y;
+
+ focalLength_ = 0;
+
+ count = 0;
+ for (y = 0; y < (int)input_->height; y++)
+ for (x = 0; x < (int)input_->width; x++)
+ {
+ i = y * input_->width + x;
+ if ((input_->points[i].x == input_->points[i].x) && // check for NaNs
+ (input_->points[i].y == input_->points[i].y) && (input_->points[i].z == input_->points[i].z))
+ {
+ const PointT& point = input_->points[i];
+ if ((double)(x - input_->width / 2) * (double)(y - input_->height / 2) * point.z != 0)
+ {
+ // estimate the focal length for point.x and point.y
+ focalLength_ += point.x / ((double)(x - (int)input_->width / 2) * point.z);
+ focalLength_ += point.y / ((double)(y - (int)input_->height / 2) * point.z);
+ count += 2;
+ }
+ }
+ }
+ // calculate an average of the focalLength
+ focalLength_ /= (double)count;
+
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ template<typename PointT>
+ void
+ OrganizedNeighborSearch<PointT>::generateRadiusLookupTable (unsigned int width, unsigned int height)
+ {
+ int x, y, c;
+
+ if ( (this->radiusLookupTableWidth_!=(int)width) || (this->radiusLookupTableHeight_!=(int)height) )
+ {
+
+ this->radiusLookupTableWidth_ = (int)width;
+ this->radiusLookupTableHeight_= (int)height;
+
+ radiusSearchLookup_.clear ();
+ radiusSearchLookup_.resize ((2*width+1) * (2*height+1));
+
+ c = 0;
+ for (x = -(int)width; x < (int)width+1; x++)
+ for (y = -(int)height; y <(int)height+1; y++)
+ {
+ radiusSearchLookup_[c++].defineShiftedSearchPoint(x, y);
+ }
+
+ std::sort (radiusSearchLookup_.begin (), radiusSearchLookup_.end ());
+ }
+
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ template<typename PointT>
+ const PointT&
+ OrganizedNeighborSearch<PointT>::getPointByIndex (const unsigned int index_arg) const
+ {
+ // retrieve point from input cloud
+ assert (index_arg < (unsigned int)input_->points.size ());
+ return this->input_->points[index_arg];
+
+ }
+
+}
+
+
+#define PCL_INSTANTIATE_OrganizedNeighborSearch(T) template class pcl::OrganizedNeighborSearch<T>;
+
+#endif
--- /dev/null
+set(SUBSYS_NAME cuda_sample_consensus)
+set(SUBSYS_PATH cuda/sample_consensus)
+set(SUBSYS_DESC "Point Cloud CUDA Sample Consensus library")
+set(SUBSYS_DEPS cuda_common io common)
+
+# ---[ Point Cloud Library - pcl/cuda/io
+
+set(build TRUE)
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
+mark_as_advanced("BUILD_${SUBSYS_NAME}")
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS})
+PCL_SET_SUBSYS_INCLUDE_DIR("${SUBSYS_NAME}" "${SUBSYS_PATH}")
+
+if (build)
+ set(srcs
+ src/sac_model.cu
+ src/sac_model_plane.cu
+ src/sac_model_1point_plane.cu
+ src/ransac.cu
+ src/multi_ransac.cu
+ )
+
+ set(incs
+ include/pcl/cuda/sample_consensus/sac.h
+ include/pcl/cuda/sample_consensus/ransac.h
+ include/pcl/cuda/sample_consensus/multi_ransac.h
+ include/pcl/cuda/sample_consensus/sac_model.h
+ include/pcl/cuda/sample_consensus/sac_model_plane.h
+ include/pcl/cuda/sample_consensus/sac_model_1point_plane.h
+ )
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
+ PCL_CUDA_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs})
+ target_link_libraries("${LIB_NAME}" pcl_cuda_features)
+
+ set(EXT_DEPS "")
+ #set(EXT_DEPS CUDA)
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}"
+ "${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "")
+
+ # Install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}" ${incs})
+
+endif()
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_CUDA_SAMPLE_CONSENSUS_MSAC_H_
+#define PCL_CUDA_SAMPLE_CONSENSUS_MSAC_H_
+
+#include <pcl_cuda/sample_consensus/sac.h>
+#include <pcl_cuda/sample_consensus/sac_model.h>
+
+namespace pcl_cuda
+{
+
+ template <template <typename> class Storage>
+ class MEstimatorSampleConsensus : public SampleConsensus<Storage>
+ {
+ using SampleConsensus<Storage>::max_iterations_;
+ using SampleConsensus<Storage>::threshold_;
+ using SampleConsensus<Storage>::iterations_;
+ using SampleConsensus<Storage>::sac_model_;
+ using SampleConsensus<Storage>::model_;
+ using SampleConsensus<Storage>::model_coefficients_;
+ using SampleConsensus<Storage>::inliers_;
+ using SampleConsensus<Storage>::inliers_stencil_;
+ using SampleConsensus<Storage>::probability_;
+
+ typedef typename SampleConsensusModel<Storage>::Ptr SampleConsensusModelPtr;
+ typedef typename SampleConsensusModel<Storage>::Coefficients Coefficients;
+ typedef typename SampleConsensusModel<Storage>::Indices Indices;
+ typedef typename SampleConsensusModel<Storage>::Hypotheses Hypotheses;
+
+ public:
+ /** \brief MEstimatorSampleConsensus main constructor
+ * \param model a Sample Consensus model
+ */
+ MEstimatorSampleConsensus (const SampleConsensusModelPtr &model) :
+ SampleConsensus<Storage> (model)
+ {
+ // Maximum number of trials before we give up.
+ max_iterations_ = 10000;
+ }
+
+ /** \brief RANSAC (RAndom SAmple Consensus) main constructor
+ * \param model a Sample Consensus model
+ * \param threshold distance to model threshold
+ */
+ MEstimatorSampleConsensus (const SampleConsensusModelPtr &model, float threshold) :
+ SampleConsensus<Storage> (model, threshold)
+ {
+ // Maximum number of trials before we give up.
+ max_iterations_ = 10000;
+ }
+
+ /** \brief Compute the actual model and find the inliers
+ * \param debug_verbosity_level enable/disable on-screen debug
+ * information and set the verbosity level
+ */
+ bool
+ computeModel (int debug_verbosity_level = 0);
+ };
+}
+
+#endif //#ifndef PCL_CUDA_SAMPLE_CONSENSUS_MSAC_H_
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_CUDA_SAMPLE_CONSENSUS_RANSAC_H_
+#define PCL_CUDA_SAMPLE_CONSENSUS_RANSAC_H_
+
+#include <pcl/cuda/sample_consensus/sac.h>
+#include <pcl/cuda/sample_consensus/sac_model.h>
+
+namespace pcl
+{
+ namespace cuda
+ {
+ /** \brief @b RandomSampleConsensus represents an implementation of the
+ * RANSAC (RAndom SAmple Consensus) algorithm, as described in: "Random
+ * Sample Consensus: A Paradigm for Model Fitting with Applications to Image
+ * Analysis and Automated Cartography", Martin A. Fischler and Robert C. Bolles,
+ * Comm. Of the ACM 24: 381–395, June 1981.
+ * \author Radu Bogdan Rusu
+ */
+ template <template <typename> class Storage>
+ class MultiRandomSampleConsensus : public SampleConsensus<Storage>
+ {
+ using SampleConsensus<Storage>::max_iterations_;
+ using SampleConsensus<Storage>::threshold_;
+ using SampleConsensus<Storage>::iterations_;
+ using SampleConsensus<Storage>::sac_model_;
+ using SampleConsensus<Storage>::model_;
+ using SampleConsensus<Storage>::model_coefficients_;
+ using SampleConsensus<Storage>::inliers_;
+ using SampleConsensus<Storage>::inliers_stencil_;
+ using SampleConsensus<Storage>::probability_;
+
+ typedef typename SampleConsensusModel<Storage>::Ptr SampleConsensusModelPtr;
+ typedef typename SampleConsensusModel<Storage>::Coefficients Coefficients;
+ typedef typename SampleConsensusModel<Storage>::Hypotheses Hypotheses;
+
+ typedef typename SampleConsensusModel<Storage>::Indices Indices;
+ typedef typename SampleConsensusModel<Storage>::IndicesPtr IndicesPtr;
+ typedef typename SampleConsensusModel<Storage>::IndicesConstPtr IndicesConstPtr;
+
+ public:
+ /** \brief RANSAC (RAndom SAmple Consensus) main constructor
+ * \param model a Sample Consensus model
+ */
+ MultiRandomSampleConsensus (const SampleConsensusModelPtr &model) :
+ SampleConsensus<Storage> (model),
+ min_coverage_percent_ (0.9),
+ max_batches_ (5),
+ iterations_per_batch_ (1000)
+ {
+ // Maximum number of trials before we give up.
+ max_iterations_ = 10000;
+ }
+
+ /** \brief RANSAC (RAndom SAmple Consensus) main constructor
+ * \param model a Sample Consensus model
+ * \param threshold distance to model threshold
+ */
+ MultiRandomSampleConsensus (const SampleConsensusModelPtr &model, double threshold) :
+ SampleConsensus<Storage> (model, threshold)
+ {
+ // Maximum number of trials before we give up.
+ max_iterations_ = 10000;
+ }
+
+ /** \brief Compute the actual model and find the inliers
+ * \param debug_verbosity_level enable/disable on-screen debug
+ * information and set the verbosity level
+ */
+ bool
+ computeModel (int debug_verbosity_level = 0);
+
+ /** \brief how much (in percent) of the point cloud should be covered?
+ * If it is not possible to find enough planes, it will stop according to the regular ransac criteria
+ */
+ void
+ setMinimumCoverage (float percent)
+ {
+ min_coverage_percent_ = percent;
+ }
+
+ /** \brief Sets the maximum number of batches that should be processed.
+ * Every Batch computes up to iterations_per_batch_ models and verifies them.
+ * If planes with a sufficiently high total inlier count are found earlier, the
+ * actual number of batch runs might be lower.
+ */
+ void
+ setMaximumBatches (int max_batches)
+ {
+ max_batches_ = max_batches_;
+ }
+
+ /** \brief Sets the maximum number of batches that should be processed.
+ * Every Batch computes up to max_iterations_ models and verifies them.
+ * If planes with a sufficiently high total inlier count are found earlier, the
+ * actual number of batch runs might be lower.
+ */
+ void
+ setIerationsPerBatch(int iterations_per_batch)
+ {
+ iterations_per_batch_ = iterations_per_batch;
+ }
+
+ inline std::vector<IndicesPtr>
+ getAllInliers () { return all_inliers_; }
+
+ inline std::vector<int>
+ getAllInlierCounts () { return all_inlier_counts_; }
+
+ /** \brief Return the model coefficients of the best model found so far.
+ */
+ inline std::vector<float4>
+ getAllModelCoefficients ()
+ {
+ return all_model_coefficients_;
+ }
+
+ /** \brief Return the model coefficients of the best model found so far.
+ */
+ inline std::vector<float3>
+ getAllModelCentroids ()
+ {
+ return all_model_centroids_;
+ }
+
+ private:
+ float min_coverage_percent_;
+ unsigned int max_batches_;
+ unsigned int iterations_per_batch_;
+
+ /** \brief The vector of the centroids of our models computed directly from the models found. */
+ std::vector<float3> all_model_centroids_;
+
+ /** \brief The vector of coefficients of our models computed directly from the models found. */
+ std::vector<float4> all_model_coefficients_;
+
+ std::vector<IndicesPtr> all_inliers_;
+ std::vector<int> all_inlier_counts_;
+ };
+
+ } // namespace
+} // namespace
+
+#endif //#ifndef PCL_CUDA_SAMPLE_CONSENSUS_RANSAC_H_
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_CUDA_SAMPLE_CONSENSUS_RANSAC_H_
+#define PCL_CUDA_SAMPLE_CONSENSUS_RANSAC_H_
+
+#include <pcl/cuda/sample_consensus/sac.h>
+#include <pcl/cuda/sample_consensus/sac_model.h>
+
+namespace pcl
+{
+ namespace cuda
+ {
+ /** \brief @b RandomSampleConsensus represents an implementation of the
+ * RANSAC (RAndom SAmple Consensus) algorithm, as described in: "Random
+ * Sample Consensus: A Paradigm for Model Fitting with Applications to Image
+ * Analysis and Automated Cartography", Martin A. Fischler and Robert C. Bolles,
+ * Comm. Of the ACM 24: 381–395, June 1981.
+ * \author Radu Bogdan Rusu
+ */
+ template <template <typename> class Storage>
+ class RandomSampleConsensus : public SampleConsensus<Storage>
+ {
+ using SampleConsensus<Storage>::max_iterations_;
+ using SampleConsensus<Storage>::threshold_;
+ using SampleConsensus<Storage>::iterations_;
+ using SampleConsensus<Storage>::sac_model_;
+ using SampleConsensus<Storage>::model_;
+ using SampleConsensus<Storage>::model_coefficients_;
+ using SampleConsensus<Storage>::inliers_;
+ using SampleConsensus<Storage>::inliers_stencil_;
+ using SampleConsensus<Storage>::probability_;
+
+ typedef typename SampleConsensusModel<Storage>::Ptr SampleConsensusModelPtr;
+ typedef typename SampleConsensusModel<Storage>::Coefficients Coefficients;
+ typedef typename SampleConsensusModel<Storage>::Indices Indices;
+ typedef typename SampleConsensusModel<Storage>::Hypotheses Hypotheses;
+
+ public:
+ /** \brief RANSAC (RAndom SAmple Consensus) main constructor
+ * \param model a Sample Consensus model
+ */
+ RandomSampleConsensus (const SampleConsensusModelPtr &model) :
+ SampleConsensus<Storage> (model)
+ {
+ // Maximum number of trials before we give up.
+ max_iterations_ = 10000;
+ }
+
+ /** \brief RANSAC (RAndom SAmple Consensus) main constructor
+ * \param model a Sample Consensus model
+ * \param threshold distance to model threshold
+ */
+ RandomSampleConsensus (const SampleConsensusModelPtr &model, float threshold) :
+ SampleConsensus<Storage> (model, threshold)
+ {
+ // Maximum number of trials before we give up.
+ max_iterations_ = 10000;
+ }
+
+ /** \brief Compute the actual model and find the inliers
+ * \param debug_verbosity_level enable/disable on-screen debug
+ * information and set the verbosity level
+ */
+ bool
+ computeModel (int debug_verbosity_level = 0);
+ };
+ } // namespace
+} // namespace
+
+#endif //#ifndef PCL_CUDA_SAMPLE_CONSENSUS_RANSAC_H_
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_CUDA_SAMPLE_CONSENSUS_H_
+#define PCL_CUDA_SAMPLE_CONSENSUS_H_
+
+#include <pcl/cuda/sample_consensus/sac_model.h>
+#include <pcl/cuda/point_cloud.h>
+#include <cfloat>
+//#include <set>
+
+namespace pcl
+{
+ namespace cuda
+ {
+ template <template <typename> class Storage>
+ class SampleConsensus
+ {
+ typedef typename SampleConsensusModel<Storage>::Ptr SampleConsensusModelPtr;
+ typedef typename SampleConsensusModel<Storage>::Hypotheses Hypotheses;
+
+ typedef typename SampleConsensusModel<Storage>::Indices Indices;
+ typedef typename SampleConsensusModel<Storage>::IndicesPtr IndicesPtr;
+ typedef typename SampleConsensusModel<Storage>::IndicesConstPtr IndicesConstPtr;
+
+ private:
+ /** \brief Constructor for base SAC. */
+ SampleConsensus () {};
+
+ public:
+ typedef typename Storage<float>::type Coefficients;
+ typedef boost::shared_ptr <Coefficients> CoefficientsPtr;
+ typedef boost::shared_ptr <const Coefficients> CoefficientsConstPtr;
+
+ typedef boost::shared_ptr<SampleConsensus> Ptr;
+ typedef boost::shared_ptr<const SampleConsensus> ConstPtr;
+
+ /** \brief Constructor for base SAC.
+ * \param model a Sample Consensus model
+ */
+ SampleConsensus (const SampleConsensusModelPtr &model) :
+ sac_model_(model), probability_ (0.99), iterations_ (0), threshold_ (DBL_MAX),
+ max_iterations_ (1000)
+ {};
+
+ /** \brief Constructor for base SAC.
+ * \param model a Sample Consensus model
+ * \param threshold distance to model threshold
+ */
+ SampleConsensus (const SampleConsensusModelPtr &model, float threshold) :
+ sac_model_(model), probability_ (0.99), iterations_ (0), threshold_ (threshold),
+ max_iterations_ (1000)
+ {};
+
+ /** \brief Destructor for base SAC. */
+ virtual ~SampleConsensus () {};
+
+ /** \brief Set the distance to model threshold.
+ * \param threshold distance to model threshold
+ */
+ inline void
+ setDistanceThreshold (float threshold) { threshold_ = threshold; }
+
+ /** \brief Get the distance to model threshold, as set by the user. */
+ inline float
+ getDistanceThreshold () { return (threshold_); }
+
+ /** \brief Set the maximum number of iterations.
+ * \param max_iterations maximum number of iterations
+ */
+ inline void
+ setMaxIterations (int max_iterations) { max_iterations_ = max_iterations; }
+
+ /** \brief Get the maximum number of iterations, as set by the user. */
+ inline int
+ getMaxIterations () { return (max_iterations_); }
+
+ /** \brief Set the desired probability of choosing at least one sample free from
+ * outliers.
+ * \param probability the desired probability of choosing at least one sample free
+ * from outliers
+ * \note internally, the probability is set to 99% (0.99) by default.
+ */
+ inline void
+ setProbability (float probability) { probability_ = probability; }
+
+ /** \brief Obtain the probability of choosing at least one sample free from outliers,
+ * as set by the user.
+ */
+ inline float
+ getProbability () { return (probability_); }
+
+ /** \brief Compute the actual model. Pure virtual. */
+ virtual bool
+ computeModel (int debug_verbosity_level = 0) = 0;
+
+ /* \brief Get a set of randomly selected indices.
+ * \param indices the input indices vector
+ * \param nr_samples the desired number of point indices to randomly select
+ * \param indices_subset the resultant output set of randomly selected indices
+ */
+/* inline void
+ getRandomSamples (const IndicesPtr &indices, size_t nr_samples,
+ std::set<int> &indices_subset)
+ {
+ indices_subset.clear ();
+ while (indices_subset.size () < nr_samples)
+ indices_subset.insert ((*indices)[(int) (indices->size () * (rand () / (RAND_MAX + 1.0)))]);
+ }*/
+
+ /** \brief Return the best model found so far.
+ * \param model the resultant model
+ */
+ inline void
+ getModel (Indices &model) { model = model_; }
+
+ /** \brief Return the best set of inliers found so far for this model.
+ */
+ // inline void
+ // getInliers (std::vector<int> &inliers) { inliers = inliers_; }
+ inline IndicesPtr
+ getInliers () { return inliers_; }
+
+ // inline void
+ // getInliersStencil (Indices &inliers) { inliers = inliers_stencil_; }
+ inline IndicesPtr
+ getInliersStencil () { return inliers_stencil_; }
+
+ /** \brief Return the model coefficients of the best model found so far.
+ * \param model_coefficients the resultant model coefficients
+ */
+ inline void
+ getModelCoefficients (Coefficients &model_coefficients)
+ {
+ model_coefficients = model_coefficients_;
+ }
+
+ protected:
+ /** \brief The underlying data model used (what is it that we attempt to search for). */
+ SampleConsensusModelPtr sac_model_;
+
+ /** \brief The model found after the last computeModel () as point cloud indices. */
+ Indices model_;
+
+ /** \brief The indices of the points that were chosen as inliers after the last call. */
+ IndicesPtr inliers_;
+ IndicesPtr inliers_stencil_;
+
+ /** \brief The coefficients of our model computed directly from the model found. */
+ Coefficients model_coefficients_;
+
+ /** \brief Desired probability of choosing at least one sample free from outliers. */
+ float probability_;
+
+ /** \brief Total number of internal loop iterations that we've done so far. */
+ int iterations_;
+
+ /** \brief Distance to model threshold. */
+ float threshold_;
+
+ /** \brief Maximum number of iterations before giving up. */
+ int max_iterations_;
+ };
+ } // namespace
+} // namespace
+
+#endif //#ifndef PCL_CUDA_SAMPLE_CONSENSUS_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_CUDA_SAMPLE_CONSENSUS_MODEL_H_
+#define PCL_CUDA_SAMPLE_CONSENSUS_MODEL_H_
+
+#include <float.h>
+#include <thrust/sequence.h>
+#include <thrust/count.h>
+#include <thrust/remove.h>
+#include <pcl/cuda/point_cloud.h>
+#include <thrust/random/linear_congruential_engine.h>
+
+#include <pcl/pcl_exports.h>
+
+namespace pcl
+{
+ namespace cuda
+ {
+ // Forward declaration
+ //template<class T> class ProgressiveSampleConsensus;
+
+ /** \brief Check if a certain tuple is a point inlier. */
+ struct DeleteIndices
+ {
+ template <typename Tuple> __inline__ __host__ __device__ int
+ operator () (const Tuple &t);
+ };
+
+ /** \brief Check if a certain tuple is a point inlier. */
+ struct isInlier
+ {
+ __inline__ __host__ __device__ bool
+ operator()(int x) { return (x != -1); }
+ };
+
+ struct isNaNPoint
+ {
+ __inline__ __host__ __device__ bool
+ operator ()(PointXYZRGB pt)
+ {
+#ifdef __CUDACC__
+ return (isnan (pt.x) | isnan (pt.y) | isnan (pt.z)) == 1;
+#else
+ return (pcl_isnan (pt.x) | pcl_isnan (pt.y) | pcl_isnan (pt.z)) == 1;
+#endif
+ }
+ };
+
+ /** \brief @b SampleConsensusModel represents the base model class. All sample consensus models must inherit from
+ * this class.
+ */
+ template <template <typename> class Storage>
+ class SampleConsensusModel
+ {
+ public:
+ typedef PointCloudAOS<Storage> PointCloud;
+ typedef typename PointCloud::Ptr PointCloudPtr;
+ typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+ typedef boost::shared_ptr<SampleConsensusModel> Ptr;
+ typedef boost::shared_ptr<const SampleConsensusModel> ConstPtr;
+
+ typedef typename Storage<int>::type Indices;
+ typedef boost::shared_ptr<typename Storage<int>::type> IndicesPtr;
+ typedef boost::shared_ptr<const typename Storage<int>::type> IndicesConstPtr;
+
+ typedef typename Storage<float>::type Coefficients;
+ typedef boost::shared_ptr <Coefficients> CoefficientsPtr;
+ typedef boost::shared_ptr <const Coefficients> CoefficientsConstPtr;
+
+ typedef typename Storage<float4>::type Hypotheses;
+ //TODO: should be vector<int> instead of int. but currently, only 1point plane model supports this
+ typedef typename Storage<int>::type Samples;
+
+ private:
+ /** \brief Empty constructor for base SampleConsensusModel. */
+ SampleConsensusModel () : radius_min_ (-FLT_MAX), radius_max_ (FLT_MAX)
+ {};
+
+ public:
+ /** \brief Constructor for base SampleConsensusModel.
+ * \param cloud the input point cloud dataset
+ */
+ SampleConsensusModel (const PointCloudConstPtr &cloud) :
+ radius_min_ (-FLT_MAX), radius_max_ (FLT_MAX)
+ {
+ // Sets the input cloud and creates a vector of "fake" indices
+ setInputCloud (cloud);
+ }
+
+ /* \brief Constructor for base SampleConsensusModel.
+ * \param cloud the input point cloud dataset
+ * \param indices a vector of point indices to be used from \a cloud
+ */
+ /* SampleConsensusModel (const PointCloudConstPtr &cloud, const std::vector<int> &indices) :
+ input_ (cloud),
+ indices_ (boost::make_shared <std::vector<int> > (indices)),
+ radius_min_ (-DBL_MAX), radius_max_ (DBL_MAX)
+
+ {
+ if (indices_->size () > input_->points.size ())
+ {
+ ROS_ERROR ("[pcl::SampleConsensusModel] Invalid index vector given with size %lu while the input PointCloud has size %lu!", (unsigned long) indices_->size (), (unsigned long) input_->points.size ());
+ indices_->clear ();
+ }
+ };*/
+
+ /** \brief Destructor for base SampleConsensusModel. */
+ virtual ~SampleConsensusModel () {};
+
+ /** \brief Get a set of random data samples and return them as point
+ * indices. Pure virtual.
+ * \param iterations the internal number of iterations used by SAC methods
+ * \param samples the resultant model samples, <b>stored on the device</b>
+ */
+ virtual void
+ getSamples (int &iterations, Indices &samples) = 0;
+
+ /** \brief Check whether the given index samples can form a valid model,
+ * compute the model coefficients from these samples and store them
+ * in model_coefficients. Pure virtual.
+ * \param samples the point indices found as possible good candidates
+ * for creating a valid model, <b>stored on the device</b>
+ * \param model_coefficients the computed model coefficients
+ */
+ virtual bool
+ computeModelCoefficients (const Indices &samples, Coefficients &model_coefficients) = 0;
+
+ virtual bool
+ generateModelHypotheses (Hypotheses &h, int max_iterations) = 0;
+
+ virtual bool
+ generateModelHypotheses (Hypotheses &h, Samples &s, int max_iterations) = 0;
+
+ virtual bool
+ isSampleInlier (IndicesPtr &inliers_stencil, Samples &samples, unsigned int &i)
+ {return ((*inliers_stencil)[samples[i]] != -1);};
+
+ /* \brief Recompute the model coefficients using the given inlier set
+ * and return them to the user. Pure virtual.
+ *
+ * @note: these are the coefficients of the model after refinement
+ * (e.g., after a least-squares optimization)
+ *
+ * \param inliers the data inliers supporting the model
+ * \param model_coefficients the initial guess for the model coefficients
+ * \param optimized_coefficients the resultant recomputed coefficients
+ * after non-linear optimization
+ */
+ // virtual void
+ // optimizeModelCoefficients (const std::vector<int> &inliers,
+ // const Eigen::VectorXf &model_coefficients,
+ // Eigen::VectorXf &optimized_coefficients) = 0;
+
+ /* \brief Compute all distances from the cloud data to a given model. Pure virtual.
+ * \param model_coefficients the coefficients of a model that we need to
+ * compute distances to
+ * \param distances the resultant estimated distances
+ */
+ // virtual void
+ // getDistancesToModel (const Eigen::VectorXf &model_coefficients,
+ // std::vector<float> &distances) = 0;
+
+ /** \brief Select all the points which respect the given model
+ * coefficients as inliers. Pure virtual.
+ *
+ * \param model_coefficients the coefficients of a model that we need to
+ * compute distances to
+ * \param threshold a maximum admissible distance threshold for
+ * determining the inliers from the outliers
+ * \param inliers the resultant model inliers
+ * \param inliers_stencil
+ */
+ virtual int
+ selectWithinDistance (const Coefficients &model_coefficients,
+ float threshold,
+ IndicesPtr &inliers, IndicesPtr &inliers_stencil) = 0;
+ virtual int
+ selectWithinDistance (const Hypotheses &h, int idx,
+ float threshold,
+ IndicesPtr &inliers, IndicesPtr &inliers_stencil) = 0;
+ virtual int
+ selectWithinDistance (Hypotheses &h, int idx,
+ float threshold,
+ IndicesPtr &inliers_stencil,
+ float3 ¢roid) = 0;
+
+ virtual int
+ countWithinDistance (const Coefficients &model_coefficients, float threshold) = 0;
+
+ virtual int
+ countWithinDistance (const Hypotheses &h, int idx, float threshold) = 0;
+
+ int
+ deleteIndices (const IndicesPtr &indices_stencil );
+ int
+ deleteIndices (const Hypotheses &h, int idx, IndicesPtr &inliers, const IndicesPtr &inliers_delete);
+
+ /* \brief Create a new point cloud with inliers projected onto the model. Pure virtual.
+ * \param inliers the data inliers that we want to project on the model
+ * \param model_coefficients the coefficients of a model
+ * \param projected_points the resultant projected points
+ * \param copy_data_fields set to true (default) if we want the \a
+ * projected_points cloud to be an exact copy of the input dataset minus
+ * the point projections on the plane model
+ */
+ // virtual void
+ // projectPoints (const std::vector<int> &inliers,
+ // const Eigen::VectorXf &model_coefficients,
+ // PointCloud &projected_points,
+ // bool copy_data_fields = true) = 0;
+
+ /* \brief Verify whether a subset of indices verifies a given set of
+ * model coefficients. Pure virtual.
+ *
+ * \param indices the data indices that need to be tested against the model
+ * \param model_coefficients the set of model coefficients
+ * \param threshold a maximum admissible distance threshold for
+ * determining the inliers from the outliers
+ */
+ // virtual bool
+ // doSamplesVerifyModel (const std::set<int> &indices,
+ // const Eigen::VectorXf &model_coefficients,
+ // float threshold) = 0;
+
+ /** \brief Provide a pointer to the input dataset
+ * \param cloud the const boost shared pointer to a PointCloud message
+ */
+ virtual void
+ setInputCloud (const PointCloudConstPtr &cloud);
+
+ /** \brief Get a pointer to the input point cloud dataset. */
+ inline PointCloudConstPtr
+ getInputCloud () const { return (input_); }
+
+ /* \brief Provide a pointer to the vector of indices that represents the input data.
+ * \param indices a pointer to the vector of indices that represents the input data.
+ */
+ // inline void
+ // setIndices (const IndicesPtr &indices) { indices_ = indices; }
+
+ /* \brief Provide the vector of indices that represents the input data.
+ * \param indices the vector of indices that represents the input data.
+ */
+ // inline void
+ // setIndices (std::vector<int> &indices)
+ // {
+ // indices_ = boost::make_shared <std::vector<int> > (indices);
+ // }
+
+ /** \brief Get a pointer to the vector of indices used. */
+ inline IndicesPtr
+ getIndices () const
+ {
+ if (nr_indices_in_stencil_ != indices_->size())
+ {
+ typename Indices::iterator last = thrust::remove_copy (indices_stencil_->begin (), indices_stencil_->end (), indices_->begin (), -1);
+ indices_->erase (last, indices_->end ());
+ }
+
+ return (indices_);
+ }
+
+ /* \brief Return an unique id for each type of model employed. */
+ // virtual SacModel
+ // getModelType () const = 0;
+
+ /* \brief Return the size of a sample from which a model is computed */
+ // inline unsigned int
+ // getSampleSize () const { return SAC_SAMPLE_SIZE.at (getModelType ()); }
+
+ /** \brief Set the minimum and maximum allowable radius limits for the
+ * model (applicable to models that estimate a radius)
+ * \param min_radius the minimum radius model
+ * \param max_radius the maximum radius model
+ * \todo change this to set limits on the entire model
+ */
+ inline void
+ setRadiusLimits (float min_radius, float max_radius)
+ {
+ radius_min_ = min_radius;
+ radius_max_ = max_radius;
+ }
+
+ /** \brief Get the minimum and maximum allowable radius limits for the
+ * model as set by the user.
+ *
+ * \param min_radius the resultant minimum radius model
+ * \param max_radius the resultant maximum radius model
+ */
+ inline void
+ getRadiusLimits (float &min_radius, float &max_radius)
+ {
+ min_radius = radius_min_;
+ max_radius = radius_max_;
+ }
+
+ // friend class ProgressiveSampleConsensus<PointT>;
+
+ inline boost::shared_ptr<typename Storage<float4>::type>
+ getNormals () { return (normals_); }
+
+ inline
+ void setNormals (boost::shared_ptr<typename Storage<float4>::type> normals) { normals_ = normals; }
+
+
+ protected:
+ /* \brief Check whether a model is valid given the user constraints.
+ * \param model_coefficients the set of model coefficients
+ */
+ // virtual inline bool
+ // isModelValid (const Eigen::VectorXf &model_coefficients) = 0;
+
+ /** \brief A boost shared pointer to the point cloud data array. */
+ PointCloudConstPtr input_;
+ boost::shared_ptr<typename Storage<float4>::type> normals_;
+
+ /** \brief A pointer to the vector of point indices to use. */
+ IndicesPtr indices_;
+ /** \brief A pointer to the vector of point indices (stencil) to use. */
+ IndicesPtr indices_stencil_;
+ /** \brief number of indices left in indices_stencil_ */
+ unsigned int nr_indices_in_stencil_;
+
+ /** \brief The minimum and maximum radius limits for the model.
+ * Applicable to all models that estimate a radius.
+ */
+ float radius_min_, radius_max_;
+
+ /** \brief Linear-Congruent random number generator engine. */
+ thrust::minstd_rand rngl_;
+ };
+
+ /* \brief @b SampleConsensusModelFromNormals represents the base model class
+ * for models that require the use of surface normals for estimation.
+ */
+ // template <typename PointT, typename PointNT>
+ // class SampleConsensusModelFromNormals
+ // {
+ // public:
+ // typedef typename pcl::PointCloud<PointNT>::ConstPtr PointCloudNConstPtr;
+ // typedef typename pcl::PointCloud<PointNT>::Ptr PointCloudNPtr;
+ //
+ // typedef boost::shared_ptr<SampleConsensusModelFromNormals> Ptr;
+ // typedef boost::shared_ptr<const SampleConsensusModelFromNormals> ConstPtr;
+ //
+ // /* \brief Empty constructor for base SampleConsensusModelFromNormals. */
+ // SampleConsensusModelFromNormals () : normal_distance_weight_ (0.0) {};
+ //
+ // /* \brief Set the normal angular distance weight.
+ // * \param w the relative weight (between 0 and 1) to give to the angular
+ // * distance (0 to pi/2) between point normals and the plane normal.
+ // * (The Euclidean distance will have weight 1-w.)
+ // */
+ // inline void
+ // setNormalDistanceWeight (float w) { normal_distance_weight_ = w; }
+ //
+ // /* \brief Get the normal angular distance weight. */
+ // inline float
+ // getNormalDistanceWeight () { return (normal_distance_weight_); }
+ //
+ // /* \brief Provide a pointer to the input dataset that contains the point
+ // * normals of the XYZ dataset.
+ // *
+ // * \param normals the const boost shared pointer to a PointCloud message
+ // */
+ // inline void
+ // setInputNormals (const PointCloudNConstPtr &normals) { normals_ = normals; }
+ //
+ // /* \brief Get a pointer to the normals of the input XYZ point cloud dataset. */
+ // inline PointCloudNConstPtr
+ // getInputNormals () { return (normals_); }
+ //
+ // protected:
+ // /* \brief The relative weight (between 0 and 1) to give to the angular
+ // * distance (0 to pi/2) between point normals and the plane normal.
+ // */
+ // float normal_distance_weight_;
+ //
+ // /* \brief A pointer to the input dataset that contains the point normals
+ // * of the XYZ dataset.
+ // */
+ // PointCloudNConstPtr normals_;
+ // };
+ } // namespace_
+} // namespace_
+
+#endif //#ifndef PCL_CUDA_SAMPLE_CONSENSUS_MODEL_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Nico Blodow (blodow@cs.tum.edu)
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_CUDA_SAMPLE_CONSENSUS_MODEL_1POINT_PLANE_H_
+#define PCL_CUDA_SAMPLE_CONSENSUS_MODEL_1POINT_PLANE_H_
+
+#include <pcl/cuda/sample_consensus/sac_model.h>
+#include <thrust/random.h>
+
+namespace pcl
+{
+ namespace cuda
+ {
+ /** \brief Check if a certain tuple is a point inlier. */
+ struct CountPlanarInlier
+ {
+ float4 coefficients;
+ float threshold;
+
+ CountPlanarInlier (float4 coeff, float thresh) :
+ coefficients(coeff), threshold(thresh)
+ {}
+
+ template <typename Tuple> __inline__ __host__ __device__ bool
+ operator () (const Tuple &t);
+ };
+
+ /** \brief Check if a certain tuple is a point inlier. */
+ template <template <typename> class Storage>
+ struct NewCheckPlanarInlier
+ {
+ float4 coefficients;
+ float threshold;
+ const typename Storage<PointXYZRGB>::type &input_;
+
+ NewCheckPlanarInlier (float4 coeff, float thresh, const typename Storage<PointXYZRGB>::type &input) :
+ coefficients(coeff), threshold(thresh), input_(input)
+ {}
+
+ __inline__ __host__ __device__ int
+ operator () (const int &idx);
+ };
+
+ /** \brief Check if a certain tuple is a point inlier. */
+ struct CheckPlanarInlier
+ {
+ float4 coefficients;
+ float threshold;
+
+ CheckPlanarInlier (float4 coeff, float thresh) :
+ coefficients(coeff), threshold(thresh)
+ {}
+
+ template <typename Tuple> __inline__ __host__ __device__ int
+ operator () (const Tuple &t);
+ };
+
+ /** \brief Check if a certain tuple is a point inlier. */
+ struct CheckPlanarInlierIndices
+ {
+ float4 coefficients;
+ float threshold;
+
+ CheckPlanarInlierIndices (float4 coeff, float thresh) :
+ coefficients(coeff), threshold(thresh)
+ {}
+
+ __inline__ __host__ __device__ int
+ operator () (const PointXYZRGB &pt, const int &idx);
+ };
+
+ /** \brief Check if a certain tuple is a point inlier. */
+ struct CheckPlanarInlierKinectNormalIndices
+ {
+ float4 coefficients;
+ float threshold;
+ float angle_threshold;
+
+ CheckPlanarInlierKinectNormalIndices (float4 coeff, float thresh, float angle_thresh) :
+ coefficients(coeff), threshold(thresh), angle_threshold (angle_thresh)
+ {}
+
+ template <typename Tuple> __inline__ __host__ __device__ int
+ operator () (const Tuple &t, const int &idx);
+ };
+
+ /** \brief Check if a certain tuple is a point inlier. */
+ struct CheckPlanarInlierKinectIndices
+ {
+ float4 coefficients;
+ float threshold;
+ float angle_threshold;
+
+ CheckPlanarInlierKinectIndices (float4 coeff, float thresh, float angle_thresh) :
+ coefficients(coeff), threshold(thresh), angle_threshold (angle_thresh)
+ {}
+
+ __inline__ __host__ __device__ int
+ operator () (const PointXYZRGB &pt, const int &idx);
+ };
+
+ /** \brief Check if a certain tuple is a point inlier. */
+ struct CheckPlanarInlierNormalIndices
+ {
+ float4 coefficients;
+ float threshold;
+ float angle_threshold;
+
+ CheckPlanarInlierNormalIndices (float4 coeff, float thresh, float angle_thresh) :
+ coefficients(coeff), threshold(thresh), angle_threshold (angle_thresh)
+ {}
+
+ template <typename Tuple>
+ __inline__ __host__ __device__ int
+ operator () (const Tuple &pt, const int &idx);
+ };
+
+ ////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b SampleConsensusModel1PointPlane defines a model for 3D plane segmentation.
+ */
+ template <template <typename> class Storage>
+ class SampleConsensusModel1PointPlane : public SampleConsensusModel<Storage>
+ {
+ public:
+ using SampleConsensusModel<Storage>::input_;
+ using SampleConsensusModel<Storage>::normals_;
+ using SampleConsensusModel<Storage>::indices_;
+ using SampleConsensusModel<Storage>::indices_stencil_;
+ using SampleConsensusModel<Storage>::rngl_;
+
+ typedef typename SampleConsensusModel<Storage>::PointCloud PointCloud;
+ typedef typename PointCloud::Ptr PointCloudPtr;
+ typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+ typedef typename SampleConsensusModel<Storage>::Indices Indices;
+ typedef typename SampleConsensusModel<Storage>::IndicesPtr IndicesPtr;
+ typedef typename SampleConsensusModel<Storage>::IndicesConstPtr IndicesConstPtr;
+
+ typedef typename SampleConsensusModel<Storage>::Coefficients Coefficients;
+ typedef typename SampleConsensusModel<Storage>::Hypotheses Hypotheses;
+ typedef typename SampleConsensusModel<Storage>::Samples Samples;
+
+
+ typedef boost::shared_ptr<SampleConsensusModel1PointPlane> Ptr;
+
+ /** \brief Constructor for base SampleConsensusModel1PointPlane.
+ * \param cloud the input point cloud dataset
+ */
+ SampleConsensusModel1PointPlane (const PointCloudConstPtr &cloud);
+
+ /** \brief Get 3 random non-collinear points as data samples and return them as point indices.
+ * \param iterations the internal number of iterations used by SAC methods
+ * \param samples the resultant model samples
+ * \note assumes unique points!
+ */
+ void
+ getSamples (int &iterations, Indices &samples);
+
+ /** \brief Check whether the given index samples can form a valid plane model, compute the model coefficients from
+ * these samples and store them in model_coefficients. The plane coefficients are:
+ * a, b, c, d (ax+by+cz+d=0)
+ * \param samples the point indices found as possible good candidates for creating a valid model
+ * \param model_coefficients the resultant model coefficients
+ */
+ bool
+ computeModelCoefficients (const Indices &samples, Coefficients &model_coefficients);
+
+ bool
+ generateModelHypotheses (Hypotheses &h, int max_iterations);
+
+ bool
+ generateModelHypotheses (Hypotheses &h, Samples &s, int max_iterations);
+
+ /** \brief Select all the points which respect the given model coefficients as inliers.
+ * \param model_coefficients the coefficients of a plane model that we need to
+ * compute distances to
+ * \param threshold a maximum admissible distance threshold for determining the
+ * inliers from the outliers
+ * \param inliers the resultant model inliers
+ * \param inliers_stencil
+ */
+ int
+ selectWithinDistance (const Coefficients &model_coefficients,
+ float threshold, IndicesPtr &inliers, IndicesPtr &inliers_stencil);
+ int
+ selectWithinDistance (const Hypotheses &h, int idx,
+ float threshold,
+ IndicesPtr &inliers, IndicesPtr &inliers_stencil);
+ int
+ selectWithinDistance (Hypotheses &h, int idx,
+ float threshold,
+ IndicesPtr &inliers_stencil,
+ float3 ¢roid);
+ int
+ countWithinDistance (const Coefficients &model_coefficients, float threshold);
+
+ int
+ countWithinDistance (const Hypotheses &h, int idx, float threshold);
+
+ // private:
+ // /** \brief Define the maximum number of iterations for collinearity checks */
+ const static int MAX_ITERATIONS_COLLINEAR = 1000;
+ };
+
+ /** \brief Check if a certain tuple is a point inlier. */
+ template <template <typename> class Storage>
+ struct Create1PointPlaneHypothesis
+ {
+ typedef typename SampleConsensusModel<Storage>::PointCloud PointCloud;
+ typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+ typedef typename SampleConsensusModel<Storage>::Indices Indices;
+ typedef typename SampleConsensusModel<Storage>::IndicesConstPtr IndicesConstPtr;
+
+ const PointXYZRGB *input;
+ const int *indices;
+ int nr_indices;
+ float bad_value;
+
+ Create1PointPlaneHypothesis (const PointXYZRGB *_input, const int *_indices, int _nr_indices, float bad) :
+ input(_input), indices(_indices), nr_indices(_nr_indices), bad_value(bad)
+ {}
+
+ //template <typename Tuple>
+ __inline__ __host__ __device__ float4
+ //operator () (const Tuple &t);
+ operator () (int t);
+ };
+
+ /** \brief Check if a certain tuple is a point inlier. */
+ template <template <typename> class Storage>
+ struct Create1PointPlaneSampleHypothesis
+ {
+ typedef typename SampleConsensusModel<Storage>::PointCloud PointCloud;
+ typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+ typedef typename SampleConsensusModel<Storage>::Indices Indices;
+ typedef typename SampleConsensusModel<Storage>::IndicesConstPtr IndicesConstPtr;
+
+ const PointXYZRGB *input;
+ const float4 *normals_;
+ const int *indices;
+ int width_;
+ int height_;
+ int nr_indices;
+ float bad_value;
+ thrust::default_random_engine rng;
+
+ Create1PointPlaneSampleHypothesis (const PointXYZRGB *_input, const float4* normals, const int *_indices, int width, int height, int _nr_indices, float bad) :
+ input(_input), normals_(normals), indices(_indices), width_(width), height_(height), nr_indices(_nr_indices), bad_value(bad)
+ {
+ }
+
+ //template <typename Tuple>
+ __inline__ __host__ __device__ thrust::tuple<int,float4>
+ //operator () (const Tuple &t);
+ operator () (int t);
+ };
+
+ struct parallel_random_generator
+ {
+
+ __inline__ __host__ __device__
+ parallel_random_generator(unsigned int seed)
+ {
+ m_seed = seed;
+ }
+
+ __inline__ __host__ __device__
+ unsigned int operator()(const unsigned int n) const
+ {
+ thrust::default_random_engine rng(m_seed);
+ // discard n numbers to avoid correlation
+ rng.discard(n);
+ // return a random number
+ return rng();
+ }
+ unsigned int m_seed;
+ };
+
+ } // namespace
+} // namespace
+
+#endif //#ifndef PCL_CUDA_SAMPLE_CONSENSUS_MODEL_PLANE_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_CUDA_SAMPLE_CONSENSUS_MODEL_PLANE_H_
+#define PCL_CUDA_SAMPLE_CONSENSUS_MODEL_PLANE_H_
+
+#include <pcl/cuda/sample_consensus/sac_model.h>
+
+#include <thrust/random.h>
+
+namespace pcl
+{
+ namespace cuda
+ {
+ /** \brief Check if a certain tuple is a point inlier. */
+ struct CountPlanarInlier
+ {
+ float4 coefficients;
+ float threshold;
+
+ CountPlanarInlier (float4 coeff, float thresh) :
+ coefficients(coeff), threshold(thresh)
+ {}
+
+ template <typename Tuple> __inline__ __host__ __device__ bool
+ operator () (const Tuple &t);
+ };
+
+ /** \brief Check if a certain tuple is a point inlier. */
+ struct CheckPlanarInlier
+ {
+ float4 coefficients;
+ float threshold;
+
+ CheckPlanarInlier (float4 coeff, float thresh) :
+ coefficients(coeff), threshold(thresh)
+ {}
+
+ template <typename Tuple> __inline__ __host__ __device__ int
+ operator () (const Tuple &t);
+ };
+
+ ////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b SampleConsensusModelPlane defines a model for 3D plane segmentation.
+ */
+ template <template <typename> class Storage>
+ class SampleConsensusModelPlane : public SampleConsensusModel<Storage>
+ {
+ public:
+ using SampleConsensusModel<Storage>::input_;
+ using SampleConsensusModel<Storage>::indices_;
+ using SampleConsensusModel<Storage>::rngl_;
+
+ typedef typename SampleConsensusModel<Storage>::PointCloud PointCloud;
+ typedef typename PointCloud::Ptr PointCloudPtr;
+ typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+ typedef typename SampleConsensusModel<Storage>::Indices Indices;
+ typedef typename SampleConsensusModel<Storage>::IndicesPtr IndicesPtr;
+ typedef typename SampleConsensusModel<Storage>::IndicesConstPtr IndicesConstPtr;
+
+ typedef typename SampleConsensusModel<Storage>::Coefficients Coefficients;
+ typedef typename SampleConsensusModel<Storage>::Hypotheses Hypotheses;
+ typedef typename SampleConsensusModel<Storage>::Samples Samples;
+
+ typedef boost::shared_ptr<SampleConsensusModelPlane> Ptr;
+
+ /** \brief Constructor for base SampleConsensusModelPlane.
+ * \param cloud the input point cloud dataset
+ */
+ SampleConsensusModelPlane (const PointCloudConstPtr &cloud);
+
+ /* \brief Constructor for base SampleConsensusModelPlane.
+ * \param cloud the input point cloud dataset
+ * \param indices a vector of point indices to be used from \a cloud
+ */
+ // SampleConsensusModelPlane (const PointCloudConstPtr &cloud, const std::vector<int> &indices) : SampleConsensusModel<PointT> (cloud, indices) {};
+
+ /** \brief Get 3 random non-collinear points as data samples and return them as point indices.
+ * \param iterations the internal number of iterations used by SAC methods
+ * \param samples the resultant model samples
+ * \note assumes unique points!
+ */
+ void
+ getSamples (int &iterations, Indices &samples);
+
+ /** \brief Check whether the given index samples can form a valid plane model, compute the model coefficients from
+ * these samples and store them in model_coefficients. The plane coefficients are:
+ * a, b, c, d (ax+by+cz+d=0)
+ * \param samples the point indices found as possible good candidates for creating a valid model
+ * \param model_coefficients the resultant model coefficients
+ */
+ bool
+ computeModelCoefficients (const Indices &samples, Coefficients &model_coefficients);
+
+ bool
+ generateModelHypotheses (Hypotheses &h, int max_iterations);
+
+ virtual bool
+ generateModelHypotheses (Hypotheses &h, Samples &s, int max_iterations)
+ {
+ // TODO: hack.. Samples should be vector<int>, not int..
+ return generateModelHypotheses (h, max_iterations);
+ };
+
+ /* \brief Compute all distances from the cloud data to a given plane model.
+ * \param model_coefficients the coefficients of a plane model that we need to compute distances to
+ * \param distances the resultant estimated distances
+ */
+ // void
+ // getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<float> &distances);
+
+ /** \brief Select all the points which respect the given model coefficients as inliers.
+ * \param model_coefficients the coefficients of a plane model that we need to
+ * compute distances to
+ * \param threshold a maximum admissible distance threshold for determining the
+ * inliers from the outliers
+ * \param inliers the resultant model inliers
+ * \param inliers_stencil
+ */
+ int
+ selectWithinDistance (const Coefficients &model_coefficients,
+ float threshold, IndicesPtr &inliers, IndicesPtr &inliers_stencil);
+ int
+ selectWithinDistance (const Hypotheses &h, int idx,
+ float threshold,
+ IndicesPtr &inliers, IndicesPtr &inliers_stencil);
+ int
+ selectWithinDistance (Hypotheses &h, int idx,
+ float threshold,
+ IndicesPtr &inliers_stencil,
+ float3 ¢roid);
+
+ int
+ countWithinDistance (const Coefficients &model_coefficients, float threshold);
+
+ int
+ countWithinDistance (const Hypotheses &h, int idx, float threshold);
+
+ /* \brief Recompute the plane coefficients using the given inlier set and return them to the user.
+ * @note: these are the coefficients of the plane model after refinement (eg. after SVD)
+ * \param inliers the data inliers found as supporting the model
+ * \param model_coefficients the initial guess for the model coefficients
+ * \param optimized_coefficients the resultant recomputed coefficients after non-linear optimization
+ */
+ // void
+ // optimizeModelCoefficients (const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients);
+
+ /* \brief Create a new point cloud with inliers projected onto the plane model.
+ * \param inliers the data inliers that we want to project on the plane model
+ * \param model_coefficients the *normalized* coefficients of a plane model
+ * \param projected_points the resultant projected points
+ * \param copy_data_fields set to true if we need to copy the other data fields
+ */
+ // void
+ // projectPoints (const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields = true);
+
+ /* \brief Verify whether a subset of indices verifies the given plane model coefficients.
+ * \param indices the data indices that need to be tested against the plane model
+ * \param model_coefficients the plane model coefficients
+ * \param threshold a maximum admissible distance threshold for determining the inliers from the outliers
+ */
+ // bool
+ // doSamplesVerifyModel (const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, float threshold);
+
+ /* \brief Return an unique id for this model (SACMODEL_PLANE). */
+ // inline pcl::SacModel getModelType () const { return (SACMODEL_PLANE); }
+
+ // protected:
+ /* \brief Check whether a model is valid given the user constraints.
+ * \param model_coefficients the set of model coefficients
+ */
+ // inline bool
+ // isModelValid (const Eigen::VectorXf &model_coefficients)
+ // {
+ // // Needs a valid model coefficients
+ // if (model_coefficients.size () != 4)
+ // {
+ // ROS_ERROR ("[pcl::SampleConsensusModelPlane::isModelValid] Invalid number of model coefficients given (%lu)!", (unsigned long) model_coefficients.size ());
+ // return (false);
+ // }
+ // return (true);
+ // }
+
+ // private:
+ /* \brief Define the maximum number of iterations for collinearity checks */
+ const static int MAX_ITERATIONS_COLLINEAR = 1000;
+ };
+
+ /** \brief Check if a certain tuple is a point inlier. */
+ template <template <typename> class Storage>
+ struct CreatePlaneHypothesis
+ {
+ typedef typename SampleConsensusModel<Storage>::PointCloud PointCloud;
+ typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+ typedef typename SampleConsensusModel<Storage>::Indices Indices;
+ typedef typename SampleConsensusModel<Storage>::IndicesPtr IndicesPtr;
+ typedef typename SampleConsensusModel<Storage>::IndicesConstPtr IndicesConstPtr;
+
+ const PointXYZRGB *input;
+ const int *indices;
+ int nr_indices;
+ float bad_value;
+
+ CreatePlaneHypothesis (const PointXYZRGB *_input, const int *_indices, int _nr_indices, float bad) :
+ input(_input), indices(_indices), nr_indices(_nr_indices), bad_value(bad)
+ {}
+
+ //template <typename Tuple>
+ __inline__ __host__ __device__ float4
+ //operator () (const Tuple &t);
+ operator () (int t);
+ };
+
+
+ struct parallel_random_generator
+ {
+
+ __inline__ __host__ __device__
+ parallel_random_generator(unsigned int seed)
+ {
+ m_seed = seed;
+ }
+
+ __inline__ __host__ __device__
+ unsigned int operator()(const unsigned int n) const
+ {
+ thrust::default_random_engine rng(m_seed);
+ // discard n numbers to avoid correlation
+ rng.discard(n);
+ // return a random number
+ return rng();
+ }
+ unsigned int m_seed;
+ };
+
+ } // namespace
+} // namespace
+
+#endif //#ifndef PCL_CUDA_SAMPLE_CONSENSUS_MODEL_PLANE_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <pcl_cuda/sample_consensus/multi_ransac.h>
+#include <pcl_cuda/time_gpu.h>
+#include <stdio.h>
+//CUPRINTF #include "cuPrintf.cu"
+
+int min_nr_in_shape = 5000;
+
+//////////////////////////////////////////////////////////////////////////
+template <template <typename> class Storage> bool
+pcl_cuda::MultiRandomSampleConsensus<Storage>::computeModel (int debug_verbosity_level)
+{
+ // Warn and exit if no threshold was set
+ if (threshold_ == DBL_MAX)
+ {
+ std::cerr << "[pcl_cuda::MultiRandomSampleConsensus::computeModel] No threshold set!" << std::endl;
+ return (false);
+ }
+
+ // compute number of points
+ int nr_points = sac_model_->getIndices ()->size ();
+ int nr_remaining_points = nr_points;
+ //std::cerr << "nr_points = " << nr_points << std::endl;
+ // number of total iterations
+ unsigned int cur_iteration = 0;
+ // number of valid iterations
+ int valid_iterations = 0;
+ // each batch has a vector of plane coefficients (float4)
+ std::vector<Hypotheses> h(max_batches_);
+ std::vector<typename Storage<int>::type> h_samples (max_batches_);
+ std::vector<float3> centroids (max_batches_ * iterations_per_batch_);
+ // current batch number
+ int cur_batch = 0;
+ //// stencil vector that holds the current inliers
+ std::vector<IndicesPtr> hypotheses_inliers_stencils (max_batches_ * iterations_per_batch_);
+ std::vector<int> hypotheses_inlier_count (max_batches_ * iterations_per_batch_);
+ // initialize some things
+ all_inliers_.clear ();
+ all_model_coefficients_.clear ();
+ all_model_centroids_.clear ();
+ int n_inliers_count = 0;
+ int n_best_inliers_count = 0;
+ int good_coeff = -1;
+ float k = max_batches_ * iterations_per_batch_;
+
+ //thrust::host_vector<float3> host_points = sac_model_->getInputCloud()->points;
+ //std::cerr << "Input Points:" << std::endl;
+ //for (unsigned int print_iter = 0; print_iter < nr_points; ++print_iter)
+ //{
+ // std::cerr << print_iter << " : [ "
+ // << host_points[print_iter].x << ", "
+ // << host_points[print_iter].y << ", "
+ // << host_points[print_iter].z << " ]" << std::endl;
+ //}
+
+ ScopeTime t ("ALLLLLLLLLLL");
+ do // multiple models ..
+ {
+ thrust::host_vector<int> host_samples;
+ thrust::host_vector<float4> host_coeffs;
+ // make sure that sac_model_->indices_ only contains remaining point indices
+ sac_model_->getIndices ();
+
+ // generate a new batch of hypotheses
+ {
+ ScopeTime t ("generateModelHypotheses");
+ sac_model_->generateModelHypotheses (h[cur_batch], h_samples[cur_batch], iterations_per_batch_);
+ }
+ host_samples = h_samples[cur_batch];
+ host_coeffs = h[cur_batch];
+
+ if (debug_verbosity_level > 1)
+ {
+ std::cerr << "Samples / Coefficients:" << std::endl;
+ for (unsigned int print_iter = 0; print_iter < iterations_per_batch_; ++print_iter)
+ {
+ std::cerr << host_samples[print_iter] << " : [ "
+ << host_coeffs[print_iter].x << ", "
+ << host_coeffs[print_iter].y << ", "
+ << host_coeffs[print_iter].z << ", "
+ << host_coeffs[print_iter].w << std::endl;
+ }
+ }
+
+ std::vector<bool> hypothesis_valid (max_batches_ * iterations_per_batch_, true);
+
+ // evaluate each hypothesis in this batch
+ {
+ ScopeTime t ("evaluate");
+ for (unsigned int i = 0; i < iterations_per_batch_; i++, cur_iteration ++, valid_iterations ++)
+ {
+ // hypothesis could be invalid because it's initial sample point was inlier previously
+ if (!hypothesis_valid[cur_batch * iterations_per_batch_ + i])
+ {
+ hypotheses_inlier_count[cur_iteration] = 0;
+ valid_iterations --;
+ continue;
+ }
+
+ // compute inliers for each model
+ IndicesPtr inl_stencil;
+ {
+ ScopeTime t ("selectWithinDistance");
+ int d_cur_penalty = 0;
+ n_inliers_count = sac_model_->selectWithinDistance (h[cur_batch], i, threshold_, inl_stencil, centroids[cur_iteration], d_cur_penalty);
+ }
+ // store inliers and inlier count
+ if (n_inliers_count < min_nr_in_shape)
+ {
+ n_inliers_count = 0;
+ inl_stencil.reset (); // release stencil
+ hypothesis_valid[cur_batch * iterations_per_batch_ + i] = false;
+ }
+
+ hypotheses_inliers_stencils[cur_iteration] = inl_stencil;
+ hypotheses_inlier_count[cur_iteration] = n_inliers_count;
+
+ // Better match ?
+ if (n_inliers_count > n_best_inliers_count)
+ {
+ n_best_inliers_count = n_inliers_count;
+ good_coeff = cur_iteration;
+
+ // Compute the k parameter (k=log(z)/log(1-w^n))
+ float w = (float)((float)n_best_inliers_count / (float)nr_remaining_points);
+ float p_no_outliers = 1.0f - pow (w, 1.0f);
+ p_no_outliers = (std::max) (std::numeric_limits<float>::epsilon (), p_no_outliers); // Avoid division by -Inf
+ p_no_outliers = (std::min) (1.0f - std::numeric_limits<float>::epsilon (), p_no_outliers); // Avoid division by 0.
+ if (p_no_outliers == 1.0f)
+ k++;
+ else
+ k = log (1.0f - probability_) / log (p_no_outliers);
+ }
+
+ //fprintf (stderr, "[pcl_cuda::MultiRandomSampleConsensus::computeModel] Trial %d out of %f: %d inliers (best is: %d so far).\n",
+ //cur_iteration, k, n_inliers_count, n_best_inliers_count);
+ // check if we found a valid model
+
+ {
+ ScopeTime t("extracmodel");
+
+ if (valid_iterations >= k)
+ {
+ unsigned int extracted_model = good_coeff;
+ //int nr_remaining_points_before_delete = nr_remaining_points;
+ bool find_no_better = false;
+ nr_remaining_points = sac_model_->deleteIndices (hypotheses_inliers_stencils[extracted_model]);
+ //if (nr_remaining_points != nr_remaining_points_before_delete)
+ {
+
+ // Compute the k parameter (k=log(z)/log(1-w^n))
+ float w = (float)((float)min_nr_in_shape / (float)nr_remaining_points);
+ float p_no_outliers = 1.0f - pow (w, 1.0f);
+ p_no_outliers = (std::max) (std::numeric_limits<float>::epsilon (), p_no_outliers); // Avoid division by -Inf
+ p_no_outliers = (std::min) (1.0f - std::numeric_limits<float>::epsilon (), p_no_outliers); // Avoid division by 0.
+ if (p_no_outliers != 1.0f)
+ {
+ if (log (1.0f - probability_) / log (p_no_outliers) < valid_iterations) // we won't find a model with min_nr_in_shape points anymore...
+ find_no_better = true;
+ else
+ if (debug_verbosity_level > 1)
+ std::cerr << "------->" << log (1.0f - probability_) / log (p_no_outliers) << " -vs- " << valid_iterations << std::endl;
+ }
+ }
+
+ std::cerr << "found model: " << n_best_inliers_count << ", points remaining: " << nr_remaining_points << " after " << valid_iterations << " / " << cur_iteration << " iterations" << std::endl;
+
+ all_inliers_.push_back (hypotheses_inliers_stencils[extracted_model]);
+ all_inlier_counts_.push_back (n_best_inliers_count);
+ all_model_centroids_.push_back (centroids [extracted_model]);
+ thrust::host_vector<float4> host_coeffs_extracted_model = h [extracted_model / iterations_per_batch_];
+ all_model_coefficients_.push_back (host_coeffs_extracted_model [extracted_model % iterations_per_batch_]);
+
+ //float4 model_coeff = h[extracted_model/iterations_per_batch_][extracted_model%iterations_per_batch_];
+ //std::cerr << "MODEL COEFFICIENTS: " << model_coeff.x << " " << model_coeff.y << " " << model_coeff.z << " " << model_coeff.w << std::endl;
+
+ // so we only get it once:
+ hypothesis_valid[extracted_model] = false;
+
+ if (nr_remaining_points < (1.0-min_coverage_percent_) * nr_points)
+ {
+ std::cerr << "batches: " << cur_batch << ", valid iterations: " << valid_iterations << ", remaining points:" << nr_remaining_points << std::endl;
+ return true;
+ }
+ if (find_no_better)
+ {
+ std::cerr << "will find no better model! batches: " << cur_batch << ", valid iterations: " << valid_iterations << ", remaining points:" << nr_remaining_points << std::endl;
+ return true;
+ }
+
+ n_best_inliers_count = 0;
+ k = max_batches_ * iterations_per_batch_;
+ // go through all other models, invalidating those whose samples are inliers to the best one
+ int counter_invalid = 0;
+ int counter_inliers = 0;
+
+ //for (unsigned int b = 0; b <= cur_batch; b++)
+ unsigned int b = cur_batch;
+ for (unsigned int j = 0; j < iterations_per_batch_; j++)
+ {
+ // todo: precheck for very similar models
+ // if (h[best_coeff] - h[]) // <---- copies from device! slow!
+ // continue;
+ if (!hypothesis_valid[b * iterations_per_batch_ + j])
+ {
+ //std::cerr << "model " << j << " in batch " << b <<" is an invalid model" << std::endl;
+ counter_invalid ++;
+ continue;
+ }
+ if ((b*iterations_per_batch_ + j) == extracted_model)
+ {
+ if (debug_verbosity_level > 1)
+ std::cerr << "model " << j << " in batch " << b <<" is being extracted..." << std::endl;
+ continue;
+ }
+ if (sac_model_->isSampleInlier (hypotheses_inliers_stencils[extracted_model], h_samples[b], j))
+ {
+ //std::cerr << "sample point for model " << j << " in batch " << b <<" is inlier to best model " << extracted_model << std::endl;
+ counter_inliers ++;
+ hypothesis_valid[b * iterations_per_batch_ + j] = false;
+ hypotheses_inlier_count[b*iterations_per_batch_ + j] = 0;
+ if (j <= i)
+ valid_iterations --;
+ }
+ else if (j <= i) // if it is not inlier to the best model, we subtract the inliers of the extracted model
+ {
+ //todo: recompute inliers... / deleteindices
+ // ... determine best remaining model
+ int old_score = hypotheses_inlier_count[b*iterations_per_batch_ + j];
+ if (old_score != 0)
+ {
+ //std::cerr << "inliers for model " << b*iterations_per_batch_ + j << " : " << old_score;
+ n_inliers_count = sac_model_->deleteIndices (h[b], j,
+ hypotheses_inliers_stencils[b*iterations_per_batch_ + j], hypotheses_inliers_stencils[extracted_model]);
+ hypotheses_inlier_count[b*iterations_per_batch_ + j] = n_inliers_count;
+ //std::cerr << " ---> " << hypotheses_inlier_count[b * iterations_per_batch_ + j] << std::endl;
+ }
+
+ // Better match ?
+ if (n_inliers_count > n_best_inliers_count)
+ {
+ n_best_inliers_count = n_inliers_count;
+ good_coeff = b * iterations_per_batch_ + j;
+
+ // Compute the k parameter (k=log(z)/log(1-w^n))
+ float w = (float)((float)n_best_inliers_count / (float)nr_remaining_points);
+ float p_no_outliers = 1.0f - pow (w, 1.0f);
+ p_no_outliers = (std::max) (std::numeric_limits<float>::epsilon (), p_no_outliers); // Avoid division by -Inf
+ p_no_outliers = (std::min) (1.0f - std::numeric_limits<float>::epsilon (), p_no_outliers); // Avoid division by 0.
+ if (p_no_outliers == 1.0f)
+ k++;
+ else
+ k = log (1.0f - probability_) / log (p_no_outliers);
+ }
+
+ }
+ }
+ //std::cerr << "invalid models: " << counter_invalid << " , inlier models: " << counter_inliers << std::endl;
+ }
+ }
+ }
+ }
+
+ // one batch done, go to next
+ cur_batch ++;
+
+ } while (cur_iteration < max_batches_ * iterations_per_batch_);
+
+ return (false);
+}
+
+template class pcl_cuda::MultiRandomSampleConsensus<pcl_cuda::Device>;
+template class pcl_cuda::MultiRandomSampleConsensus<pcl_cuda::Host>;
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifdef _WIN32
+# define NOMINMAX
+# define WIN32_LEAN_AND_MEAN
+# include <windows.h>
+#endif
+
+#include <pcl/pcl_exports.h>
+
+#include "pcl/cuda/sample_consensus/multi_ransac.h"
+#include "pcl/cuda/time_gpu.h"
+#include <stdio.h>
+#include <pcl/cuda/time_cpu.h>
+//CUPRINTF #include "cuPrintf.cu"
+
+namespace pcl
+{
+ namespace cuda
+ {
+
+ int min_nr_in_shape = 1;
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> bool
+ MultiRandomSampleConsensus<Storage>::computeModel (int debug_verbosity_level)
+ {
+ double starttime = pcl::cuda::getTime ();
+ int counter = 0;
+ // Warn and exit if no threshold was set
+ if (threshold_ == DBL_MAX)
+ {
+ std::cerr << "[pcl::cuda::MultiRandomSampleConsensus::computeModel] No threshold set!" << std::endl;
+ return (false);
+ }
+
+ // compute number of points
+ int nr_points = sac_model_->getIndices ()->size ();
+ int nr_remaining_points = nr_points;
+ //std::cerr << "nr_points = " << nr_points << std::endl;
+ // number of total iterations
+ unsigned int cur_iteration = 0;
+ // number of valid iterations
+ int valid_iterations = 0;
+ // each batch has a vector of plane coefficients (float4)
+ std::vector<Hypotheses> h(max_batches_);
+ std::vector<typename Storage<int>::type> h_samples (max_batches_);
+ std::vector<float3> centroids (max_batches_ * iterations_per_batch_);
+ // current batch number
+ int cur_batch = 0;
+ //// stencil vector that holds the current inliers
+ std::vector<IndicesPtr> hypotheses_inliers_stencils (max_batches_ * iterations_per_batch_);
+ std::vector<int> hypotheses_inlier_count (max_batches_ * iterations_per_batch_);
+ // initialize some things
+ all_inliers_.clear ();
+ all_model_coefficients_.clear ();
+ all_model_centroids_.clear ();
+ int n_inliers_count = 0;
+ int n_best_inliers_count = 0;
+ int good_coeff = -1;
+ float k = max_batches_ * iterations_per_batch_;
+
+ //thrust::host_vector<float3> host_points = sac_model_->getInputCloud()->points;
+ //std::cerr << "Input Points:" << std::endl;
+ //for (unsigned int print_iter = 0; print_iter < nr_points; ++print_iter)
+ //{
+ // std::cerr << print_iter << " : [ "
+ // << host_points[print_iter].x << ", "
+ // << host_points[print_iter].y << ", "
+ // << host_points[print_iter].z << " ]" << std::endl;
+ //}
+ std::vector<bool> hypothesis_valid (max_batches_ * iterations_per_batch_, true);
+
+
+ ScopeTimeCPU t ("ALLLLLLLLLLL");
+ do // multiple models ..
+ {
+ double now = pcl::cuda::getTime ();
+ if ((now - starttime) > 1)
+ {
+ std::cout << "SLOW FRAME " << counter++ << std::endl;
+ starttime = now;
+ }
+ thrust::host_vector<int> host_samples;
+ thrust::host_vector<float4> host_coeffs;
+ // make sure that sac_model_->indices_ only contains remaining point indices
+ sac_model_->getIndices ();
+
+ // generate a new batch of hypotheses
+ {
+ ScopeTimeCPU t ("generateModelHypotheses");
+ sac_model_->generateModelHypotheses (h[cur_batch], h_samples[cur_batch], iterations_per_batch_);
+ }
+ host_samples = h_samples[cur_batch];
+ host_coeffs = h[cur_batch];
+
+ if (debug_verbosity_level > 1)
+ {
+ std::cerr << "Samples / Coefficients:" << std::endl;
+ for (unsigned int print_iter = 0; print_iter < iterations_per_batch_; ++print_iter)
+ {
+ std::cerr << host_samples[print_iter] << " : [ "
+ << host_coeffs[print_iter].x << ", "
+ << host_coeffs[print_iter].y << ", "
+ << host_coeffs[print_iter].z << ", "
+ << host_coeffs[print_iter].w << std::endl;
+ }
+ }
+
+ // evaluate each hypothesis in this batch
+ {
+ ScopeTimeCPU t ("evaluate");
+ for (unsigned int i = 0; i < iterations_per_batch_; i++, cur_iteration ++, valid_iterations ++)
+ {
+ // hypothesis could be invalid because it's initial sample point was inlier previously
+ if (!hypothesis_valid[cur_batch * iterations_per_batch_ + i])
+ {
+ hypotheses_inlier_count[cur_iteration] = 0;
+ valid_iterations --;
+ continue;
+ }
+
+ // compute inliers for each model
+ IndicesPtr inl_stencil;
+ {
+ ScopeTimeCPU t ("selectWithinDistance");
+ n_inliers_count = sac_model_->selectWithinDistance (h[cur_batch], i, threshold_, inl_stencil, centroids[cur_iteration]);
+ }
+ // store inliers and inlier count
+ if (n_inliers_count < min_nr_in_shape)
+ {
+ n_inliers_count = 0;
+ inl_stencil.reset (); // release stencil
+ hypothesis_valid[cur_batch * iterations_per_batch_ + i] = false;
+ }
+
+ hypotheses_inliers_stencils[cur_iteration] = inl_stencil;
+ hypotheses_inlier_count[cur_iteration] = n_inliers_count;
+
+ // Better match ?
+ if (n_inliers_count > n_best_inliers_count)
+ {
+ n_best_inliers_count = n_inliers_count;
+ good_coeff = cur_iteration;
+
+ // Compute the k parameter (k=log(z)/log(1-w^n))
+ float w = (float)((float)n_best_inliers_count / (float)nr_remaining_points);
+ float p_no_outliers = 1.0f - pow (w, 1.0f);
+ p_no_outliers = (std::max) (std::numeric_limits<float>::epsilon (), p_no_outliers); // Avoid division by -Inf
+ p_no_outliers = (std::min) (1.0f - std::numeric_limits<float>::epsilon (), p_no_outliers); // Avoid division by 0.
+ if (p_no_outliers == 1.0f)
+ k++;
+ else
+ k = log (1.0f - probability_) / log (p_no_outliers);
+ }
+
+ //fprintf (stderr, "[pcl::cuda::MultiRandomSampleConsensus::computeModel] Trial %d out of %f: %d inliers (best is: %d so far).\n",
+ // cur_iteration, k, n_inliers_count, n_best_inliers_count);
+ // check if we found a valid model
+
+ {
+ ScopeTimeCPU t("extracmodel");
+
+ if (valid_iterations >= k)
+ {
+ unsigned int extracted_model = good_coeff;
+ //int nr_remaining_points_before_delete = nr_remaining_points;
+ bool find_no_better = false;
+ nr_remaining_points = sac_model_->deleteIndices (hypotheses_inliers_stencils[extracted_model]);
+ //if (nr_remaining_points != nr_remaining_points_before_delete)
+ {
+
+ // Compute the k parameter (k=log(z)/log(1-w^n))
+ float w = (float)((float)min_nr_in_shape / (float)nr_remaining_points);
+ float p_no_outliers = 1.0f - pow (w, 1.0f);
+ p_no_outliers = (std::max) (std::numeric_limits<float>::epsilon (), p_no_outliers); // Avoid division by -Inf
+ p_no_outliers = (std::min) (1.0f - std::numeric_limits<float>::epsilon (), p_no_outliers); // Avoid division by 0.
+ if (p_no_outliers != 1.0f)
+ {
+ if (log (1.0f - probability_) / log (p_no_outliers) < valid_iterations) // we won't find a model with min_nr_in_shape points anymore...
+ find_no_better = true;
+ else
+ if (debug_verbosity_level > 1)
+ std::cerr << "------->" << log (1.0f - probability_) / log (p_no_outliers) << " -vs- " << valid_iterations << std::endl;
+ }
+ }
+
+ std::cerr << "found model: " << n_best_inliers_count << ", points remaining: " << nr_remaining_points << " after " << valid_iterations << " / " << cur_iteration << " iterations" << std::endl;
+
+ all_inliers_.push_back (hypotheses_inliers_stencils[extracted_model]);
+ all_inlier_counts_.push_back (n_best_inliers_count);
+ all_model_centroids_.push_back (centroids [extracted_model]);
+ thrust::host_vector<float4> host_coeffs_extracted_model = h [extracted_model / iterations_per_batch_];
+ all_model_coefficients_.push_back (host_coeffs_extracted_model [extracted_model % iterations_per_batch_]);
+
+
+ // so we only get it once:
+ hypothesis_valid[extracted_model] = false;
+
+ if (nr_remaining_points < (1.0-min_coverage_percent_) * nr_points)
+ {
+ std::cerr << "batches: " << cur_batch << ", valid iterations: " << valid_iterations << ", remaining points:" << nr_remaining_points << std::endl;
+ return true;
+ }
+ if (find_no_better)
+ {
+ std::cerr << "will find no better model! batches: " << cur_batch << ", valid iterations: " << valid_iterations << ", remaining points:" << nr_remaining_points << std::endl;
+ return true;
+ }
+
+ n_best_inliers_count = 0;
+ k = max_batches_ * iterations_per_batch_;
+ // go through all other models, invalidating those whose samples are inliers to the best one
+ int counter_invalid = 0;
+ int counter_inliers = 0;
+
+ //for (unsigned int b = 0; b <= cur_batch; b++)
+ unsigned int b = cur_batch;
+ for (unsigned int j = 0; j < iterations_per_batch_; j++)
+ {
+ // todo: precheck for very similar models
+ // if (h[best_coeff] - h[]) // <---- copies from device! slow!
+ // continue;
+ if (!hypothesis_valid[b * iterations_per_batch_ + j])
+ {
+ //std::cerr << "model " << j << " in batch " << b <<" is an invalid model" << std::endl;
+ counter_invalid ++;
+ continue;
+ }
+ if ((b*iterations_per_batch_ + j) == extracted_model)
+ {
+ if (debug_verbosity_level > 1)
+ std::cerr << "model " << j << " in batch " << b <<" is being extracted..." << std::endl;
+ continue;
+ }
+ if (sac_model_->isSampleInlier (hypotheses_inliers_stencils[extracted_model], h_samples[b], j))
+ {
+ //std::cerr << "sample point for model " << j << " in batch " << b <<" is inlier to best model " << extracted_model << std::endl;
+ counter_inliers ++;
+ hypothesis_valid[b * iterations_per_batch_ + j] = false;
+ hypotheses_inlier_count[b*iterations_per_batch_ + j] = 0;
+ if (j <= i)
+ valid_iterations --;
+ }
+ else if (j <= i) // if it is not inlier to the best model, we subtract the inliers of the extracted model
+ {
+ //todo: recompute inliers... / deleteindices
+ // ... determine best remaining model
+ int old_score = hypotheses_inlier_count[b*iterations_per_batch_ + j];
+ if (old_score != 0)
+ {
+ //std::cerr << "inliers for model " << b*iterations_per_batch_ + j << " : " << old_score;
+ n_inliers_count = sac_model_->deleteIndices (h[b], j,
+ hypotheses_inliers_stencils[b*iterations_per_batch_ + j], hypotheses_inliers_stencils[extracted_model]);
+ hypotheses_inlier_count[b*iterations_per_batch_ + j] = n_inliers_count;
+ //std::cerr << " ---> " << hypotheses_inlier_count[b * iterations_per_batch_ + j] << std::endl;
+ }
+
+ // Better match ?
+ if (n_inliers_count > n_best_inliers_count)
+ {
+ n_best_inliers_count = n_inliers_count;
+ good_coeff = b * iterations_per_batch_ + j;
+
+ // Compute the k parameter (k=log(z)/log(1-w^n))
+ float w = (float)((float)n_best_inliers_count / (float)nr_remaining_points);
+ float p_no_outliers = 1.0f - pow (w, 1.0f);
+ p_no_outliers = (std::max) (std::numeric_limits<float>::epsilon (), p_no_outliers); // Avoid division by -Inf
+ p_no_outliers = (std::min) (1.0f - std::numeric_limits<float>::epsilon (), p_no_outliers); // Avoid division by 0.
+ if (p_no_outliers == 1.0f)
+ k++;
+ else
+ k = log (1.0f - probability_) / log (p_no_outliers);
+ }
+
+ }
+ }
+ //std::cerr << "invalid models: " << counter_invalid << " , inlier models: " << counter_inliers << std::endl;
+ }
+ }
+ }
+ }
+
+ // one batch done, go to next
+ cur_batch ++;
+
+ } while (cur_iteration < max_batches_ * iterations_per_batch_);
+
+ // coming here means we did all 5000 iterations..
+ // let's find out why it took so long.
+
+ //std::cerr << "Inlier indices:" << std::endl;
+ //thrust::host_vector<int> best_inlier_indices = *all_inliers_[0];
+ //for (unsigned int ii = 0; ii < best_inlier_indices.size (); ++ii)
+ // std::cout << best_inlier_indices[ii] << std::endl;
+
+ //std::cout << "Samples / Coefficients:" << std::endl;
+ //for (unsigned int batch_iter = 0; batch_iter < max_batches_; ++batch_iter)
+ //{
+ // thrust::host_vector<int> host_samples = h_samples[batch_iter];
+ // thrust::host_vector<float4> host_coeffs = h[batch_iter];
+
+ // for (unsigned int print_iter = 0; print_iter < iterations_per_batch_; ++print_iter)
+ // {
+ // std::cout << host_samples[print_iter] << " : [ "
+ // << host_coeffs[print_iter].x << ", "
+ // << host_coeffs[print_iter].y << ", "
+ // << host_coeffs[print_iter].z << ", "
+ // << host_coeffs[print_iter].w << " -- " << (hypothesis_valid[batch_iter * iterations_per_batch_ + print_iter]?"ISVALID":"INVALID") << std::endl;
+ // }
+ //}
+
+ return (false);
+ }
+
+ template class PCL_EXPORTS MultiRandomSampleConsensus<Device>;
+ template class PCL_EXPORTS MultiRandomSampleConsensus<Host>;
+
+ } // namespace
+} // namespace
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifdef _WIN32
+# define NOMINMAX
+# define WIN32_LEAN_AND_MEAN
+# include <windows.h>
+#endif
+
+#include "pcl/cuda/sample_consensus/ransac.h"
+#include "pcl/cuda/time_gpu.h"
+#include <stdio.h>
+
+namespace pcl
+{
+ namespace cuda
+ {
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> bool
+ RandomSampleConsensus<Storage>::computeModel (int debug_verbosity_level)
+ {
+ // Warn and exit if no threshold was set
+ if (threshold_ == DBL_MAX)
+ {
+ std::cerr << "[pcl::cuda::RandomSampleConsensus::computeModel] No threshold set!" << std::endl;
+ return (false);
+ }
+
+ iterations_ = 0;
+ int n_best_inliers_count = -INT_MAX;
+ float k = 1.0;
+
+ Indices inliers;
+ typename SampleConsensusModel<Storage>::IndicesPtr inl_stencil;
+ Indices selection;
+ Coefficients model_coefficients;
+ int good_coeff = -1;
+
+ int n_inliers_count = 0;
+
+#if 1
+ Hypotheses h;
+ sac_model_->generateModelHypotheses (h, max_iterations_);
+#endif
+ // Iterate
+ while (iterations_ < k)
+ {
+#if 0
+ // Get X samples which satisfy the model criteria
+ sac_model_->getSamples (iterations_, selection);
+
+ if (selection.empty ())
+ {
+ std::cerr << "[pcl::cuda::RandomSampleConsensus::computeModel] No samples could be selected!" << std::endl;
+ break;
+ }
+
+ // Search for inliers in the point cloud for the current plane model M
+ if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
+ {
+ //++iterations_;
+ continue;
+ }
+
+ // Select the inliers that are within threshold_ from the model
+ n_inliers_count = sac_model_->countWithinDistance (model_coefficients, threshold_);
+#endif
+
+#if 1
+ float3 centroid;
+ n_inliers_count = sac_model_->selectWithinDistance (h, iterations_, threshold_, inl_stencil, centroid);
+ //n_inliers_count = sac_model_->countWithinDistance (h, iterations_, threshold_);
+#endif
+ //if (inliers.empty () && k > 1.0)
+ // continue;
+
+ //n_inliers_count = inliers.size ();
+
+ // Better match ?
+ if (n_inliers_count > n_best_inliers_count)
+ {
+ n_best_inliers_count = n_inliers_count;
+
+ // Save the current model/inlier/coefficients selection as being the best so far
+ //inliers_ = inliers;
+ //inliers_stencil_ = inliers_stencil;
+#if 0
+ model_ = selection;
+ model_coefficients_ = model_coefficients;
+#endif
+
+#if 1
+ good_coeff = iterations_;
+#endif
+
+ // Compute the k parameter (k=log(z)/log(1-w^n))
+ float w = (float)((float)n_best_inliers_count / (float)sac_model_->getIndices ()->size ());
+ // float p_no_outliers = 1.0 - pow (w, (float)selection.size ());
+ float p_no_outliers = 1.0f - pow (w, (float)1);
+ p_no_outliers = (std::max) (std::numeric_limits<float>::epsilon (), p_no_outliers); // Avoid division by -Inf
+ p_no_outliers = (std::min) (1.0f - std::numeric_limits<float>::epsilon (), p_no_outliers); // Avoid division by 0.
+ if (p_no_outliers == 1.0f)
+ k++;
+ else
+ k = log (1.0f - probability_) / log (p_no_outliers);
+ }
+
+ ++iterations_;
+ if (debug_verbosity_level > 1)
+ fprintf (stderr, "[pcl::cuda::RandomSampleConsensus::computeModel] Trial %d out of %f: %d inliers (best is: %d so far).\n", iterations_, k, n_inliers_count, n_best_inliers_count);
+ if (iterations_ > max_iterations_)
+ {
+ if (debug_verbosity_level > 0)
+ std::cerr << "[pcl::cuda::RandomSampleConsensus::computeModel] RANSAC reached the maximum number of trials." << std::endl;
+ break;
+ }
+ }
+
+ if (debug_verbosity_level > 0)
+ fprintf (stderr, "[pcl::cuda::RandomSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", (unsigned long) model_.size (), n_best_inliers_count);
+
+ // if (model_.empty ())
+ // {
+ // inliers_.clear ();
+ // return (false);
+ // }
+
+#if 1
+ if (good_coeff == -1)
+ return (false);
+#endif
+
+ /* model_coefficients_.resize (4);
+ */
+ // Get the set of inliers that correspond to the best model found so far
+#if 0
+ sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_, inliers_stencil_);
+#endif
+
+#if 1
+ float3 centroid;
+ sac_model_->selectWithinDistance (h, good_coeff, threshold_, inliers_stencil_, centroid);//inliers_stencil_);
+ std::cerr << "selected model " << good_coeff << std::endl;
+#endif
+ return (true);
+ }
+
+ template class RandomSampleConsensus<Device>;
+ template class RandomSampleConsensus<Host>;
+
+ } // namespace
+} // namespace
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifdef WIN32
+# define WIN32_LEAN_AND_MEAN
+#endif
+
+#include <pcl/pcl_exports.h>
+
+#include "pcl/cuda/sample_consensus/sac_model.h"
+#include "pcl/cuda/time_gpu.h"
+
+#include <thrust/replace.h>
+#include <thrust/copy.h>
+#include <thrust/sequence.h>
+#include <thrust/gather.h>
+
+
+namespace pcl
+{
+ namespace cuda
+ {
+
+ template <template <typename> class Storage>
+ __inline__ __host__
+ void create_scatter_stencil (int new_w, int new_h, int skip, int width, int height, typename Storage<int>::type &stencil)
+ {
+ for (unsigned int i = 0; i < new_w * new_h; ++i)
+ {
+ int xIdx = i % new_w;
+ int yIdx = i / new_w;
+ stencil [i] = (xIdx * skip) + width * (yIdx * skip);
+ }
+ }
+
+ template <template <typename> class Storage> void
+ SampleConsensusModel<Storage>::setInputCloud (const PointCloudConstPtr &cloud)
+ {
+ //input_.reset (new PointCloud);
+ //int skip = 2;
+ //int new_w = cloud->width/skip;
+ //int new_h = cloud->height/skip;
+
+ //input_->width = new_w;
+ //input_->height = new_h;
+ //input_->is_dense = cloud->is_dense;
+ //
+ //typename Storage<int>::type stencil (new_w * new_h);
+ //create_scatter_stencil<Storage> (new_w, new_h, skip, cloud->width, cloud->height, stencil);
+ //
+ //input_->points.resize (stencil.size ());
+ //thrust::gather (stencil.begin (), stencil.end (), cloud->points.begin (), input_->points.begin ());
+ // TODO pcl_cuda::ScopeTime t ("setInputCloud");
+ input_ = cloud;
+
+ //// first, we create an inlier stencil that has -1 set for all nan points
+ if (!indices_stencil_)
+ indices_stencil_.reset (new Indices);
+
+ indices_stencil_->resize (input_->points.size());
+ thrust::sequence (indices_stencil_->begin (), indices_stencil_->end ());
+
+ thrust::replace_if (indices_stencil_->begin (), indices_stencil_->end (),
+ input_->points.begin (), isNaNPoint (),
+ -1);
+
+ // copy all non-(-1) values from indices_stencil_ to indices_
+ if (!indices_)
+ indices_.reset (new Indices);
+
+ indices_->resize (input_->points.size());
+ typename Indices::iterator it;
+ it = thrust::copy_if (indices_stencil_->begin (), indices_stencil_->end (), indices_->begin (), isInlier ());
+ indices_->erase (it, indices_->end ());
+
+ std::cerr << "setInputCloud :" << indices_->size () << " valid points ( " << indices_->size () / (float) input_->points.size () << " %)" << std::endl;
+
+ }
+
+
+ // ------ got moved to inline function def in sac_model.h
+ //template <template <typename> class Storage> typename SampleConsensusModel<Storage>::IndicesPtr
+ //SampleConsensusModel<Storage>::getIndices () const
+ //{
+ // if (nr_indices_in_stencil_ != indices_->size())
+ // {
+ // typename Indices::iterator last = thrust::remove_copy (indices_stencil_->begin (), indices_stencil_->end (), indices_->begin (), -1);
+ // indices_->resize (last - indices_->begin ());
+ // }
+ //
+ // return (indices_);
+ //}
+
+ template <template <typename> class Storage> int
+ SampleConsensusModel<Storage>::deleteIndices (const IndicesPtr &indices_stencil )
+ {
+ //assert (indices_->size() == indices_stencil->size());
+ //thrust::transform (thrust::make_zip_iterator (thrust::make_tuple (indices_stencil_->begin(), indices_stencil->begin ())),
+ // thrust::make_zip_iterator (thrust::make_tuple (indices_stencil_->begin(), indices_stencil->begin ())) + indices_stencil_->size(),
+ // indices_stencil_->begin (),
+ // DeleteIndices ());
+ //typename Indices::iterator last = thrust::remove_copy (indices_stencil_->begin (), indices_stencil_->end (), indices_->begin (), -1);
+ //indices_->resize (last - indices_->begin());
+ //return (int)indices_->size();
+
+ assert (indices_stencil_->size() == indices_stencil->size());
+ thrust::transform (thrust::make_zip_iterator (thrust::make_tuple (indices_stencil_->begin(), indices_stencil->begin ())),
+ thrust::make_zip_iterator (thrust::make_tuple (indices_stencil_->begin(), indices_stencil->begin ())) + indices_stencil_->size(),
+ indices_stencil_->begin (),
+ DeleteIndices ());
+ int pts_deleted = (int) thrust::count (indices_stencil_->begin (), indices_stencil_->end (), -1);
+
+ return (int) indices_stencil_->size ()- pts_deleted;
+ }
+
+ template <template <typename> class Storage> int
+ SampleConsensusModel<Storage>::deleteIndices (const Hypotheses &h, int idx, IndicesPtr &inliers, const IndicesPtr &inliers_delete)
+ {
+ if (inliers->size() != inliers_delete->size())
+ std::cerr << "assert (inliers->size() == inliers_delete->size()); ---> " << inliers->size () << " != " << inliers_delete->size () << std::endl;
+ //assert (inliers->size() == inliers_delete->size());
+ thrust::transform (thrust::make_zip_iterator (thrust::make_tuple (inliers->begin(), inliers_delete->begin ())),
+ thrust::make_zip_iterator (thrust::make_tuple (inliers->begin(), inliers_delete->begin ())) + inliers_delete->size(),
+ inliers->begin (),
+ DeleteIndices ());
+ int i = (int) thrust::count (inliers->begin (), inliers->end (), -1);
+ return (int)inliers->size() - i;
+ }
+
+
+ //////////////////////////////////////////////////////////////////////////
+ // set all points which are inliers in the current model to -1,
+ // points which are outliers (-1) to the current model are copied
+ template <typename Tuple> int
+ DeleteIndices::operator () (const Tuple &t)
+ {
+ if (thrust::get<1>(t) == -1)
+ return thrust::get<0>(t);
+ else
+ return -1;
+ }
+
+
+ template class PCL_EXPORTS SampleConsensusModel<Device>;
+ template class PCL_EXPORTS SampleConsensusModel<Host>;
+
+ } // namespace
+} // namespace
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <pcl/pcl_exports.h>
+
+#include <pcl/cuda/sample_consensus/sac_model_1point_plane.h>
+#include <pcl/cuda/common/eigen.h>
+#include <pcl/cuda/cutil_math.h>
+
+#include <thrust/copy.h>
+#include <thrust/count.h>
+#include <thrust/random.h>
+
+#include <vector_types.h>
+#include <stdio.h>
+#include <limits>
+
+// specify inlier computation method
+//#define KINECT_NORMALS
+#define KINECT
+
+namespace pcl
+{
+ namespace cuda
+ {
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage>
+ SampleConsensusModel1PointPlane<Storage>::SampleConsensusModel1PointPlane (
+ const PointCloudConstPtr &cloud) :
+ SampleConsensusModel<Storage> (cloud)
+ {
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> void
+ SampleConsensusModel1PointPlane<Storage>::getSamples (int &iterations, Indices &samples)
+ {
+ samples.resize (1);
+ float trand = indices_->size () / (RAND_MAX + 1.0f);
+ int idx = (int)(rngl_ () * trand);
+ samples[0] = (*indices_)[idx];
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> bool
+ SampleConsensusModel1PointPlane<Storage>::computeModelCoefficients (
+ const Indices &samples, Coefficients &model_coefficients)
+ {
+ if (samples.size () != 1)
+ return (false);
+
+ /* if (isnan ((PointXYZRGB)input_->points[samples[0]]).x ||
+ isnan ((PointXYZRGB)input_->points[samples[1]]).x ||
+ isnan ((PointXYZRGB)input_->points[samples[2]]).x)
+ return (false);*/
+
+ float3 normal;
+ normal.x = 0;
+ normal.y = 0;
+ normal.z = -1;
+
+ // Compute the plane coefficients
+ // calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1)
+ float3 mc = normalize (normal);
+
+ if (model_coefficients.size () != 4)
+ model_coefficients.resize (4);
+ model_coefficients[0] = mc.x;
+ model_coefficients[1] = mc.y;
+ model_coefficients[2] = mc.z;
+ // ... + d = 0
+ model_coefficients[3] = -1 * dot (mc, ((PointXYZRGB)input_->points[samples[0]]).xyz);
+
+ return (true);
+ }
+
+ __host__ __device__
+ unsigned int hash(unsigned int a)
+ {
+ a = (a+0x7ed55d16) + (a<<12);
+ a = (a^0xc761c23c) ^ (a>>19);
+ a = (a+0x165667b1) + (a<<5);
+ a = (a+0xd3a2646c) ^ (a<<9);
+ a = (a+0xfd7046c5) + (a<<3);
+ a = (a^0xb55a4f09) ^ (a>>16);
+ return a;
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage>
+ //template <typename Tuple>
+ thrust::tuple <int, float4>
+ Create1PointPlaneSampleHypothesis<Storage>::operator () (int t)
+ {
+ float4 coeff;
+ coeff.x = coeff.y = coeff.z = coeff.w = 5;
+
+ float trand = (float) nr_indices / (thrust::default_random_engine::max + 1.0f);
+ //rng.seed (hash (t));
+
+ //int sample_point = indices[(int)(rng () * trand)];
+ int sample_point = indices[(int)(t * trand)];
+
+ if (isnan (input[sample_point].x))
+ return (thrust::make_tuple (sample_point, coeff));
+
+#if 0
+ //TODO:: kind of important: get normal! :D
+ int xIdx = sample_point % width_;
+ int yIdx = sample_point / width_;
+
+ //int counter = 1;
+
+ int window_size = 3;
+ int left_index = 0;
+ int top_index = 0;
+ // West
+ if (xIdx >= window_size)
+ {
+ left_index = sample_point - window_size;
+ }
+ else
+ {
+ left_index = sample_point + window_size;
+ }
+
+ // North
+ if (yIdx >= window_size)
+ {
+ top_index = sample_point - window_size * width_;
+ }
+ else
+ {
+ top_index = sample_point + window_size * width_;
+ }
+
+ float3 left_point;
+
+ left_point.x = input[left_index].x - input[sample_point].x;
+ left_point.y = input[left_index].y - input[sample_point].y;
+ left_point.z = input[left_index].z - input[sample_point].z;
+
+ float3 top_point;
+
+ top_point.x = input[top_index].x - input[sample_point].x;
+ top_point.y = input[top_index].y - input[sample_point].y;
+ top_point.z = input[top_index].z - input[sample_point].z;
+
+ float3 normal = cross (top_point, left_point);
+
+ // Compute the plane coefficients from the 3 given points in a straightforward manner
+ // calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1)
+ float3 mc = normalize (normal);
+
+ if (0 == (normal.x) && 0 == (normal.y) && 0 == (normal.z))
+ {
+ //mc.x = mc.y = 0;
+ if (top_point.x == 0 && top_point.y == 0 && top_point.z == 0)
+ {
+ mc.x = 999999;
+ mc.y = input[top_index].x;
+ mc.z = input[sample_point].x;
+ //mc.z = top_index - sample_point;
+ //mc.z = 999999;
+ }
+ else
+ {
+ if (left_point.x == 0 && left_point.y == 0 && left_point.z == 0)
+ {
+ mc.x = mc.y = 888888;
+ mc.z = left_index - sample_point;
+ //mc.z = 888888;
+ }
+ }
+ }
+#else
+ float3 mc = make_float3 (normals_[sample_point].x, normals_[sample_point].y, normals_[sample_point].z);
+#endif
+
+ coeff.x = mc.x;
+ coeff.y = mc.y;
+ coeff.z = mc.z;
+ // ... + d = 0
+ coeff.w = -1 * dot (mc, input[sample_point].xyz);
+
+ return (thrust::make_tuple (sample_point, coeff));
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage>
+ //template <typename Tuple>
+ float4
+ Create1PointPlaneHypothesis<Storage>::operator () (int t)
+ {
+ float4 coeff;
+ coeff.x = coeff.y = coeff.z = coeff.w = bad_value;
+
+ float trand = nr_indices / (RAND_MAX + 1.0f);
+ thrust::default_random_engine rng (t);
+
+ int sample_point = indices[(int)(rng () * trand)];
+
+ if (isnan (input[sample_point].x))
+ return (coeff);
+
+ //TODO:: kind of important: get normal! :D
+
+ //int xIdx = sample_point % width;
+ //int yIdx = sample_point / width;
+
+ //float3 b = input[sample_point];
+ //int counter = 1;
+
+ //// West
+ //if (xIdx < width-window_size)
+ //{
+ // b += input[sample_point + window_size];
+ // counter += 1
+ //}
+
+ //// North
+ //if (yIdx >= window_size)
+ //{
+ // b += input[sample_point - window_size * width];
+ //}
+
+ //// South
+ //if (yIdx < height-window_size)
+ //{
+ // b += input[sample_point + window_size * width];
+ //}
+
+ //// East
+ //if (xIdx >= window_size)
+ //{
+ // b += input[sample_point + window_size];
+ //}
+
+ //// Estimate the XYZ centroid
+ //compute3DCentroid (cloud, xyz_centroid);
+
+ //// Compute the 3x3 covariance matrix
+ //computeCovarianceMatrix (cloud, xyz_centroid, covariance_matrix);
+
+ //// Get the plane normal and surface curvature
+ //solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature);
+
+ //int[5] idxs;
+ //idxs[0] = sample_point;
+ // west = sample_point - window_size;
+ //else
+ // west = -1;
+
+
+ float3 normal;
+ normal.x = 0;
+ normal.y = 0;
+ normal.z = -1;
+
+ // Compute the plane coefficients from the 3 given points in a straightforward manner
+ // calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1)
+ float3 mc = normalize (normal);
+
+ coeff.x = mc.x;
+ coeff.y = mc.y;
+ coeff.z = mc.z;
+ // ... + d = 0
+ coeff.w = -1 * dot (mc, input[sample_point].xyz);
+
+ return (coeff);
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> bool
+ SampleConsensusModel1PointPlane<Storage>::generateModelHypotheses (
+ Hypotheses &h, int max_iterations)
+ {
+ using namespace thrust;
+
+ // Create a vector of how many samples/coefficients do we want to get
+ h.resize (max_iterations);
+
+ typename Storage<int>::type randoms (max_iterations);
+ // a sequence counting up from 0
+ thrust::counting_iterator<int> index_sequence_begin (0);
+ // transform the range [0,1,2,...N]
+ // to a range of random numbers
+ thrust::transform (index_sequence_begin,
+ index_sequence_begin + max_iterations,
+ randoms.begin (),
+ parallel_random_generator (0));
+
+ //thrust::counting_iterator<int> first (0);
+ // Input: Point Cloud, Indices
+ // Output: Hypotheses
+ transform (//first, first + max_iterations,
+ //index_sequence_begin,
+ //index_sequence_begin + max_iterations,
+ randoms.begin (), randoms.begin () + max_iterations,
+ h.begin (),
+ Create1PointPlaneHypothesis<Storage> (thrust::raw_pointer_cast (&input_->points[0]),
+ thrust::raw_pointer_cast (&(*indices_)[0]),
+ indices_->size (), std::numeric_limits<float>::quiet_NaN ()));
+ return (true);
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> bool
+ SampleConsensusModel1PointPlane<Storage>::generateModelHypotheses (
+ Hypotheses &h, Samples &samples, int max_iterations)
+ {
+ using namespace thrust;
+
+ // Create a vector of how many samples/coefficients do we want to get
+ h.resize (max_iterations);
+ samples.resize (max_iterations);
+
+ typename Storage<int>::type randoms (max_iterations);
+ // a sequence counting up from 0
+ thrust::counting_iterator<int> index_sequence_begin (0);
+ // transform the range [0,1,2,...N]
+ // to a range of random numbers
+ thrust::transform (index_sequence_begin,
+ index_sequence_begin + max_iterations,
+ randoms.begin (),
+ parallel_random_generator (0));
+
+
+ //thrust::counting_iterator<int> first (0);
+ // Input: Point Cloud, Indices
+ // Output: Hypotheses
+ transform (//first, first + max_iterations,
+ //index_sequence_begin,
+ //index_sequence_begin + max_iterations,
+ randoms.begin (), randoms.begin () + max_iterations,
+ //index_sequence_begin, index_sequence_begin + max_iterations,
+ thrust::make_zip_iterator (thrust::make_tuple (samples.begin (), h.begin())),
+ // h.begin (),
+ Create1PointPlaneSampleHypothesis<Storage> (thrust::raw_pointer_cast (&input_->points[0]),
+ thrust::raw_pointer_cast (&(*normals_)[0]),
+ thrust::raw_pointer_cast (&(*indices_)[0]),
+ input_->width, input_->height,
+ indices_->size (), std::numeric_limits<float>::quiet_NaN ()));
+ return (true);
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <typename Tuple> bool
+ CountPlanarInlier::operator () (const Tuple &t)
+ {
+ if (!isfinite (thrust::get<0>(t).x))
+ return (false);
+
+ //TODO: make threshold adaptive, depending on z
+
+ return (fabs (thrust::get<0>(t).x * coefficients.x +
+ thrust::get<0>(t).y * coefficients.y +
+ thrust::get<0>(t).z * coefficients.z + coefficients.w) < threshold);
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> int
+ NewCheckPlanarInlier<Storage>::operator () (const int &idx)
+ {
+ if (idx == -1)
+ return -1;
+
+ PointXYZRGB p = input_[idx];
+
+ if (isnan (p.x))
+ return -1;
+
+ if (fabs (p.x * coefficients.x +
+ p.y * coefficients.y +
+ p.z * coefficients.z + coefficients.w) < threshold)
+ // If inlier, return its position in the vector
+ return idx;
+ else
+ // If outlier, return -1
+ return -1;
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <typename Tuple> int
+ CheckPlanarInlier::operator () (const Tuple &t)
+ {
+ if (thrust::get<1>(t) == -1)
+ return (-1);
+ if (isnan (thrust::get<0>(t).x))
+ return (-1);
+ // Fill in XYZ (and copy NaNs with it)
+ float4 pt;
+ pt.x = thrust::get<0>(t).x;
+ pt.y = thrust::get<0>(t).y;
+ pt.z = thrust::get<0>(t).z;
+ pt.w = 1;
+
+ //TODO: make threshold adaptive, depending on z
+
+ if (fabs (dot (pt, coefficients)) < threshold)
+ // If inlier, return its position in the vector
+ return (thrust::get<1>(t));
+ else
+ // If outlier, return -1
+ return (-1);
+ }
+
+ int CheckPlanarInlierKinectIndices::operator () (const PointXYZRGB &pt, const int &idx)
+ {
+ //if (isnan (pt.x) | isnan (pt.y) | isnan (pt.z) | (idx == -1))
+ // return (-1);
+
+ const float b = 0.075f;
+ const float f = 580.0f/2.0f;
+ float length_pt = sqrtf (dot (pt, pt));
+ float dot_n_p = pt.x * coefficients.x +
+ pt.y * coefficients.y +
+ pt.z * coefficients.z;
+ float D = - coefficients.w * length_pt / dot_n_p - length_pt;
+
+ float orig_disparity = b * f / pt.z;
+ float actual_disparity = orig_disparity * length_pt / (length_pt + D);
+
+ if ((fabs (actual_disparity - orig_disparity) <= 1.0/6.0) & idx != -1)
+ return (idx);
+ else
+ return -1;
+ }
+
+ template <typename Tuple>
+ int CheckPlanarInlierKinectNormalIndices::operator () (const Tuple &t, const int &idx)
+ {
+ //if (isnan (pt.x) | isnan (pt.y) | isnan (pt.z) | (idx == -1))
+ // return (-1);
+
+ const PointXYZRGB &pt = thrust::get<0>(t);
+ float4 &normal = thrust::get<1>(t);
+
+ const float b = 0.075f;
+ const float f = 580.0f/2.0f;
+ float length_pt = sqrtf (dot (pt, pt));
+ float dot_n_p = pt.x * coefficients.x +
+ pt.y * coefficients.y +
+ pt.z * coefficients.z;
+ float D = - coefficients.w * length_pt / dot_n_p - length_pt;
+
+ float orig_disparity = b * f / pt.z;
+ float actual_disparity = orig_disparity * length_pt / (length_pt + D);
+
+ if ((fabs (actual_disparity - orig_disparity) <= 1.0/2.0) & (idx != -1)
+ &
+ (
+ fabs (acos (normal.x*coefficients.x + normal.y*coefficients.y + normal.z*coefficients.z)) < angle_threshold
+ |
+ fabs (acos (-(normal.x*coefficients.x + normal.y*coefficients.y + normal.z*coefficients.z))) < angle_threshold
+ )
+ )
+ return (idx);
+ else
+ return -1;
+ }
+
+ template <typename Tuple>
+ int CheckPlanarInlierNormalIndices::operator () (const Tuple &t, const int &idx)
+ {
+ const PointXYZRGB &pt = thrust::get<0>(t);
+ if (isnan (pt.x) | isnan (pt.y) | isnan (pt.z) | (idx == -1))
+ return (-1);
+
+ float4 &normal = thrust::get<1>(t);
+ //TODO: make threshold adaptive, depending on z
+
+ if (fabs (pt.x * coefficients.x +
+ pt.y * coefficients.y +
+ pt.z * coefficients.z + coefficients.w) < threshold
+ &
+ (
+ fabs (acos (normal.x*coefficients.x + normal.y*coefficients.y + normal.z*coefficients.z)) < angle_threshold
+ |
+ fabs (acos (-(normal.x*coefficients.x + normal.y*coefficients.y + normal.z*coefficients.z))) < angle_threshold
+ )
+ )
+ // If inlier, return its position in the vector
+ return (idx);
+ else
+ // If outlier, return -1
+ return (-1);
+ }
+
+ int CheckPlanarInlierIndices::operator () (const PointXYZRGB &pt, const int &idx)
+ {
+ if (idx == -1)
+ return (-1);
+ if (isnan (pt.x) | isnan (pt.y) | isnan (pt.z))
+ return (-1);
+
+ //TODO: make threshold adaptive, depending on z
+
+ if (fabs (pt.x * coefficients.x +
+ pt.y * coefficients.y +
+ pt.z * coefficients.z + coefficients.w) < threshold)
+ // If inlier, return its position in the vector
+ return (idx);
+ else
+ // If outlier, return -1
+ return (-1);
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> int
+ SampleConsensusModel1PointPlane<Storage>::countWithinDistance (
+ const Coefficients &model_coefficients, float threshold)
+ {
+ using namespace thrust;
+
+ // Needs a valid set of model coefficients
+ if (model_coefficients.size () != 4)
+ {
+ fprintf (stderr, "[pcl::cuda::SampleConsensusModel1PointPlane::countWithinDistance] Invalid number of model coefficients given (%lu)!\n", (unsigned long) model_coefficients.size ());
+ return 0;
+ }
+
+ float4 coefficients;
+ coefficients.x = model_coefficients[0];
+ coefficients.y = model_coefficients[1];
+ coefficients.z = model_coefficients[2];
+ coefficients.w = model_coefficients[3];
+
+ return (int) count_if (
+ make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())),
+ make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())) +
+ indices_->size (),
+ CountPlanarInlier (coefficients, threshold));
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> int
+ SampleConsensusModel1PointPlane<Storage>::countWithinDistance (
+ const Hypotheses &h, int idx, float threshold)
+ {
+ if (isnan (((float4)h[idx]).x))
+ return (0);
+
+ return (int)
+ (thrust::count_if (
+ thrust::make_zip_iterator (thrust::make_tuple (input_->points.begin (), indices_->begin ())),
+ thrust::make_zip_iterator (thrust::make_tuple (input_->points.begin (), indices_->begin ())) +
+ indices_->size (),
+ CountPlanarInlier (h[idx], threshold)));
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> int
+ SampleConsensusModel1PointPlane<Storage>::selectWithinDistance (
+ const Coefficients &model_coefficients, float threshold, IndicesPtr &inliers, IndicesPtr &inliers_stencil)
+ {
+ using namespace thrust;
+
+ // Needs a valid set of model coefficients
+ if (model_coefficients.size () != 4)
+ {
+ fprintf (stderr, "[SampleConsensusModel1PointPlane::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", (unsigned long) model_coefficients.size ());
+ return 0;
+ }
+
+ int nr_points = (int) indices_->size ();
+ {
+ // pcl::ScopeTime t ("Resize inl");
+ if (!inliers_stencil)
+ inliers_stencil.reset (new Indices());
+
+ inliers_stencil->resize (nr_points);
+ }
+
+ float4 coefficients;
+ coefficients.x = model_coefficients[0];
+ coefficients.y = model_coefficients[1];
+ coefficients.z = model_coefficients[2];
+ coefficients.w = model_coefficients[3];
+
+ {
+ // pcl::ScopeTime t ("transform");
+ // Send the data to the device
+ transform (
+ make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())),
+ make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())) +
+ nr_points,
+ inliers_stencil->begin (),
+ CheckPlanarInlier (coefficients, threshold));
+ }
+
+ {
+ // pcl::ScopeTime t ("Resize all");
+ if (!inliers)
+ inliers.reset (new Indices());
+ inliers->resize (nr_points);
+ }
+ typename Indices::iterator it;
+ {
+ // pcl::ScopeTime t ("copy-if");
+ // Copy data
+ it = copy_if (inliers_stencil->begin (), inliers_stencil->end (), inliers->begin (), isInlier ());
+ //it = remove_copy (inliers_stencil->begin (), inliers_stencil->end (), inliers->begin (), -1);
+ }
+ {
+ // pcl::ScopeTime t ("Resize");
+ inliers->resize (it - inliers->begin ());
+ }
+ return (int) inliers->size();
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> int
+ SampleConsensusModel1PointPlane<Storage>::selectWithinDistance (
+ const Hypotheses &h, int idx, float threshold, IndicesPtr &inliers, IndicesPtr &inliers_stencil)
+ {
+ using namespace thrust;
+
+ // Needs a valid set of model coefficients
+ /* if (model_coefficients.size () != 4)
+ {
+ fprintf (stderr, "[SampleConsensusModel1PointPlane::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", (unsigned long) model_coefficients.size ());
+ return;
+ }*/
+
+ int nr_points = (int) indices_->size ();
+ {
+ // pcl::ScopeTime t ("Resize inl");
+
+ if (!inliers_stencil)
+ inliers_stencil.reset (new Indices());
+
+ inliers_stencil->resize (nr_points);
+ }
+
+ float4 coefficients;
+ coefficients.x = ((float4)h[idx]).x;
+ coefficients.y = ((float4)h[idx]).y;
+ coefficients.z = ((float4)h[idx]).z;
+ coefficients.w = ((float4)h[idx]).w;
+
+ {
+ // pcl::ScopeTime t ("transform");
+ // Send the data to the device
+ transform (
+ make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())),
+ make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())) +
+ nr_points,
+ inliers_stencil->begin (),
+ CheckPlanarInlier (coefficients, threshold));
+ }
+
+ {
+ // pcl::ScopeTime t ("Resize all");
+ if (!inliers)
+ inliers.reset (new Indices());
+ inliers->resize (nr_points);
+ }
+ typename Indices::iterator it;
+ {
+ // pcl::ScopeTime t ("copy-if");
+ // Copy data
+ it = copy_if (inliers_stencil->begin (), inliers_stencil->end (), inliers->begin (), isInlier ());
+ }
+ {
+ // pcl::ScopeTime t ("Resize");
+ inliers->resize (it - inliers->begin ());
+ }
+ return (int) inliers->size ();
+ }
+
+
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> int
+ SampleConsensusModel1PointPlane<Storage>::selectWithinDistance (
+ Hypotheses &h, int idx, float threshold, IndicesPtr &inliers_stencil, float3 &c)
+ {
+ float angle_threshold = 0.26f;
+ using namespace thrust;
+
+ int nr_points = (int) indices_stencil_->size ();
+ float bad_point = std::numeric_limits<float>::quiet_NaN ();
+
+ if (!inliers_stencil)
+ inliers_stencil.reset (new Indices());
+ inliers_stencil->resize (nr_points);
+
+ // necessary for the transform_if call below (since not all elements get written, we init with -1)..
+ //inliers_stencil->resize (nr_points, -1);
+
+ float4 coefficients;
+ coefficients.x = ((float4)h[idx]).x;
+ coefficients.y = ((float4)h[idx]).y;
+ coefficients.z = ((float4)h[idx]).z;
+ coefficients.w = ((float4)h[idx]).w;
+
+ if (isnan (coefficients.x) |
+ isnan (coefficients.y) |
+ isnan (coefficients.z) |
+ isnan (coefficients.w) )
+ {
+ c.x = c.y = c.z = 0;
+ return 0;
+ }
+
+ float3 best_centroid;
+ IndicesPtr best_inliers_stencil;
+
+ float3 centroid;
+
+ centroid.x = centroid.y = centroid.z = 0;
+ best_centroid = centroid;
+
+ //ORIG
+ // transform (
+ // make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())),
+ // make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())) +
+ // nr_points,
+ // inliers_stencil->begin (),
+ // CheckPlanarInlier (coefficients, threshold));
+
+ // this is just as fast as the ORIG version, but requires initialization to -1 (see above) --> much slower
+ // transform_if (
+ // make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())),
+ // make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())) +
+ // nr_points,
+ // indices_->begin(),
+ // inliers_stencil->begin (),
+ // CheckPlanarInlier (coefficients, threshold),
+ // isInlier ()
+ // );
+
+ // i forgot why this was slow. but it was. :)
+ // transform (
+ // indices_stencil_->begin (),
+ // indices_stencil_->end(),
+ // inliers_stencil->begin (),
+ // NewCheckPlanarInlier<Storage> (coefficients, (float)threshold, input_->points));
+
+ // compute inliers
+ // fastest
+#ifdef KINECT
+ // NOTE: this performs inlier checks with kinect disparity error model, without normal check
+ transform (
+ input_->points.begin (), input_->points.end (),
+ indices_stencil_->begin (),
+ inliers_stencil->begin (),
+ CheckPlanarInlierKinectIndices (coefficients, threshold, angle_threshold));
+#endif
+
+#ifdef KINECT_NORMALS
+ // NOTE: this performs inlier checks with kinect disparity error model, with normal check
+ transform (
+ make_zip_iterator (make_tuple (input_->points.begin (), normals_->begin())),
+ make_zip_iterator (make_tuple (input_->points.begin (), normals_->begin())) + nr_points,
+ indices_stencil_->begin (),
+ inliers_stencil->begin (),
+ CheckPlanarInlierKinectNormalIndices (coefficients, threshold, angle_threshold));
+#endif
+
+ // store inliers here
+ Indices inliers;
+ inliers.resize (indices_->size ()); // is this necessary?
+
+ typename Indices::iterator last = thrust::remove_copy (inliers_stencil->begin (), inliers_stencil->end (), inliers.begin (), -1);
+ inliers.erase (last, inliers.end ());
+
+ if (inliers.size () < 1)
+ return (int) inliers.size ();
+
+ best_inliers_stencil = inliers_stencil;
+ int best_nr_inliers = (int) inliers.size ();
+
+ int nr_inliers_after_refit = (int) inliers.size ();
+ int nr_inliers_before_refit;
+ int nr_refit_iterations = 0;
+
+ do {
+ nr_inliers_before_refit = nr_inliers_after_refit;
+
+ compute3DCentroid (make_permutation_iterator (input_->points.begin (), inliers.begin ()),
+ make_permutation_iterator (input_->points.begin (), inliers.end ()),
+ centroid);
+
+ if (isnan (centroid.x) | isnan (centroid.y) | isnan (centroid.z))
+ {
+ std::cerr << "Wow, centroid contains nans!" << std::endl;
+
+ inliers_stencil = best_inliers_stencil;
+ c = make_float3 (bad_point, bad_point, bad_point);
+ //best_centroid;
+ return best_nr_inliers;
+ }
+
+ // Note: centroid contains centroid * inliers.size() at this point !
+#if 0
+ std::cerr << "----------------------------------------------------------------------------" << std::endl;
+ std::cerr << "inliers before: " << inliers.size () << std::endl;
+ std::cerr << "Centroid: " <<
+ centroid.x << ", " << centroid.y << ", " << centroid.z << ", " << std::endl;
+#endif
+
+ CovarianceMatrix covariance_matrix;
+
+ computeCovariance (make_permutation_iterator (input_->points.begin (), inliers.begin ()),
+ make_permutation_iterator (input_->points.begin (), inliers.end ()),
+ covariance_matrix, centroid);
+
+ if (isnan (covariance_matrix.data[0].x))
+ {
+ std::cerr << "Wow, covariance matrix contains nans!" << std::endl;
+ inliers_stencil = best_inliers_stencil;
+ c = make_float3 (bad_point, bad_point, bad_point);
+ //best_centroid;
+ return best_nr_inliers;
+ }
+
+#if 0
+ std::cerr << "Covariance: " <<
+ covariance_matrix.data[0].x << ", " << covariance_matrix.data[0].y << ", " << covariance_matrix.data[0].z << std::endl <<
+ covariance_matrix.data[1].x << ", " << covariance_matrix.data[1].y << ", " << covariance_matrix.data[1].z << std::endl <<
+ covariance_matrix.data[2].x << ", " << covariance_matrix.data[2].y << ", " << covariance_matrix.data[2].z << std::endl;
+#endif
+
+ CovarianceMatrix evecs;
+ float3 evals;
+
+ // compute eigenvalues and -vectors
+ eigen33 (covariance_matrix, evecs, evals);
+
+ float3 mc = normalize (evecs.data[0]);
+
+#if 0
+ std::cerr << "Eigenvectors: " <<
+ evecs.data[0].x << ", " << evecs.data[0].y << ", " << evecs.data[0].z << std::endl <<
+ evecs.data[1].x << ", " << evecs.data[1].y << ", " << evecs.data[1].z << std::endl <<
+ evecs.data[2].x << ", " << evecs.data[2].y << ", " << evecs.data[2].z << std::endl;
+ std::cerr << "Coefficients before: " <<
+ coefficients.x << ", " << coefficients.y << ", " << coefficients.z << ", " << coefficients.w << ", " << std::endl;
+#endif
+
+ // compute plane coefficients from eigenvector corr. to smallest eigenvalue and centroid
+ coefficients.x = mc.x;
+ coefficients.y = mc.y;
+ coefficients.z = mc.z;
+ // ... + d = 0
+ coefficients.w = -1 * dot (mc, centroid);
+
+#if 0
+ std::cerr << "Coefficients after: " <<
+ coefficients.x << ", " << coefficients.y << ", " << coefficients.z << ", " << coefficients.w << ", " << std::endl;
+#endif
+
+ // finally, another inlier check:
+#ifdef KINECT
+ transform (
+ input_->points.begin (), input_->points.end (),
+ //make_zip_iterator (make_tuple (input_->points.begin (), normals_.begin())),
+ //make_zip_iterator (make_tuple (input_->points.begin (), normals_.begin())) + nr_points,
+ // input_->points.begin (),
+ // input_->points.end (),
+ indices_stencil_->begin (),
+ inliers_stencil->begin (),
+ CheckPlanarInlierKinectIndices (coefficients, threshold, angle_threshold));
+#endif
+
+#ifdef KINECT_NORMALS
+ transform (
+ make_zip_iterator (make_tuple (input_->points.begin (), normals_->begin())),
+ make_zip_iterator (make_tuple (input_->points.begin (), normals_->begin())) + nr_points,
+ indices_stencil_->begin (),
+ inliers_stencil->begin (),
+ CheckPlanarInlierKinectNormalIndices (coefficients, threshold, angle_threshold));
+#endif
+
+ // copy inliers from stencil to inlier vector
+ inliers.resize (inliers_stencil->size ()); // is this necessary?
+ last = thrust::remove_copy (inliers_stencil->begin (), inliers_stencil->end (), inliers.begin (), -1);
+ inliers.erase (last, inliers.end ());
+
+ nr_inliers_after_refit = (int) inliers.size ();
+
+ compute3DCentroid (make_permutation_iterator (input_->points.begin (), inliers.begin ()),
+ make_permutation_iterator (input_->points.begin (), inliers.end ()),
+ centroid);
+
+ if (nr_inliers_after_refit > best_nr_inliers)
+ {
+ best_nr_inliers = nr_inliers_after_refit;
+ best_inliers_stencil = inliers_stencil;
+ best_centroid = centroid;
+ h[idx] = coefficients;
+ }
+
+ //fprintf (stderr, "iteration %i: %f, %f, %f, %f ---> %i\n", nr_refit_iterations, coefficients.x, coefficients.y, coefficients.z, coefficients.w, best_nr_inliers);
+
+ } while (nr_inliers_after_refit > nr_inliers_before_refit & ++nr_refit_iterations < 120);
+
+#if 0
+ std::cerr << "inliers after: " << nr_inliers_after_refit << std::endl;
+#endif
+ //std::cerr << "--> refitting steps: " << nr_refit_iterations << std::endl;
+
+ inliers_stencil = best_inliers_stencil;
+ c = best_centroid;
+ return best_nr_inliers;
+ }
+
+
+ // explicit template instantiation for device and host
+ template class PCL_EXPORTS SampleConsensusModel1PointPlane<Device>;
+ template class PCL_EXPORTS SampleConsensusModel1PointPlane<Host>;
+
+ } // namespace
+} // namespace
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include "pcl/cuda/sample_consensus/sac_model_plane.h"
+#include "pcl/cuda/cutil_math.h"
+
+#include <vector_types.h>
+#include <thrust/copy.h>
+#include <thrust/count.h>
+
+#include <stdio.h>
+#include <limits>
+
+
+namespace pcl
+{
+ namespace cuda
+ {
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage>
+ SampleConsensusModelPlane<Storage>::SampleConsensusModelPlane (
+ const PointCloudConstPtr &cloud) :
+ SampleConsensusModel<Storage> (cloud)
+ {
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> void
+ SampleConsensusModelPlane<Storage>::getSamples (int &iterations, Indices &samples)
+ {
+ samples.resize (3);
+ float trand = indices_->size () / (RAND_MAX + 1.0f);
+ for (int i = 0; i < 3; ++i)
+ {
+ int idx = (int)(rngl_ () * trand);
+ samples[i] = (*indices_)[idx];
+ }
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> bool
+ SampleConsensusModelPlane<Storage>::computeModelCoefficients (
+ const Indices &samples, Coefficients &model_coefficients)
+ {
+ if (samples.size () != 3)
+ {
+ return (false);
+ }
+
+ // Compute the segment values (in 3d) between p1 and p0
+ float3 p1p0 = ((PointXYZRGB)input_->points[samples[1]]).xyz - ((PointXYZRGB)input_->points[samples[0]]).xyz;
+ // Compute the segment values (in 3d) between p2 and p0
+ float3 p2p0 = ((PointXYZRGB)input_->points[samples[2]]).xyz - ((PointXYZRGB)input_->points[samples[0]]).xyz;
+
+ // Avoid some crashes by checking for collinearity here
+ float3 dy1dy2 = p1p0 / p2p0;
+ if ( (dy1dy2.x == dy1dy2.y) && (dy1dy2.z == dy1dy2.y) ) // Check for collinearity
+ return (false);
+
+ // Compute the plane coefficients from the 3 given points in a straightforward manner
+ // calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1)
+ float3 mc = normalize (cross (p1p0, p2p0));
+
+ if (model_coefficients.size () != 4)
+ model_coefficients.resize (4);
+ model_coefficients[0] = mc.x;
+ model_coefficients[1] = mc.y;
+ model_coefficients[2] = mc.z;
+ // ... + d = 0
+ model_coefficients[3] = -1 * dot (mc, ((PointXYZRGB)input_->points[samples[0]]).xyz);
+
+ return (true);
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage>
+ //template <typename Tuple>
+ float4
+ CreatePlaneHypothesis<Storage>::operator () (int t)
+ {
+ float4 coeff;
+ coeff.x = coeff.y = coeff.z = coeff.w = bad_value;
+
+ int3 samples;
+ float trand = nr_indices / (RAND_MAX + 1.0f);
+ thrust::default_random_engine rng (t);
+ // rng.discard (10);
+ samples.x = indices[(int)(rng () * trand)];
+ // rng.discard (20);
+ samples.y = indices[(int)(rng () * trand)];
+ // rng.discard (30);
+ samples.z = indices[(int)(rng () * trand)];
+ /* samples.x = indices[(int)(thrust::get<0>(t) * trand)];
+ samples.y = indices[(int)(thrust::get<1>(t) * trand)];
+ samples.z = indices[(int)(thrust::get<2>(t) * trand)];*/
+
+ if (isnan (input[samples.x].x) ||
+ isnan (input[samples.y].x) ||
+ isnan (input[samples.z].x))
+ return (coeff);
+
+ // Compute the segment values (in 3d) between p1 and p0
+ float3 p1p0 = input[samples.y].xyz - input[samples.x].xyz;
+ // Compute the segment values (in 3d) between p2 and p0
+ float3 p2p0 = input[samples.z].xyz - input[samples.x].xyz;
+
+ // Avoid some crashes by checking for collinearity here
+ float3 dy1dy2 = p1p0 / p2p0;
+ if ( (dy1dy2.x == dy1dy2.y) && (dy1dy2.z == dy1dy2.y) ) // Check for collinearity
+ return (coeff);
+
+ // Compute the plane coefficients from the 3 given points in a straightforward manner
+ // calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1)
+ float3 mc = normalize (cross (p1p0, p2p0));
+
+ coeff.x = mc.x;
+ coeff.y = mc.y;
+ coeff.z = mc.z;
+ // ... + d = 0
+ coeff.w = -1 * dot (mc, input[samples.x].xyz);
+
+ return (coeff);
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> bool
+ SampleConsensusModelPlane<Storage>::generateModelHypotheses (
+ Hypotheses &h, int max_iterations)
+ {
+ using namespace thrust;
+
+ // Create a vector of how many samples/coefficients do we want to get
+ h.resize (max_iterations);
+
+ typename Storage<int>::type randoms (max_iterations);
+ // a sequence counting up from 0
+ thrust::counting_iterator<int> index_sequence_begin (0);
+ // transform the range [0,1,2,...N]
+ // to a range of random numbers
+ thrust::transform (index_sequence_begin,
+ index_sequence_begin + max_iterations,
+ randoms.begin (),
+ parallel_random_generator ((int) time (0)));
+
+ thrust::counting_iterator<int> first (0);
+ // Input: Point Cloud, Indices
+ // Output: Hypotheses
+ transform (//first, first + max_iterations,
+ randoms.begin (), randoms.begin () + max_iterations,
+ h.begin (),
+ CreatePlaneHypothesis<Storage> (thrust::raw_pointer_cast (&input_->points[0]),
+ thrust::raw_pointer_cast (&(*indices_)[0]),
+ (int) indices_->size (), std::numeric_limits<float>::quiet_NaN ()));
+ return (true);
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <typename Tuple> bool
+ CountPlanarInlier::operator () (const Tuple &t)
+ {
+ if (!isfinite (thrust::get<0>(t).x))
+ return (false);
+
+ return (fabs (thrust::get<0>(t).x * coefficients.x +
+ thrust::get<0>(t).y * coefficients.y +
+ thrust::get<0>(t).z * coefficients.z + coefficients.w) < threshold);
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <typename Tuple> int
+ CheckPlanarInlier::operator () (const Tuple &t)
+ {
+ if (isnan (thrust::get<0>(t).x))
+ return (-1);
+ // Fill in XYZ (and copy NaNs with it)
+ float4 pt;
+ pt.x = thrust::get<0>(t).x;
+ pt.y = thrust::get<0>(t).y;
+ pt.z = thrust::get<0>(t).z;
+ pt.w = 1;
+
+ if (fabs (dot (pt, coefficients)) < threshold)
+ // If inlier, return its position in the vector
+ return (thrust::get<1>(t));
+ else
+ // If outlier, return -1
+ return (-1);
+ }
+
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> int
+ SampleConsensusModelPlane<Storage>::countWithinDistance (
+ const Coefficients &model_coefficients, float threshold)
+ {
+ using namespace thrust;
+
+ // Needs a valid set of model coefficients
+ if (model_coefficients.size () != 4)
+ {
+ fprintf (stderr, "[pcl::cuda::SampleConsensusModelPlane::countWithinDistance] Invalid number of model coefficients given (%lu)!\n", (unsigned long) model_coefficients.size ());
+ return 0;
+ }
+
+ float4 coefficients;
+ coefficients.x = model_coefficients[0];
+ coefficients.y = model_coefficients[1];
+ coefficients.z = model_coefficients[2];
+ coefficients.w = model_coefficients[3];
+
+ return (int) count_if (
+ make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())),
+ make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())) +
+ indices_->size (),
+ CountPlanarInlier (coefficients, threshold));
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> int
+ SampleConsensusModelPlane<Storage>::countWithinDistance (
+ const Hypotheses &h, int idx, float threshold)
+ {
+ if (isnan (((float4)h[idx]).x))
+ return (0);
+
+ return (int)
+ (thrust::count_if (
+ thrust::make_zip_iterator (thrust::make_tuple (input_->points.begin (), indices_->begin ())),
+ thrust::make_zip_iterator (thrust::make_tuple (input_->points.begin (), indices_->begin ())) +
+ indices_->size (),
+ CountPlanarInlier (h[idx], threshold)));
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> int
+ SampleConsensusModelPlane<Storage>::selectWithinDistance (
+ const Coefficients &model_coefficients, float threshold, IndicesPtr &inliers, IndicesPtr &inliers_stencil)
+ {
+ using namespace thrust;
+
+ // Needs a valid set of model coefficients
+ if (model_coefficients.size () != 4)
+ {
+ fprintf (stderr, "[pcl::cuda::SampleConsensusModelPlane::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", (unsigned long) model_coefficients.size ());
+ return 0;
+ }
+
+ int nr_points = (int) indices_->size ();
+ if (!inliers_stencil)
+ inliers_stencil.reset (new Indices());
+
+ inliers_stencil->resize (nr_points);
+
+ float4 coefficients;
+ coefficients.x = model_coefficients[0];
+ coefficients.y = model_coefficients[1];
+ coefficients.z = model_coefficients[2];
+ coefficients.w = model_coefficients[3];
+
+ // Send the data to the device
+ transform (
+ make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())),
+ make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())) +
+ nr_points,
+ inliers_stencil->begin (),
+ CheckPlanarInlier (coefficients, threshold));
+
+ if (!inliers)
+ inliers.reset (new Indices());
+ inliers->resize (nr_points);
+
+ typename Indices::iterator it = copy_if (inliers_stencil->begin (), inliers_stencil->end (), inliers->begin (), isInlier ());
+ // Copy data
+ //it = remove_copy (inliers_stencil->begin (), inliers_stencil->end (), inliers->begin (), -1);
+
+ inliers->resize (it - inliers->begin ());
+ return (int) inliers->size();
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> int
+ SampleConsensusModelPlane<Storage>::selectWithinDistance (
+ const Hypotheses &h, int idx, float threshold, IndicesPtr &inliers, IndicesPtr &inliers_stencil)
+ {
+ using namespace thrust;
+
+ // Needs a valid set of model coefficients
+ /* if (model_coefficients.size () != 4)
+ {
+ fprintf (stderr, "[pcl::cuda::SampleConsensusModelPlane::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", (unsigned long) model_coefficients.size ());
+ return;
+ }*/
+
+ int nr_points = (int) indices_->size ();
+
+ if (!inliers_stencil)
+ inliers_stencil.reset (new Indices());
+
+ inliers_stencil->resize (nr_points);
+
+ float4 coefficients;
+ coefficients.x = ((float4)h[idx]).x;
+ coefficients.y = ((float4)h[idx]).y;
+ coefficients.z = ((float4)h[idx]).z;
+ coefficients.w = ((float4)h[idx]).w;
+
+ // Send the data to the device
+ transform (
+ make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())),
+ make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())) +
+ nr_points,
+ inliers_stencil->begin (),
+ CheckPlanarInlier (coefficients, threshold));
+
+ if (!inliers)
+ inliers.reset (new Indices());
+ inliers->resize (nr_points);
+
+ // Copy data
+ typename Indices::iterator it = copy_if (inliers_stencil->begin (), inliers_stencil->end (), inliers->begin (), isInlier ());
+ inliers->resize (it - inliers->begin ());
+ return (int) inliers->size ();
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ template <template <typename> class Storage> int
+ SampleConsensusModelPlane<Storage>::selectWithinDistance (
+ Hypotheses &h, int idx, float threshold, IndicesPtr &inliers_stencil, float3 & centroid)
+ {
+ using namespace thrust;
+
+ // Needs a valid set of model coefficients
+ /* if (model_coefficients.size () != 4)
+ {
+ fprintf (stderr, "[pcl::cuda::SampleConsensusModelPlane::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", (unsigned long) model_coefficients.size ());
+ return;
+ }*/
+
+ int nr_points = (int) indices_->size ();
+
+ if (!inliers_stencil)
+ inliers_stencil.reset (new Indices());
+ inliers_stencil->resize (nr_points);
+
+ float4 coefficients;
+ coefficients.x = ((float4)h[idx]).x;
+ coefficients.y = ((float4)h[idx]).y;
+ coefficients.z = ((float4)h[idx]).z;
+ coefficients.w = ((float4)h[idx]).w;
+
+ transform (
+ make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())),
+ make_zip_iterator (make_tuple (input_->points.begin (), indices_->begin ())) +
+ nr_points,
+ inliers_stencil->begin (),
+ CheckPlanarInlier (coefficients, threshold));
+ return nr_points - (int) thrust::count (inliers_stencil->begin (), inliers_stencil->end (), -1);
+ }
+
+ template class SampleConsensusModelPlane<Device>;
+ template class SampleConsensusModelPlane<Host>;
+
+ } // namespace
+} // namespace
+
--- /dev/null
+set(SUBSYS_NAME cuda_segmentation)
+set(SUBSYS_PATH cuda/segmentation)
+set(SUBSYS_DESC "Point cloud CUDA Segmentation library")
+set(SUBSYS_DEPS cuda_common io common)
+
+# ---[ Point Cloud Library - pcl/cuda/io
+
+set(build TRUE)
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
+mark_as_advanced("BUILD_${SUBSYS_NAME}")
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS})
+PCL_SET_SUBSYS_INCLUDE_DIR("${SUBSYS_NAME}" "${SUBSYS_PATH}")
+
+
+if (build)
+ set(incs
+ include/pcl/cuda/segmentation/connected_components.h
+ )
+
+ set(srcs
+ src/connected_components.cu
+ )
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
+ PCL_CUDA_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs})
+
+ set(EXT_DEPS "")
+ #set(EXT_DEPS CUDA)
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}"
+ "${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "")
+
+ # Install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}" ${incs})
+
+endif()
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#pragma once
+
+#include <pcl/cuda/point_cloud.h>
+
+namespace pcl
+{
+ namespace cuda
+ {
+
+ template <template <typename> class Storage, typename OutT, typename InT>
+ void createIndicesImage (OutT &dst, InT ®ion_mask);
+
+ template <template <typename> class Storage, typename OutT, typename InT>
+ void createNormalsImage (const OutT &dst, InT &normals);
+
+ template <template <typename> class Storage>
+ void markInliers (const typename PointCloudAOS<Storage>::ConstPtr &input, typename Storage<int>::type ®ion_mask, std::vector<boost::shared_ptr<typename Storage<int>::type> > inlier_stencils);
+
+ template <template <typename> class Storage>
+ std::vector<typename Storage<int>::type> createRegionStencils (typename Storage<int>::type &parent, typename Storage<int>::type &rank, typename Storage<int>::type &size, int min_size, float percentage);
+
+ } // namespace
+} // namespace
+
--- /dev/null
+// this file is extracted from OpenCV/modules/gpu/src/mssegmentation.cpp
+// to get access to the intermediate results of meanShiftSegmentation()
+// minor changes to compile correctly with pcl::cuda and namespace reorganization
+
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+// By downloading, copying, installing or using the software you agree to this license.
+// If you do not agree to this license, do not download, install,
+// copy or use the software.
+//
+//
+// License Agreement
+// For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+// * Redistribution's of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+//
+// * Redistribution's in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+//
+// * The name of the copyright holders may not be used to endorse or promote products
+// derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#pragma once
+
+#include <pcl/pcl_exports.h>
+
+#include <vector>
+#include <opencv2/core/core.hpp>
+#include <opencv2/gpu/gpu.hpp>
+
+using namespace std;
+
+namespace pcl
+{
+namespace cuda
+{
+namespace detail
+{
+
+//
+// Declarations
+//
+
+class DjSets
+{
+public:
+ DjSets(int n);
+ int find(int elem);
+ int merge(int set1, int set2);
+
+ void init (int n);
+
+ vector<int> parent;
+ vector<int> rank;
+ vector<int> size;
+private:
+ DjSets(const DjSets&);
+ void operator =(const DjSets&);
+};
+
+
+template <typename T>
+struct GraphEdge
+{
+ GraphEdge() {}
+ GraphEdge(int to, int next, const T& val) : to(to), next(next), val(val) {}
+ int to;
+ int next;
+ T val;
+};
+
+
+template <typename T>
+class Graph
+{
+public:
+ typedef GraphEdge<T> Edge;
+
+ Graph(int numv, int nume_max);
+
+ void addEdge(int from, int to, const T& val=T());
+
+ vector<int> start;
+ vector<Edge> edges;
+
+ int numv;
+ int nume_max;
+ int nume;
+private:
+ Graph(const Graph&);
+ void operator =(const Graph&);
+};
+
+
+struct SegmLinkVal
+{
+ SegmLinkVal() {}
+ SegmLinkVal(int dr, int dsp) : dr(dr), dsp(dsp) {}
+ bool operator <(const SegmLinkVal& other) const
+ {
+ return dr + dsp < other.dr + other.dsp;
+ }
+ int dr;
+ int dsp;
+};
+
+
+struct SegmLink
+{
+ SegmLink() {}
+ SegmLink(int from, int to, const SegmLinkVal& val)
+ : from(from), to(to), val(val) {}
+ bool operator <(const SegmLink& other) const
+ {
+ return val < other.val;
+ }
+ int from;
+ int to;
+ SegmLinkVal val;
+};
+
+//
+// Implementation
+//
+
+DjSets::DjSets(int n)
+{
+ init (n);
+}
+
+
+inline int DjSets::find(int elem)
+{
+ int set = elem;
+ while (set != parent[set])
+ set = parent[set];
+ while (elem != parent[elem])
+ {
+ int next = parent[elem];
+ parent[elem] = set;
+ elem = next;
+ }
+ return set;
+}
+
+inline void DjSets::init(int n)
+{
+ parent.resize(n);
+ rank.resize(n, 0);
+ size.resize(n, 1);
+ for (int i = 0; i < n; ++i)
+ parent[i] = i;
+}
+
+inline int DjSets::merge(int set1, int set2)
+{
+ if (rank[set1] < rank[set2])
+ {
+ parent[set1] = set2;
+ size[set2] += size[set1];
+ return set2;
+ }
+ if (rank[set2] < rank[set1])
+ {
+ parent[set2] = set1;
+ size[set1] += size[set2];
+ return set1;
+ }
+ parent[set1] = set2;
+ rank[set2]++;
+ size[set2] += size[set1];
+ return set2;
+}
+
+
+template <typename T>
+Graph<T>::Graph(int numv, int nume_max) : start(numv, -1), edges(nume_max)
+{
+ this->numv = numv;
+ this->nume_max = nume_max;
+ nume = 0;
+}
+
+
+template <typename T>
+inline void Graph<T>::addEdge(int from, int to, const T& val)
+{
+ edges[nume] = Edge(to, start[from], val);
+ start[from] = nume;
+ nume++;
+}
+
+
+inline int pix(int y, int x, int ncols)
+{
+ return y * ncols + x;
+}
+
+
+inline int sqr(int x)
+{
+ return x * x;
+}
+
+
+inline int dist2(const cv::Vec4b& lhs, const cv::Vec4b& rhs)
+{
+ return sqr(lhs[0] - rhs[0]) + sqr(lhs[1] - rhs[1]) + sqr(lhs[2] - rhs[2]);
+}
+
+
+inline int dist2(const cv::Vec2s& lhs, const cv::Vec2s& rhs)
+{
+ return sqr(lhs[0] - rhs[0]) + sqr(lhs[1] - rhs[1]);
+}
+
+} // namespace
+
+PCL_EXPORTS void meanShiftSegmentation(const cv::gpu::GpuMat& src, cv::Mat& dst, int sp, int sr, int minsize, detail::DjSets &comps, cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 5, 1) );
+
+} // namespace
+} // namespace
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <pcl/pcl_exports.h>
+
+#include <pcl/cuda/point_cloud.h>
+//#include <pcl/cuda/segmentation/connected_components.h>
+#include <thrust/transform.h>
+
+//struct ConnectedComponentSegmentation
+//{
+// ConnectedComponentSegmentation () {}
+//
+// __device__ __host__ __inline__
+// void operator () {}
+//};
+
+namespace pcl
+{
+ namespace cuda
+ {
+ struct InlierLabeling
+ {
+ InlierLabeling (int** stencils, int nr_stencils)
+ : nr_stencils_(nr_stencils)
+ , stencils_(stencils)
+ {
+ }
+
+ __inline__ __host__ __device__
+ int operator () (int idx)
+ {
+ int ret = -1;
+ for (int i = 0; i < nr_stencils_; ++i)
+ {
+ if (stencils_[i][idx] != -1)
+ {
+ ret = (int) ((i / (float)nr_stencils_) * 255);
+ }
+ }
+ return ret;
+ }
+
+ int nr_stencils_;
+ int ** stencils_;
+ };
+
+
+
+ template <template <typename> class Storage>
+ void markInliers (const typename PointCloudAOS<Storage>::ConstPtr &input, typename Storage<int>::type ®ion_mask, std::vector<boost::shared_ptr<typename Storage<int>::type> > inlier_stencils)
+ {
+ region_mask.resize (input->points.size());
+ //int** stencils = new int*[inlier_stencils.size()];
+ thrust::host_vector<int*> stencils_host (inlier_stencils.size ());
+ for (int i = 0; i < inlier_stencils.size (); ++i)
+ stencils_host[i] = thrust::raw_pointer_cast(&(*inlier_stencils[i])[0]);
+ //stencils_host[i] = thrust::raw_pointer_cast<int> (&(*inlier_stencils[i])[0]);
+
+ typename Storage<int*>::type stencils_storage = stencils_host;
+ int** stencils = thrust::raw_pointer_cast(&stencils_storage[0]);
+
+ thrust::counting_iterator<int> first (0);
+ thrust::counting_iterator<int> last = first + region_mask.size ();
+ thrust::transform (first, last, region_mask.begin (), InlierLabeling (stencils, (int) inlier_stencils.size ()));
+ }
+
+ //struct ConvertToImage1
+ //{
+ // ConvertToImage1 () {}
+ // __host__ __device__ __inline__ unsigned char operator () (unsigned char i)
+ // {
+ // return i;
+ // }
+ //};
+ //
+ //struct ConvertToImage3
+ //{
+ // __host__ __device__ __inline__ OpenNIRGB operator () (unsigned char i)
+ // {
+ // OpenNIRGB r;
+ // r.r = i;
+ // return r;
+ // }
+ //};
+ //
+ //struct ConvertToImage4
+ //{
+ // ConvertToImage4 () {}
+ // __host__ __device__ __inline__ int operator () (unsigned char i)
+ // {
+ // return i;
+ // }
+ //};
+
+ struct ConvertToChar4
+ {
+ ConvertToChar4 () {}
+ __host__ __device__ __inline__ char4 operator () (float4 n)
+ {
+ char4 r;
+ r.x = (char) ((1.0f + n.x) * 128);
+ r.y = (char) ((1.0f + n.y) * 128);
+ r.z = (char) ((1.0f + n.z) * 128);
+ r.w = (char) ((1.0f + n.w) * 128);
+ return r;
+ }
+ };
+
+ // createIndicesImage
+ template <template <typename> class Storage, typename OutT, typename InT>
+ void createIndicesImage (OutT &dst, InT ®ion_mask)
+ {
+ thrust::copy (region_mask.begin (), region_mask.end (), dst);
+ }
+
+ // createNormalsImage
+ template <template <typename> class Storage, typename OutT, typename InT>
+ void createNormalsImage (const OutT &dst, InT &normals)
+ {
+ thrust::transform (normals.begin (), normals.end (), dst, ConvertToChar4 ());
+ }
+
+ // findConnectedComponents
+ template <template <typename> class Storage>
+ void findConnectedComponents (const typename PointCloudAOS<Storage>::ConstPtr &input, typename Storage<int>::type ®ion_mask)
+ {
+
+
+ }
+
+ // createRegionStencils
+ template <template <typename> class Storage> std::vector<typename Storage<int>::type>
+ createRegionStencils (typename Storage<int>::type &parent,
+ typename Storage<int>::type &rank,
+ typename Storage<int>::type &size,
+ int min_size,
+ float percentage)
+ {
+ return (std::vector<typename Storage<int>::type> ());
+ }
+
+
+ template PCL_EXPORTS void markInliers<Device> (const typename PointCloudAOS<Device>::ConstPtr &input, Device<int>::type ®ion_mask, std::vector<boost::shared_ptr<Device<int>::type> > inlier_stencils);
+ template PCL_EXPORTS void markInliers<Host> (const typename PointCloudAOS<Host> ::ConstPtr &input, Host<int>::type ®ion_mask, std::vector<boost::shared_ptr<Host<int>::type> > inlier_stencils);
+
+
+ template PCL_EXPORTS void createIndicesImage<Device,
+ StoragePointer<Device,unsigned char>::type,
+ Device<int>::type>
+ (StoragePointer<Device,unsigned char>::type &dst, typename Device<int>::type®ion_mask);
+ template PCL_EXPORTS void createIndicesImage<Host,
+ typename StoragePointer<Host,unsigned char>::type,
+ typename Host<int>::type>
+ (typename StoragePointer<Host,unsigned char>::type &dst, typename Host<int>::type ®ion_mask);
+
+
+ template PCL_EXPORTS void createNormalsImage<Device,
+ StoragePointer<Device,char4>::type,
+ Device<float4>::type>
+ (const StoragePointer<Device,char4>::type &dst, typename Device<float4>::type®ion_mask);
+ template PCL_EXPORTS void createNormalsImage<Host,
+ typename StoragePointer<Host,char4>::type,
+ typename Host<float4>::type>
+ (const typename StoragePointer<Host,char4>::type &dst, typename Host<float4>::type ®ion_mask);
+
+ } // namespace
+} // namespace
+
--- /dev/null
+// this file is copied from OpenCV/modules/gpu/src/mssegmentation.cpp
+// to get access to the intermediate results of meanShiftSegmentation()
+// minor changes to compile correctly with pcl::cuda and namespace reorganization
+
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+// By downloading, copying, installing or using the software you agree to this license.
+// If you do not agree to this license, do not download, install,
+// copy or use the software.
+//
+//
+// License Agreement
+// For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+// * Redistribution's of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+//
+// * Redistribution's in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+//
+// * The name of the copyright holders may not be used to endorse or promote products
+// derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include <pcl/cuda/segmentation/mssegmentation.h>
+
+using namespace std;
+
+// Auxiliray stuff
+namespace pcl
+{
+namespace cuda
+{
+
+using namespace cv;
+using namespace cv::gpu;
+using namespace pcl::cuda::detail;
+void meanShiftSegmentation(const GpuMat& src, Mat& dst, int sp, int sr, int minsize, DjSets &comps, TermCriteria criteria)
+
+{
+ CV_Assert(TargetArchs::builtWith(FEATURE_SET_COMPUTE_12) && DeviceInfo().supports(FEATURE_SET_COMPUTE_12));
+
+ CV_Assert(src.type() == CV_8UC4);
+ const int nrows = src.rows;
+ const int ncols = src.cols;
+ const int hr = sr;
+ const int hsp = sp;
+
+ // Perform mean shift procedure and obtain region and spatial maps
+ GpuMat h_rmap, h_spmap;
+ meanShiftProc(src, h_rmap, h_spmap, sp, sr, criteria);
+ Mat rmap = (Mat)h_rmap;
+ Mat spmap = (Mat)h_spmap;
+
+ Graph<SegmLinkVal> g(nrows * ncols, 4 * (nrows - 1) * (ncols - 1)
+ + (nrows - 1) + (ncols - 1));
+
+ // Make region adjacent graph from image
+ Vec4b r1;
+ Vec4b r2[4];
+ Vec2s sp1;
+ Vec2s sp2[4];
+ int dr[4];
+ int dsp[4];
+ for (int y = 0; y < nrows - 1; ++y)
+ {
+ Vec4b* ry = rmap.ptr<Vec4b>(y);
+ Vec4b* ryp = rmap.ptr<Vec4b>(y + 1);
+ Vec2s* spy = spmap.ptr<Vec2s>(y);
+ Vec2s* spyp = spmap.ptr<Vec2s>(y + 1);
+ for (int x = 0; x < ncols - 1; ++x)
+ {
+ r1 = ry[x];
+ sp1 = spy[x];
+
+ r2[0] = ry[x + 1];
+ r2[1] = ryp[x];
+ r2[2] = ryp[x + 1];
+ r2[3] = ryp[x];
+
+ sp2[0] = spy[x + 1];
+ sp2[1] = spyp[x];
+ sp2[2] = spyp[x + 1];
+ sp2[3] = spyp[x];
+
+ dr[0] = dist2(r1, r2[0]);
+ dr[1] = dist2(r1, r2[1]);
+ dr[2] = dist2(r1, r2[2]);
+ dsp[0] = dist2(sp1, sp2[0]);
+ dsp[1] = dist2(sp1, sp2[1]);
+ dsp[2] = dist2(sp1, sp2[2]);
+
+ r1 = ry[x + 1];
+ sp1 = spy[x + 1];
+
+ dr[3] = dist2(r1, r2[3]);
+ dsp[3] = dist2(sp1, sp2[3]);
+
+ g.addEdge(pix(y, x, ncols), pix(y, x + 1, ncols), SegmLinkVal(dr[0], dsp[0]));
+ g.addEdge(pix(y, x, ncols), pix(y + 1, x, ncols), SegmLinkVal(dr[1], dsp[1]));
+ g.addEdge(pix(y, x, ncols), pix(y + 1, x + 1, ncols), SegmLinkVal(dr[2], dsp[2]));
+ g.addEdge(pix(y, x + 1, ncols), pix(y + 1, x, ncols), SegmLinkVal(dr[3], dsp[3]));
+ }
+ }
+ for (int y = 0; y < nrows - 1; ++y)
+ {
+ r1 = rmap.at<Vec4b>(y, ncols - 1);
+ r2[0] = rmap.at<Vec4b>(y + 1, ncols - 1);
+ sp1 = spmap.at<Vec2s>(y, ncols - 1);
+ sp2[0] = spmap.at<Vec2s>(y + 1, ncols - 1);
+ dr[0] = dist2(r1, r2[0]);
+ dsp[0] = dist2(sp1, sp2[0]);
+ g.addEdge(pix(y, ncols - 1, ncols), pix(y + 1, ncols - 1, ncols), SegmLinkVal(dr[0], dsp[0]));
+ }
+ for (int x = 0; x < ncols - 1; ++x)
+ {
+ r1 = rmap.at<Vec4b>(nrows - 1, x);
+ r2[0] = rmap.at<Vec4b>(nrows - 1, x + 1);
+ sp1 = spmap.at<Vec2s>(nrows - 1, x);
+ sp2[0] = spmap.at<Vec2s>(nrows - 1, x + 1);
+ dr[0] = dist2(r1, r2[0]);
+ dsp[0] = dist2(sp1, sp2[0]);
+ g.addEdge(pix(nrows - 1, x, ncols), pix(nrows - 1, x + 1, ncols), SegmLinkVal(dr[0], dsp[0]));
+ }
+
+ comps.init (g.numv);
+
+ // Find adjacent components
+ for (int v = 0; v < g.numv; ++v)
+ {
+ for (int e_it = g.start[v]; e_it != -1; e_it = g.edges[e_it].next)
+ {
+ int c1 = comps.find(v);
+ int c2 = comps.find(g.edges[e_it].to);
+ if (c1 != c2 && g.edges[e_it].val.dr < hr && g.edges[e_it].val.dsp < hsp)
+ comps.merge(c1, c2);
+ }
+ }
+
+ vector<SegmLink> edges;
+ edges.reserve(g.numv);
+
+ // Prepare edges connecting differnet components
+ for (int v = 0; v < g.numv; ++v)
+ {
+ int c1 = comps.find(v);
+ for (int e_it = g.start[v]; e_it != -1; e_it = g.edges[e_it].next)
+ {
+ int c2 = comps.find(g.edges[e_it].to);
+ if (c1 != c2)
+ edges.push_back(SegmLink(c1, c2, g.edges[e_it].val));
+ }
+ }
+
+ // Sort all graph's edges connecting differnet components (in asceding order)
+ sort(edges.begin(), edges.end());
+
+ // Exclude small components (starting from the nearest couple)
+ for (size_t i = 0; i < edges.size(); ++i)
+ {
+ int c1 = comps.find(edges[i].from);
+ int c2 = comps.find(edges[i].to);
+ if (c1 != c2 && (comps.size[c1] < minsize || comps.size[c2] < minsize))
+ comps.merge(c1, c2);
+ }
+
+ // Compute sum of the pixel's colors which are in the same segment
+ Mat h_src = (Mat)src;
+ vector<Vec4i> sumcols(nrows * ncols, Vec4i(0, 0, 0, 0));
+ for (int y = 0; y < nrows; ++y)
+ {
+ Vec4b* h_srcy = h_src.ptr<Vec4b>(y);
+ for (int x = 0; x < ncols; ++x)
+ {
+ int parent = comps.find(pix(y, x, ncols));
+ Vec4b col = h_srcy[x];
+ Vec4i& sumcol = sumcols[parent];
+ sumcol[0] += col[0];
+ sumcol[1] += col[1];
+ sumcol[2] += col[2];
+ }
+ }
+
+ // Create final image, color of each segment is the average color of its pixels
+ dst.create(src.size(), src.type());
+
+ for (int y = 0; y < nrows; ++y)
+ {
+ Vec4b* dsty = dst.ptr<Vec4b>(y);
+ for (int x = 0; x < ncols; ++x)
+ {
+ int parent = comps.find(pix(y, x, ncols));
+ const Vec4i& sumcol = sumcols[parent];
+ Vec4b& dstcol = dsty[x];
+ dstcol[0] = static_cast<uchar>(sumcol[0] / comps.size[parent]);
+ dstcol[1] = static_cast<uchar>(sumcol[1] / comps.size[parent]);
+ dstcol[2] = static_cast<uchar>(sumcol[2] / comps.size[parent]);
+ }
+ }
+}
+} // namespace
+} // namespace
+
+
{
public:
MyException (const std::string& error_description,
- const std::string& file_name = "",
- const std::string& function_name = "",
- unsigned line_number = 0) throw ()
+ const char* file_name = NULL,
+ const char* function_name = NULL,
+ unsigned line_number = 0)
: pcl::PCLException (error_description, file_name, function_name, line_number) { }
};
{
std::ostringstream s;
s << message;
- throw ExceptionName (s.str (), __FILE__, "", __LINE__);
+ throw ExceptionName (s.str (), __FILE__, BOOST_CURRENT_FUNCTION, __LINE__);
}
Then in your code, add:
--- /dev/null
+;;; pcl-c-style.el --- Pcl's C/C++ style for c-mode\r
+;;\r
+;; Keywords: c, tools\r
+;;\r
+;;; pcl-c-style.el \r
+;;\r
+;;; Software License Agreement (BSD License)\r
+;;\r
+;; Point Cloud Library (PCL) - www.pointclouds.org\r
+;; Copyright (c) 2012, Willow Garage, Inc.\r
+;;\r
+;; All rights reserved.\r
+;;\r
+;; Redistribution and use in source and binary forms, with or without\r
+;; modification, are permitted provided that the following conditions\r
+;; are met:\r
+;;\r
+;; * Redistributions of source code must retain the above copyright\r
+;; notice, this list of conditions and the following disclaimer.\r
+;; * Redistributions in binary form must reproduce the above\r
+;; copyright notice, this list of conditions and the following\r
+;; disclaimer in the documentation and/or other materials provided\r
+;; with the distribution.\r
+;; * Neither the name of Willow Garage, Inc. nor the names of its\r
+;; contributors may be used to endorse or promote products derived\r
+;; from this software without specific prior written permission.\r
+;;\r
+;; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\r
+;; "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\r
+;; LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\r
+;; FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\r
+;; COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\r
+;; INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\r
+;; BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\r
+;; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\r
+;; CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\r
+;; LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\r
+;; ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\r
+;; POSSIBILITY OF SUCH DAMAGE.\r
+;;\r
+;;; Commentary:\r
+;;\r
+;; Provides the pcl C/C++ coding style. You may wish to add\r
+;; `pcl-set-c-style' to your `c-mode-common-hook' after requiring this\r
+;; file. For example:\r
+;;\r
+;; (add-hook 'c-mode-common-hook 'pcl-set-c-style)\r
+;;\r
+\r
+;; Consider files with ".h" extension as c++\r
+(setq auto-mode-alist (cons '("\\.h\\'" . c++-mode) auto-mode-alist))\r
+\r
+;; Require cc-mode\r
+(require 'cc-mode)\r
+\r
+;; Define c++ access keys\r
+(setq c-C++-access-key "\\<\\(slots\\|signals\\|private\\|protected\\|public\\)\\>[ \t]*[(slots\\|signals)]*[ \t]*:")\r
+\r
+;; An inline function that return true if we are inside a class\r
+(defsubst pcl-at-inclass-topmost-intro (sintax)\r
+ (eq (caar sintax) 'inclass))\r
+\r
+\r
+;; Indent after access keys if any\r
+;; This code was adapted from here \r
+;; http://lists.gnu.org/archive/html/help-gnu-emacs/2009-09/msg00593.html\r
+(defun pcl-indent-after-access-label-maybe (langelem)\r
+ "Give an extra level of indentation to class members under an access\r
+specifier, e.g.:\r
+\r
+public\r
+A(); <========== extra level of indentation.\r
+\r
+This should really be properly implemented in CC Mode, but it's not."\r
+ (and\r
+ (let (m-type)\r
+ (when\r
+ (save-excursion\r
+ (back-to-indentation)\r
+ ;; Go back one "statement" at a time till we reach a label or\r
+ ;; something which isn't an inclass-topmost-intro\r
+ (while\r
+ (and (eq (setq m-type (c-beginning-of-statement-1)) 'previous)\r
+ (pcl-at-inclass-topmost-intro (c-guess-basic-syntax))))\r
+ ;; Have we found "private:", "public": or "protected"?\r
+ (and (eq m-type 'label)\r
+ (looking-at c-C++-access-key)))\r
+ (save-excursion\r
+ (back-to-indentation)\r
+ c-basic-offset)))))\r
+\r
+(defconst pcl-c-style\r
+ `("gnu"\r
+ (c-recognize-knr-p . nil)\r
+ (c-enable-xemacs-performance-kludge-p . t) ; speed up indentation in XEmacs\r
+ (c-basic-offset . 2)\r
+;; (c-tab-always-indent . t)\r
+ (c-comment-only-line-offset . 0)\r
+ (c-hanging-braces-alist . ((defun-open before)\r
+ (defun-open after)\r
+ (namespace-open before)\r
+ (namespace-open after)))\r
+ (c-hanging-colons-alist . ((member-init-intro before)\r
+ (inher-intro)))\r
+ (c-cleanup-list . (scope-operator\r
+ empty-defun-braces\r
+ defun-close-semi\r
+ space-before-funcall))\r
+ (c-offsets-alist\r
+ (inline-open . +)\r
+ (inclass . +)\r
+ (access-label . 0)\r
+ (topmost-intro . pcl-indent-after-access-label-maybe)\r
+ (topmost-intro-cont 0)\r
+ (statement-cont . 0)\r
+ (substatement-open . 0)\r
+ (substatement-label . 0)\r
+ (statement-case-open . 0)\r
+ (case-label . +)\r
+ ))\r
+ "PCL C/C++ Programming Style")\r
+\r
+(defun pcl-set-c-style ()\r
+ "Set the current buffer's c-style to PCL C/C++ Programming\r
+ Style. Meant to be added to `c-mode-common-hook'."\r
+ (interactive)\r
+ (setq tab-width 2\r
+ ;; this will make sure spaces are used instead of tabs\r
+ indent-tabs-mode nil)\r
+ (c-add-style "PCL" pcl-c-style t)\r
+ (c-toggle-auto-newline 1)\r
+ (c-toggle-hungry-state 1))\r
+\r
+;; Switch from *.<impl> to *.<head> and vice versa\r
+(defun switch-cpp-or-hpp-to-h ()\r
+ (interactive)\r
+ (when (string-match "^\\(.*\\)/\\(.*\\)\\.\\([^.]*\\)$" buffer-file-name)\r
+ (let ((path (match-string 1 buffer-file-name))\r
+ (name (match-string 2 buffer-file-name))\r
+ (suffix (match-string 3 buffer-file-name)))\r
+ (cond ((string-match suffix "cpp")\r
+ (when (string-match "^\\(.*\\)/\\(.*\\)/src$" path)\r
+ (let ((pcl-root (match-string 1 path))\r
+ (subsys (match-string 2 path)))\r
+ (if (file-exists-p (concat pcl-root "/" subsys "/include/pcl/" subsys "/" name ".h"))\r
+ (find-file (concat pcl-root "/" subsys "/include/pcl/" subsys "/" name ".h")))))\r
+ (cond ((file-exists-p (concat name ".hpp"))\r
+ (find-file (concat name ".hpp"))\r
+ )))\r
+ ((string-match suffix "h")\r
+ (message "in h")\r
+ (cond ((file-exists-p (concat path "/impl/" name ".hpp"))\r
+ (find-file (concat path "/impl/" name ".hpp"))\r
+ )\r
+ ((file-exists-p (concat name ".cpp"))\r
+ (find-file (concat name ".cpp"))\r
+ )))\r
+ ((string-match suffix "hpp")\r
+ (message "in hpp")\r
+ (cond ((file-exists-p (concat path "/../" name ".h"))\r
+ (find-file (concat path "/../" name ".h"))\r
+ )\r
+ ))\r
+ ))))\r
+\r
+(provide 'pcl-c-style)\r
+;;; pcl-c-style.el\r
""""""""""""
You can use the following `PCL C/C++ style file
-<http://dev.pointclouds.org/attachments/download/748/pcl-c-style.el>`_,
+<https://raw.githubusercontent.com/PointCloudLibrary/pcl/master/doc/advanced/content/files/pcl-c-style.el>`_,
download it to some known location and then:
* open .emacs
"@PCL_SOURCE_DIR@/surface/include/pcl/surface/on_nurbs" \
"@PCL_SOURCE_DIR@/surface/include/pcl/surface/openNURBS" \
"@PCL_SOURCE_DIR@/surface/include/pcl/surface/impl/nurbs" \
+ "@PCL_SOURCE_DIR@/simulation" \
"@PCL_SOURCE_DIR@/test/gtest-1.6.0"
EXCLUDE_SYMLINKS = YES
EXCLUDE_PATTERNS =
#---------------------------------------------------------------------------
GENERATE_XML = NO
XML_OUTPUT = xml
-XML_SCHEMA =
-XML_DTD =
XML_PROGRAMLISTING = YES
#---------------------------------------------------------------------------
#PREDEFINED = protected=private \
PREDEFINED = = "HAVE_QHULL=1" \
"HAVE_OPENNI=1" \
+ "HAVE_ENSENSO=1" \
+ "HAVE_DAVIDSDK=1" \
+ "HAVE_DSSDK=1" \
+ "HAVE_RSSDK=1" \
"PCL_DEPRECATED(x)="
EXPAND_AS_DEFINED =
SKIP_FUNCTION_MACROS = YES
--- /dev/null
+.. _compiling_pcl_posix:
+
+====================================================
+Compiling PCL from source on POSIX compliant systems
+====================================================
+
+Though not a dependency per se, don’t forget that you also need the `CMake build system <http://www.cmake.org/download/>`_, at least version 2.8.3.
+Additional help on how to use the CMake build system is available `here <http://www.pointclouds.org/documentation/tutorials/building_pcl.php#building-pcl>`_.
+
+Please note that the following installation instructions are only valid for POSIX systems (e.g., Linux, MacOS) with an already installed make/gnu toolchain.
+For instructions on how to download and compile PCL in Windows (which uses a slightly different process), please visit
+`our tutorials page <http://www.pointclouds.org/documentation/tutorials/index.php>`_.
+
+.. contents::
+
+Stable
+======
+
+For systems for which we do not offer precompiled binaries, you need to compile Point Cloud Library (PCL) from source. Here are the steps that you need to take:
+Go to `Github <https://github.com/PointCloudLibrary/pcl/releases>`_ and download the version number of your choice.
+Uncompress the tar-bzip archive, e.g. (replace 1.7.2 with the correct version number)::
+
+ tar xvfj pcl-pcl-1.7.2.tar.gz
+
+Change the directory to the pcl-pcl-1.7.2 (replace 1.7.2 with the correct version number) directory, and create a build directory in there::
+
+ cd pcl-pcl-1.7.2 && mkdir build && cd build
+
+Run the CMake build system using the default options::
+
+ cmake ..
+
+Or change them (uses cmake-curses-gui)::
+
+ ccmake ..
+
+Please note that cmake might default to a debug build. If you want to compile a release build of PCL with enhanced compiler optimizations, you can change the build target to "Release" with "-DCMAKE_BUILD_TYPE=Release"::
+
+ cmake -DCMAKE_BUILD_TYPE=Release ..
+
+Finally compile everything (see `compiler_optimizations <http://www.pointclouds.org/documentation/advanced/compiler_optimizations.php>`_)::
+
+ make -j2
+
+And install the result::
+
+ make -j2 install
+
+Or alternatively, if you did not change the variable which declares where PCL should be installed, do::
+
+ sudo make -j2 install
+
+Here's everything again, in case you want to copy & paste it::
+
+ cd pcl-pcl-1.7.2 && mkdir build && cd build
+ cmake -DCMAKE_BUILD_TYPE=Release ..
+ make -j2
+ sudo make -j2 install
+
+Again, for a detailed tutorial on how to compile and install PCL and its dependencies in Microsoft Windows, please visit `our tutorials page <http://www.pointclouds.org/documentation/tutorials/index.php>`_. Additional information for developers is available at the `Github PCL Wiki <https://github.com/PointCloudLibrary/pcl/wiki>`_.
+
+Experimental
+============
+If you are eager to try out a certain feature of PCL that is currently under development (or you plan on developing and contributing to PCL), we recommend you try checking out our source repository, as shown below. If you're just interested in browsing our source code, you can do so by visiting `https://github.com/PointCloudLibrary/pcl <https://github.com/PointCloudLibrary/pcl>`_.
+
+Clone the repository::
+
+ git clone https://github.com/PointCloudLibrary/pcl pcl-trunk
+
+Please note that above steps (3-5) are almost identical for compiling the experimental PCL trunk code::
+
+ cd pcl-trunk && mkdir build && cd build
+ cmake -DCMAKE_BUILD_TYPE=RelWithDebInfo ..
+ make -j2
+ sudo make -j2 install
+
+
+Dependencies
+============
+Because PCL is split into a list of code libraries, the list of dependencies differs based on what you need to compile. The difference between mandatory and optional dependencies, is that a mandatory dependency is required in order for that particular PCL library to compile and function, while an optional dependency disables certain functionality within a PCL library but compiles the rest of the library that does not require the dependency.
+
+Mandatory
+---------
+The following code libraries are **required** for the compilation and usage of the PCL libraries shown below:
+
+.. note::
+pcl_* denotes all PCL libraries, meaning that the particular dependency is a strict requirement for the usage of anything in PCL.
+
++---------------------------------------------------------------+-----------------+-------------------------+-------------------+
+| Logo | Library | Minimum version | Mandatory |
++===============================================================+=================+=========================+===================+
+| .. image:: images/posix_building_pcl/boost_logo.png | Boost | | 1.40 (without OpenNI) | pcl_* |
+| | | | 1.47 (with OpenNI) | |
++---------------------------------------------------------------+-----------------+-------------------------+-------------------+
+| .. image:: images/posix_building_pcl/eigen_logo.png | Eigen | 3.0 | pcl_* |
++---------------------------------------------------------------+-----------------+-------------------------+-------------------+
+| .. image:: images/posix_building_pcl/flann_logo.png | FLANN | 1.7.1 | pcl_* |
++---------------------------------------------------------------+-----------------+-------------------------+-------------------+
+| .. image:: images/posix_building_pcl/vtk_logo.png | VTK | 5.6 | pcl_visualization |
++---------------------------------------------------------------+-----------------+-------------------------+-------------------+
+
+Optional
+--------
+The following code libraries enable certain additional features for the PCL libraries shown below, and are thus **optional**:
+
++---------------------------------------------------------------+-----------------+-------------------+-------------------+
+| Logo | Library | Minimum version | Mandatory |
++===============================================================+=================+===================+===================+
+| .. image:: images/posix_building_pcl/qhull_logo.png | Qhull | 2011.1 | pcl_surface |
++---------------------------------------------------------------+-----------------+-------------------+-------------------+
+| .. image:: images/posix_building_pcl/openni_logo.png | OpenNI | 1.3 | pcl_io |
++---------------------------------------------------------------+-----------------+-------------------+-------------------+
+| .. image:: images/posix_building_pcl/cuda_logo.png | CUDA | 4.0 | pcl_* |
++---------------------------------------------------------------+-----------------+-------------------+-------------------+
+
+Troubleshooting
+===============
+In certain situations, the instructions above might fail, either due to custom versions of certain library dependencies installed, or different operating systems than the ones we usually develop on, etc. This section here contains links to discussions held in our community regarding such cases. Please read it before posting new questions on the mailing list, and also **use the search features provided by our forums** - there's no point in starting a new thread if an older one that discusses the same issue already exists.
+
+MacOS X
+-------
+`libGL issue when running visualization apps on OSX <http://www.pcl-users.org/libGL-issue-when-running-visualization-apps-on-OSX-td3574302.html#a3574775>`_
--- /dev/null
+.. _david_sdk:
+
+=====================================================
+Grabbing point clouds / meshes from davidSDK scanners
+=====================================================
+
+In this tutorial we will learn how to use the `davidSDK <http://www.david-3d.com/en/products/david-sdk>`_ through PCL. This tutorial will show you how to configure PCL and how to use the examples to fetch point clouds/meshes/images from a davidSDK compliant device (such as the `SLS-2 <http://www.david-3d.com/en/products/sls-2>`_).
+
+.. contents::
+
+Install davidSDK
+================
+
+You need a davidSDK to run the SDK on the server side, the official davidSDK does not come with a Makefile or a CMake project. An un-official fork provides a CMake project that enables to easily use the SDK under Linux (with minor tweaks)
+
+ * `Official davidSDK download page <http://www.david-3d.com/en/support/downloads>`_
+ * `Victor Lamoine davidSDK fork <https://github.com/InstitutMaupertuis/davidSDK>`_
+
+Please test `the example project <https://github.com/InstitutMaupertuis/davidSDK/blob/master/README.md#example-project-using-the-davidsdk>`_ before going further.
+
+.. note:: If you use the trial version of the server, the only format available is OBJ (used by default)
+
+Configuring PCL
+===============
+
+You need at least PCL 1.8.0 to be able to use the davidSDK. You need to make sure ``WITH_DAVIDSDK`` is set to ``true`` in the CMake configuration (it should be set to true by default if you have used the un-official davidSDK fork).
+
+The default following values can be tweaked into CMake if you don't have a standard installation, for example:
+
+.. code-block:: cmake
+
+ DAVIDSDK_ABI_DIR /opt/davidsdk
+
+You can deactivate building the davidSDK support by setting ``BUILD_DAVIDSDK`` to false. Compile and install PCL.
+
+Platform specific directives
+============================
+
+It should be easy to use the davidSDK PCL support if you are using PCL on the davidSDK server; the meshes are locally exported on the storage drive and then loaded into PCL as point clouds/meshes. If you are using a Linux distribution you will need to configure more things for the davidSDK PCL implementation to work, create a temporary directory for the davidSDK meshes storage:
+
+.. code-block:: bash
+
+ mkdir -p /var/tmp/davidsdk
+ sudo chmod 755 /var/tmp/davidsdk
+
+Edit samba configuration (samba must be installed first):
+
+.. code-block:: bash
+
+ echo -e "[davidsdk]\n\
+ path = /var/tmp/davidsdk\n\
+ public = yes\n\
+ writeable = yes\n\
+ browseable = yes\n\
+ guest ok = yes\n\
+ create mask = 0775" |\
+ sudo tee -a /etc/samba/smb.conf
+
+Restard samba server:
+
+.. code-block:: bash
+
+ sudo service smbd restart
+
+Use the :pcl:`setLocalAndRemotePaths <pcl::DavidSDKGrabber::setLocalAndRemotePaths>` function to set the local and remote paths, if you use the same path as above; this doesn't have to be called if the server is running of the same machine as the client.
+
+.. code-block:: cpp
+
+ davidsdk_ptr->setLocalAndRemotePaths ("/var/tmp/davidsdk/", "\\\\name_of_machine\\davidsdk\\");
+
+.. note::
+
+ If you get a Error_Fail = -107 error, it is most probably a write access missing in the temporary directory.
+
+File formats
+============
+
+Three file formats are available to export the meshes / clouds.
+
+* STL: No texture support, binary format
+* OBJ: Texture support, no binary format available
+* PLY: Texture support, binary format is available but davidSDK uses ASCII format
+
+Use the :pcl:`setFileFormatToOBJ <pcl::DavidSDKGrabber::setFileFormatToOBJ>`,
+:pcl:`setFileFormatToPLY <pcl::DavidSDKGrabber::setFileFormatToPLY>`,
+:pcl:`setFileFormatToSTL <pcl::DavidSDKGrabber::setFileFormatToSTL>` to choose between the different formats.
+
+The default format used is OBJ. (it is compatible with davidSDK server trial version)
+
+Calibration
+===========
+
+In order to use the davidSDK scanner the camera and the projector must be calibrated. This can be done by calling the :pcl:`calibrate <pcl::DavidSDKGrabber::calibrate>` function of the DavidSDKGrabber object, if the calibration fails, please check `the wiki <http://wiki.david-3d.com/david-wiki>`_.
+
+The davidSDK will only allow you to scan if the scanner is calibrated, the davidSDK provides functions to load and save configuration files for the calibration. Also note that the davidSDK server will automatically reload the last calibration data when restarted.
+
+Using the example
+==================
+
+The `pcl_davidsdk_viewer <https://github.com/PointCloudLibrary/pcl/blob/master/visualization/tools/davidsdk_viewer.cpp>`_ example shows how to display a point cloud grabbed from a davidSDK device using the :pcl:`DavidSDKGrabber <pcl::DavidSDKGrabber>` class.
+
+When using the DavidSDKGrabber you must connect to the server first; if the server is running locally you don't need to specify an IP address. If you are using davidSDK over a network just call :pcl:`connect <pcl::DavidSDKGrabber::connect>` with the address IP as a string, please also check that the connection didn't failed:
+
+.. code-block:: cpp
+
+ davidsdk_ptr->connect ("192.168.1.50");
+ if (!davidsdk_ptr->isConnected ())
+ {
+ PCL_ERROR ("Cannot connect to davidSDK server.\n");
+ return (-1);
+ }
+
+.. image:: images/davidsdk/davidsdk_viewer.jpg
+ :height: 550
+
+.. warning::
+
+ Fetching clouds/meshes from the davidSDK is very slow because the point clouds/meshes are sent through the JSON interface.
+ Do not expect better performance than 0.07 FPS (using STL format gives best performance).
+
+Another example is available in `PCL sources <https://github.com/PointCloudLibrary/pcl/blob/master/doc/tutorials/content/sources/davidsdk/>`_, it uses OpenCV to display davidSDK images and the PCLVisualizer to display the point cloud at the same time.
+
--- /dev/null
+.. _depth_sense_grabber:
+
+Grabbing point clouds from DepthSense cameras
+---------------------------------------------
+
+In PCL 1.8.0 a new grabber for `DepthSense <http://www.softkinetic.com/Products/DepthSenseCameras>`_
+cameras was added. It is based on DepthSense SDK and, as such, should work with
+any camera supported by the SDK (e.g. `Creative Senz3D <http://us.creative.com/p/web-cameras/creative-senz3d>`_,
+`DepthSense DS325 <http://www.softkinetic.com/Store/ProductID/6>`_).
+
+.. note:: This grabber obsoletes `PXCGrabber`, which was a Windows-only solution
+ based on discontinued `Intel Perceptual Computing SDK <https://web.archive.org/web/20141228120859/https://software.intel.com/en-us/perceptual-computing-sdk>`_.
+
+In this tutorial we will learn how to setup and use DepthSense cameras within
+PCL on both Linux and Windows platforms.
+
+.. image:: images/creative_camera.jpg
+ :align: center
+
+.. contents::
+
+DepthSense SDK installation
+---------------------------
+
+Download and install the SDK from `SoftKinetic website <http://www.softkinetic.com/support/download.aspx>`_.
+Note that to obtain Linux drivers you need to register (free of charge).
+
+Linux
+^^^^^
+
+The Linux version of camera driver was built against an outdated version of
+`libudev`, so it will not work unless you have version 0.13 of this library
+installed (for example Ubuntu 14.04 comes with a newer version). There are
+several easy ways to solve this problem, see `this <https://web.archive.org/web/20150326145256/http://choorucode.com/2014/05/06/depthsense-error-some-dll-files-are-missing/>`_
+or `this <https://ph4m.wordpress.com/2014/02/11/getting-softkinetics-depthsense-sdk-to-work-on-arch-linux/>`_
+blog post.
+
+Furthermore, the Linux version of SDK is shipped with its own `libusb-1.0.so`
+library. You may have this library already installed on your system (e.g.
+because it is required by some other grabbers). In this case there will be
+conflicts, which will manifest in a flood of CMake warnings during configuration
+stage. To avoid this simply delete the corresponding files from the SDK
+installation path::
+
+ $ sudo rm /opt/softkinetic/DepthSenseSDK/lib/libusb-1.0*
+
+You can verify your installation by plugging in the camera and running the
+viewer app distributed with the SDK::
+
+ $ /opt/softkinetic/DepthSenseSDK/bin/DepthSenseViewer --standalone
+
+Windows
+^^^^^^^
+
+After the installation is completed you need to add the SDK path to the `PATH`
+environment variable. The installation path itself is stored in
+`DEPTHSENSESDK64` (on a 64-bit system) environment variable, thus you need to
+append `;%DEPTHSENSESDK64%\\bin` to your path. Do not forget to re-login for the
+changes to take effect.
+
+Verify installation by running `DepthSenseViewer.exe` in command prompt.
+
+PCL configuration
+-----------------
+
+You need at least PCL 1.8.0 to be able to use the DepthSense SDK. The
+``WITH_DSSDK`` option should be enabled in the CMake configuration.
+
+DepthSense Viewer
+-----------------
+
+The grabber is accompanied by an example tool `pcl_depth_sense_viewer <https://github.com/PointCloudLibrary/pcl/blob/master/visualization/tools/depth_sense_viewer.cpp>`_
+which can be used to view and save point clouds coming from a DepthSense device.
+Internally it uses the `DepthSenseGrabber <http://docs.pointclouds.org/trunk/classpcl_1_1_depth_sense_grabber.html>`_
+class that implements the standard PCL grabber interface.
+
+You can run the tool with `--help` option to view the usage guide.
+
+The video below demontrates the features of the DepthSense viewer tool. Please
+note that the bilateral filtering (which can be observed in the end of the
+video) is currently disabled is the tool.
+
+.. raw:: html
+
+ <center><iframe title="DepthSense viewer" width="560" height="315" src="https://www.youtube.com/embed/W3_VYiiEPjQ" frameborder="0" allowfullscreen></iframe></center>
+
--- /dev/null
+.. _ensenso_cameras:
+
+==========================================
+Grabbing point clouds from Ensenso cameras
+==========================================
+
+In this tutorial we will learn how to use the `IDS-Imaging <http://en.ids-imaging.com/>`_ Ensenso cameras within PCL. This tutorial will show you how to configure PCL
+and how to use the examples to fetch point clouds from the `Ensenso <http://www.ensenso.de/>`_.
+
+.. contents::
+
+Install Ensenso drivers
+=======================
+
+The Ensenso drivers are free (as in beer) and available for download, for each of them follow the instructions provided:
+
+ * `uEye <http://en.ids-imaging.com/download-ueye.html>`_
+ * `Ensenso SDK <http://www.ensenso.de/download>`_
+
+Plug-in the camera and test if the Ensenso is working, launch ``nxView`` in your terminal to check if you can actually use the camera.
+
+Configuring PCL
+===============
+
+You need at least PCL 1.8.0 to be able to use the Ensenso cameras. You need to make sure ``WITH_ENSENSO`` is set to ``true`` in the CMake
+configuration (it should be set to true by default if you have followed the instructions before).
+
+The default following values can be tweaked into cmake if you don't have a standard installation, for example:
+
+.. code-block::
+
+ ENSENSO_ABI_DIR /opt/ensenso_test/development/c
+
+You can deactivate building the Ensenso support by setting ``WITH_ENSENSO`` to false.
+Compile and install PCL.
+
+Using the example
+=================
+
+The `pcl_ensenso_viewer <https://github.com/PointCloudLibrary/pcl/blob/master/visualization/tools/ensenso_viewer.cpp>`_ example shows how to
+display a point cloud grabbed from an Ensenso device using the `EnsensoGrabber <http://docs.pointclouds.org/trunk/classpcl_1_1_ensenso_grabber.html>`_ class.
+
+Note that this program opens the TCP port of the nxLib tree, this allows you to open the nxLib tree with the nxTreeEdit program (port 24000).
+The capture parameters (exposure, gain etc..) are set to default values.
+If you have performed and stored an extrinsic calibration it will be temporary reset.
+
+.. code-block:: cpp
+
+ ensenso_ptr->enumDevices ();
+ ensenso_ptr->openTcpPort ();
+ ensenso_ptr->openDevice ();
+ ensenso_ptr->configureCapture ();
+ ensenso_ptr->setExtrinsicCalibration ();
+
+The code is very similar to the ``pcl_openni_viewer``.
+All the Ensenso devices connected are listed and then the point cloud are fetched as fast as possible.
+
+Here is an example of the terminal output ::
+
+ $ pcl_ensenso_viewer
+ Initialising nxLib
+ Number of connected cameras: 1
+ Serial No Model Status
+ 140242 N10-1210-18 Available
+
+ Opening Ensenso stereo camera id = 0
+ FPS: 3.32594
+ FPS: 3.367
+ FPS: 3.79441
+ FPS: 4.01204
+ FPS: 4.07747
+ FPS: 4.20309
+ Closing Ensenso stereo camera
+
+.. image:: images/ensenso/ensenso_viewer.jpg
+ :height: 550
+
+Another example is available in `PCL sources <https://github.com/PointCloudLibrary/pcl/blob/master/doc/tutorials/content/sources/ensenso_cameras/>`_, it uses OpenCV to display Ensenso
+images and the PCLVisualizer to display the point cloud at the same time.
+
+Extrinsic calibration
+=====================
+
+If you want to perform extrinsic calibration of the sensor, please first make sure your EnsensoSDK version is greater than 1.3.
+A fully automated extrinsic calibration ROS package is available to help you calibrate the sensor mounted on a robot arm,
+the package can be found in the `Institut Maupertuis repository <https://github.com/InstitutMaupertuis/ensenso_extrinsic_calibration>`_.
+
+The following video shows the automatic calibration procedure on a Fanuc R1000iA 80f industrial robot:
+
+.. raw:: html
+
+ <iframe width="800" height="500" src="https://www.youtube.com/embed/2g6gdx8fKX8" frameborder="0" allowfullscreen></iframe>
+
--- /dev/null
+.. _global_hypothesis_verification:
+
+Tutorial: Hypothesis Verification for 3D Object Recognition
+-----------------------------------------------------------
+
+This tutorial aims at explaining how to do 3D object recognition in clutter by verifying model hypotheses
+in cluttered and heavily occluded 3D scenes. After descriptor matching, the tutorial runs one of the
+Correspondence Grouping algorithms available in PCL in order to cluster the set of point-to-point
+correspondences, determining instances of object hypotheses in the scene. On these hypotheses,
+the Global Hypothesis Verification algorithm is applied in order to
+decrease the amount of false positives.
+
+Suggested readings and prerequisites
+------------------------------------
+
+This tutorial is the follow-up of a previous tutorial on object recognition: :ref:`correspondence_grouping`
+To understand this tutorial, we suggest first to read and understand that tutorial.
+
+More details on the Global Hypothesis Verification method can be found here:
+A. Aldoma, F. Tombari, L. Di Stefano, M. Vincze, `A global hypothesis verification method for 3D object recognition`, ECCV 2012
+
+For more information on 3D Object Recognition in Clutter and on the standard feature-based recognition pipeline, we suggest this tutorial paper:
+A. Aldoma, Z.C. Marton, F. Tombari, W. Wohlkinger, C. Potthast, B. Zeisl, R.B. Rusu, S. Gedikli, M. Vincze, "Point Cloud Library: Three-Dimensional Object Recognition and 6 DOF Pose Estimation", IEEE Robotics and Automation Magazine, 2012
+
+The Code
+--------
+
+Before starting, you should download from the GitHub folder: `Correspondence Grouping <https://github.com/PointCloudLibrary/data/tree/master/tutorials/correspondence_grouping>`_ the example PCD clouds
+used in this tutorial (milk.pcd and milk_cartoon_all_small_clorox.pcd), and place the files in the source older.
+
+Then copy and paste the following code into your editor and save it as ``global_hypothesis_verification.cpp``.
+
+.. literalinclude:: sources/global_hypothesis_verification/global_hypothesis_verification.cpp
+ :language: c++
+ :linenos:
+
+Walkthrough
+-----------
+
+Take a look at the various parts of the code to see how it works.
+
+Input Parameters
+****************
+
+.. literalinclude:: sources/global_hypothesis_verification/global_hypothesis_verification.cpp
+ :language: c++
+ :lines: 107-112
+
+.. literalinclude:: sources/global_hypothesis_verification/global_hypothesis_verification.cpp
+ :language: c++
+ :lines: 142-149
+
+``showHelp`` function prints out the input parameters accepted by the program. ``parseCommandLine`` binds the user input with program parameters.
+
+The only two mandatory parameters are ``model_filename`` and ``scene_filename`` (all other parameters are initialized with a default value).
+Other usefuls commands are:
+
+* ``--algorithm (Hough|GC)`` used to switch clustering algorithm. See :ref:`correspondence_grouping`.
+* ``-k`` shows the keypoints used to compute the correspondences
+
+Hypotheses Verification parameters are:
+
+* ``--hv_clutter_reg val: Clutter Regularizer (default 5.0)``
+* ``--hv_inlier_th val: Inlier threshold (default 0.005)``
+* ``--hv_occlusion_th val: Occlusion threshold (default 0.01)``
+* ``--hv_rad_clutter val: Clutter radius (default 0.03)``
+* ``--hv_regularizer val: Regularizer value (default 3.0)``
+* ``--hv_rad_normals val: Normals radius (default 0.05)``
+* ``--hv_detect_clutter val: TRUE if clutter detect enabled (default true)``
+
+More details on the Global Hypothesis Verification parameters can be found here:
+A. Aldoma, F. Tombari, L. Di Stefano, M. Vincze, `A global hypothesis verification method for 3D object recognition`, ECCV 2012.
+
+Helpers
+*******
+
+.. literalinclude:: sources/global_hypothesis_verification/global_hypothesis_verification.cpp
+ :language: c++
+ :lines: 60-83
+
+This simple struct is used to create `Color` presets for the clouds being visualized.
+
+
+Clustering
+**********
+
+The code below implements a full Clustering Pipeline: the input of the pipeline is a pair of point clouds (the ``model`` and the ``scene``), and the output is
+
+::
+
+ std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
+
+``rototraslations`` represents a list of `coarsely` transformed models ("object hypotheses") in the scene.
+
+Take a look at the full pipeline:
+
+.. literalinclude:: sources/global_hypothesis_verification/global_hypothesis_verification.cpp
+ :language: c++
+ :lines: 245-374
+ :emphasize-lines: 6,9
+
+For a full explanation of the above code see `3D Object Recognition based on Correspondence Grouping <http://pointclouds.org/documentation/tutorials/correspondence_grouping.php>`_.
+
+
+Model-in-Scene Projection
+*************************
+
+To improve the `coarse` transformation associated to each object hypothesis, we apply some ICP iterations.
+We create a ``instances`` list to store the "coarse" transformations :
+
+.. literalinclude:: sources/global_hypothesis_verification/global_hypothesis_verification.cpp
+ :language: c++
+ :lines: 389-399
+
+
+then, we run ICP on the ``instances`` wrt. the ``scene`` to obtain the ``registered_instances``:
+
+.. literalinclude:: sources/global_hypothesis_verification/global_hypothesis_verification.cpp
+ :language: c++
+ :lines: 401-431
+
+Hypotheses Verification
+***********************
+
+.. literalinclude:: sources/global_hypothesis_verification/global_hypothesis_verification.cpp
+ :language: c++
+ :lines: 433-466
+
+``GlobalHypothesesVerification`` takes as input a list of ``registered_instances`` and a ``scene`` so we can ``verify()`` them
+to get a ``hypotheses_mask``: this is a `bool` array where ``hypotheses_mask[i]`` is ``TRUE`` if ``registered_instances[i]`` is a
+verified hypothesis, ``FALSE`` if it has been classified as a False Positive (hence, must be rejected).
+
+
+Visualization
+*************
+
+The first part of the Visualization code section is pretty simple, with ``-k`` options the program displays `goog keypoints` in model and in scene
+with a ``styleViolet`` color.
+
+Later we iterate on ``instances``, and each ``instances[i]`` will be displayed in `Viewer` with a ``styleRed`` color.
+Each ``registered_instances[i]`` will be displayed with two optional colors: ``styleGreen`` if the current instance is verified (``hypotheses_mask[i]`` is ``TRUE``), ``styleCyan`` otherwise.
+
+.. literalinclude:: sources/global_hypothesis_verification/global_hypothesis_verification.cpp
+ :language: c++
+ :lines: 468-525
+
+
+Compiling and running the program
+---------------------------------
+
+Create a ``CMakeLists.txt`` file and add the following lines into it:
+
+
+.. literalinclude:: sources/global_hypothesis_verification/CMakeLists.txt
+ :language: cmake
+ :linenos:
+
+After you have created the executable, you can then launch it following this example:
+
+>>> ./global_hypothesis_verification milk.pcd milk_cartoon_all_small_clorox.pcd
+
+.. figure:: images/global_hypothesis_verification/original.png
+ :alt: Original Scene Image
+
+Original Scene Image
+
+.. figure:: images/global_hypothesis_verification/single.png
+
+ Valid Hypothesis (Green) with simple parameters
+
+You can simulate more false positives by using a larger bin size parameter for the Hough Voting Correspondence Grouping algorithm:
+
+>>> ./global_hypothesis_verification milk.pcd milk_cartoon_all_small_clorox.pcd --cg_size 0.035
+
+.. figure:: images/global_hypothesis_verification/multiple.png
+
+ Valid Hypothesis (Green) among 9 false positives
+
--- /dev/null
+.. _gpu_install:
+
+Configuring your PC to use your Nvidia GPU with PCL
+---------------------------------------------------
+In this tutorial we will learn how to check if your PC is
+suitable for use with the GPU methods provided within PCL.
+This tutorial has been tested on Ubuntu 11.04 and 12.04, let
+us know on the user mailing list if you have tested this on other
+distributions.
+
+The explanation
+---------------
+
+In order to run the code you'll need a decent Nvidia GPU with Fermi or Kepler architecture you can check this by::
+
+ $ lspci | grep nVidia
+
+This should indicate which GPU you have on your system, if you don't have an Nvidia GPU, we're sorry, but you
+won't be able to use PCL GPU.
+The output of this you can compare with `this link <http://www.nvidia.co.uk/object/cuda-parallel-computing-uk.html>`_
+on the Nvidia website, your card should mention compute capability of 2.x (Fermi) or 3.x (Kepler) or higher.
+If you want to run with a GUI, you can also run::
+
+ $ nvidia-settings
+
+From either CLI or from your system settings menu. This should mention the same information.
+
+First you need to test if your CPU is 32 or 64 bit, if it is 64-bit, it is preferred to work in this mode.
+For this you can run::
+
+ $ lscpu | grep op-mode
+
+As a next step you will need a up to date version of the Cuda Toolkit. You can get this
+`here <http://developer.nvidia.com/cuda/cuda-downloads>`_, at the time of writing the
+latest version was 4.2 and the beta release of version 5 was available as well.
+
+First you will need to install the latest video drivers, download the correct one from the site
+and run the install file, after this, download the toolkit and install it.
+At the moment of writing this was version 295.41, please choose the most up to date one::
+
+ $ wget http://developer.download.nvidia.com/compute/cuda/4_2/rel/drivers/devdriver_4.2_linux_64_295.41.run
+
+Make the driver executable::
+
+ $ chmod +x devdriver_4.2_linux_64_295.41.run
+
+Run the driver::
+
+ $ sudo ./devdriver_4.2_linux_64_295.41.run
+
+You need to run this script without your X-server running. You can shut your X-server down as follows:
+Go to a terminal by pressing Ctrl-Alt-F1 and typing::
+
+ $ sudo service gdm stop
+
+Once you have installed you GPU device driver you will also need to install the CUDA Toolkit::
+
+ $ wget http://developer.download.nvidia.com/compute/cuda/4_2/rel/toolkit/cudatoolkit_4.2.9_linux_64_ubuntu11.04.run
+ $ chmod +x cudatoolkit_4.2.9_linux_64_ubuntu11.04.run
+ $ sudo ./cudatoolkit_4.2.9_linux_64_ubuntu11.04.run
+
+You can get the SDK, but for PCL this is not needed, this provides you with general CUDA examples
+and some scripts to test the performance of your CPU as well as your hardware specifications.
+
+CUDA only compiles with gcc 4.4 and lower, so if your default installed gcc is 4.5 or higher you'll need to install gcc 4.4:
+
+ $ sudo apt-get install gcc-4.4
+
+Now you need to force your gcc to use this version, you can remove the older version, the other option is to create a symlink in your home folder and include that in the beginning of your path:
+
+ $ cd
+ $ mkdir bin
+
+Add 'export PATH=$HOME/bin:$PATH as the last line to your ~/.bashrc file.
+Now create the symlinks in your bin folder:
+
+ $ cd ~/bin
+ $ ln -s <your_gcc_installation> c++
+ $ ln -s <your_gcc_installation> cc
+ $ ln -s <your_gcc_installation> g++
+ $ ln -s <your_gcc_installation> gcc
+
+If you use colorgcc these links all need to point to /usr/bin/colorgcc.
+
+Now you can get the latest git master (or another one) of PCL and configure your
+installation to use the CUDA functions.
+
+Go to your PCL root folder and do::
+
+ $ mkdir build; cd build
+ $ ccmake ..
+
+Press c to configure ccmake, press t to toggle to the advanced mode as a number of options
+only appear in advanced mode. The latest CUDA algorithms are beeing kept in the GPU project, for
+this the BUILD_GPU option needs to be on and the BUILD_gpu_<X> indicate the different
+GPU subprojects.
+
+.. image:: images/gpu/gpu_ccmake.png
+ :width: 400 pt
+
+Press c again to configure for you options, press g to generate the makefiles and to exit. Now
+the makefiles have been generated successfully and can be executed by doing::
+
+ $ make
+
+If you want to install your PCL installation for everybody to use::
+
+ $ make install
+
+Now your installation is finished!
+
+Tested Hardware
+---------------
+Please report us the hardware you have tested the following methods with.
+
++-----------------------+----------------------------------------------------------------------+----------------+
+| Method | Hardware | Reported FPS |
++=======================+======================================================================+================+
+| Kinfu | GTX680, Intel Xeon CPU E5620 @ 2.40Ghz x 8, 24Gb RAM | 20-27 |
++-----------------------+----------------------------------------------------------------------+----------------+
+| | GTX480, Intel Xeon CPU W3550 @ 3.07GHz × 4, 7.8Gb RAM | 40 |
++-----------------------+----------------------------------------------------------------------+----------------+
+| | C2070, Intel Xeon CPU E5620 @ 2.40Ghz x 8, 24Gb RAM | 29 |
++-----------------------+----------------------------------------------------------------------+----------------+
+| People Pose Detection | GTX680, Intel Xeon CPU E5620 @ 2.40Ghz x 8, 24Gb RAM | 20-23 |
++-----------------------+----------------------------------------------------------------------+----------------+
+| | C2070, Intel Xeon CPU E5620 @ 2.40Ghz x 8, 24Gb RAM | 10-20 |
++-----------------------+----------------------------------------------------------------------+----------------+
+
+
--- /dev/null
+.. _gpu_people:
+
+Detecting people and their poses using PointCloud Library
+---------------------------------------------------------
+In this tutorial we will learn how detect a person and its pose in a pointcloud.
+This is based on work from Koen Buys, Cedric Cagniart, Anatoly Bashkeev and Caroline Pantofaru, this
+has been presented on ICRA2012 and IROS2012 and an official reference for a journal paper is in progress. A coarse outline of how it works can be seen in the following video.
+
+
+ .. raw:: html
+
+ <iframe width="560" height="315" src="http://www.youtube.com/embed/Wd4OM8wOO1E?rel=0" frameborder="0" allowfullscreen></iframe>
+
+This shows how to detect people with an Primesense device, the full version
+working on oni and pcd files can be found in the git master.
+The code assumes a organised and projectable pointcloud, and should work with other
+sensors then the Primesense device.
+
+ .. image:: images/gpu/people/ss26_1.jpg
+ :width: 400 pt
+ :height: 372 pt
+ .. image:: images/gpu/people/ss26_2.jpg
+ :width: 400 pt
+ :height: 372 pt
+
+In order to run the code you'll need a decent Nvidia GPU with Fermi or Kepler architecture, have a look
+at the GPU installation tutorial to get up and running with your GPU installation.
+
+The code
+--------
+The full version of this code can be found in PCL gpu/people/tools,
+the following is a reduced version for the tutorial.
+This version can be found in doc/tutorials/content/sources/gpu/people_detect.
+
+The explanation
+---------------
+
+Now, let's break down the code piece by piece. Starting from the main routine.
+
+.. literalinclude:: sources/gpu/people_detect/src/people_detect.cpp
+ :language: cpp
+ :lines: 317-368
+
+First the GPU device is set, by default this is the first GPU found in the bus, but if you have multiple GPU's in
+your system, this allows you to select a specific one.
+Then a OpenNI Capture is made, see the OpenNI Grabber tutorial for more info on this. (TODO add link)
+
+.. literalinclude:: sources/gpu/people_detect/src/people_detect.cpp
+ :language: cpp
+ :lines: 330-339
+
+The implementation is based on a similar approach as Shotton et al. and thus needs off-line learned random
+decision forests for labeling. The current implementation allows up to 4 decision trees to be loaded into
+the forest. This is done by giving it the names of the text files to load.
+
+.. literalinclude:: sources/gpu/people_detect/src/people_detect.cpp
+ :language: cpp
+ :lines: 341-342
+
+An additional parameter allows you to configure the number of trees to be loaded.
+
+.. literalinclude:: sources/gpu/people_detect/src/people_detect.cpp
+ :language: cpp
+ :lines: 351-353
+
+Then the RDF object is created, loading the trees upon creation.
+
+.. literalinclude:: sources/gpu/people_detect/src/people_detect.cpp
+ :language: cpp
+ :lines: 355-360
+
+Now we create the application object, give it the pointer to the RDF object and start the loop.
+Now we'll have a look at the main loop.
+
+
+.. literalinclude:: sources/gpu/people_detect/src/people_detect.cpp
+ :language: cpp
+ :lines: 238-286
+
+This routine first connects a callback routine to the grabber and waits for valid data to arrive.
+Each time the data arrives it will call the process function of the people detector, this is a
+fully encapsulated method and will call the complete pipeline.
+Once the pipeline completed processing, the results can be fetched as public structs or methods from the
+people detector object. Have a look at doc.pointclouds.org for more documentation on the
+available structs and methods.
+The visualizeAndWrite method will illustrate one of the available methods of the people detector object:
+
+.. literalinclude:: sources/gpu/people_detect/src/people_detect.cpp
+ :language: cpp
+ :lines: 141-178
+
+Line 144 calls the RDF getLabels method which returns the labels on the device, these however
+are a discrete enum of the labels and are visually hard to recognize, so these are converted to
+colors that illustrate each body part in line 145.
+At this point the results are still stored in the device memory and need to be copied to the CPU
+host memory, this is done in line 151. Afterwards the images are shown and stored to disk.
+
+Compiling and running the program
+---------------------------------
+
+Add the following lines to your CMakeLists.txt file:
+
+.. literalinclude:: sources/gpu/people_detect/CMakeLists.txt
+ :language: cmake
+ :linenos:
+
+After you have made the executable, you can run it. Simply do:
+ $ ./people_detect
With the proposed method, people standing/walking on a planar ground plane can be detected in real time with standard CPU computation.
This implementation corresponds to the people detection algorithm for RGB-D data presented in
+- *M. Munaro and E. Menegatti*. "Fast RGB-D people tracking for service robots". In Autonomous Robots, Volume 37 Issue 3, pp. 227-242, Springer, 2014.
- *M. Munaro, F. Basso and E. Menegatti*. "Tracking people within groups with RGB-D data". In Proceedings of the International Conference on Intelligent Robots and Systems (IROS) 2012, Vilamoura (Portugal), 2012.
The code
are a few gotchas on some platforms.
The HDL Grabber supports the original HDL-64e as well as the HDL-32e.
-More information on those sensors can be found at `Velodyne's Web Site <http://velodynelidar.com/lidar/lidar.aspx>`_
+More information on those sensors can be found at `Velodyne's Web Site <http://www.velodynelidar.com/>`_
Basic Network Setup
-------------------
recording and playback. These PCAP files can be used with the HDL Grabber if PCL is compiled with
PCAP support.
-Velodyne provides sample PCAP files on their `website <http://velodyne.com/lidar/doc/Sample%20sets/HDL-32/>`_
+Velodyne provides sample PCAP files on their `website <http://midas3.kitware.com/midas/community/29>`_
Compiling the HDL Grabber with PCAP support
-------------------------------------------
* :ref:`segmentation_tutorial`
* :ref:`surface_tutorial`
* :ref:`visualization_tutorial`
+ * :ref:`gpu`
.. _basic_usage:
.. |mi_2| image:: images/pcl_logo.png
:height: 75px
+ * :ref:`compiling_pcl_posix`
+
+ ======= ======
+ |mi_11| Title: **Compiling PCL from source on POSIX compliant systems**
+
+ Author: *Victor Lamoine*
+
+ Compatibility: > PCL 1.0
+
+ In this tutorial, we will explain how to compile PCL from sources on POSIX/Unix systems.
+ ======= ======
+
+ .. |mi_11| image:: images/pcl_logo.png
+ :height: 120px
+
* :ref:`building_pcl`
====== ======
.. |mi_11| image:: images/pcl_logo.png
:height: 75px
- * :ref:`qt_visualizer`
-
- ====== ======
- |mi_9| Title: **Create a PCL visualizer in Qt with cmake**
-
- Author: *Victor Lamoine*
-
- Compatibility: > PCL 1.5
-
- This tutorial shows you how to create a PCL visualizer within a Qt application.
- ====== ======
-
- .. |mi_9| image:: images/qt_visualizer/qt.png
- :height: 128px
-
* :ref:`matrix_transform`
======= ======
.. |i_o6| image:: images/dinast_cyclopes.png
:height: 100px
+ * :ref:`ensenso_cameras`
+
+ ====== ======
+ |i_o7| Title: **Grabbing point clouds from Ensenso cameras**
+
+ Author: *Victor Lamoine*
+
+ Compatibility: >= PCL 1.8.0
+
+ In this tutorial, we will learn how to acquire point cloud data from an IDS-Imaging Ensenso camera.
+ ====== ======
+
+ .. |i_o7| image:: images/ensenso/ids.png
+ :height: 165px
+
+ * :ref:`david_sdk`
+
+ ====== ======
+ |i_o8| Title: **Grabbing point clouds / meshes from davidSDK scanners**
+
+ Author: *Victor Lamoine*
+
+ Compatibility: >= PCL 1.8.0
+
+ In this tutorial, we will learn how to acquire point cloud or mesh data from a davidSDK scanner.
+ ====== ======
+
+ .. |i_o8| image:: images/davidsdk/david.png
+ :height: 70px
+
+ * :ref:`depth_sense_grabber`
+
+ ====== ======
+ |i_o9| Title: **Grabbing point clouds from DepthSense cameras**
+
+ Author: *Sergey Alexandrov*
+
+ Compatibility: >= PCL 1.8.0
+
+ In this tutorial we will learn how to setup and use DepthSense cameras within PCL on both Linux and Windows platforms.
+ ====== ======
+
+ .. |i_o9| image:: images/creative_camera.jpg
+ :height: 70px
.. _keypoints_tutorial:
.. |rc_2| image:: images/implicit_shape_model.png
:height: 100px
+ * :ref:`global_hypothesis_verification`
+
+ ====== ======
+ |rc_3| Title: **Hypothesis Verification for 3D Object Recognition**
+
+ Author: *Daniele De Gregorio, Federico Tombari*
+
+ Compatibility: > PCL 1.7
+
+ This tutorial aims at explaining how to do 3D object recognition in clutter by verifying model hypotheses in cluttered and heavily occluded 3D scenes.
+ ====== ======
+
+ .. |rc_3| image:: images/global_hypothesis_verification/multiple.png
+ :height: 100px
+
.. _registration_tutorial:
Registration
Author: *Jeremie Papon*
- Compatibility: >= PCL 1.7
+ Compatibility: >= PCL 1.8
In this tutorial, we show to break a pointcloud into the mid-level supervoxel representation.
====== ======
.. |vi_5| image:: images/visualization_small.png
:height: 120px
+ * :ref:`qt_visualizer`
+
+ ====== ======
+ |vi_6| Title: **Create a PCL visualizer in Qt with cmake**
+
+ Author: *Victor Lamoine*
+
+ Compatibility: > PCL 1.5
+
+ This tutorial shows you how to create a PCL visualizer within a Qt application.
+ ====== ======
+
+ .. |vi_6| image:: images/qt_visualizer/qt.png
+ :height: 128px
+
+ * :ref:`qt_colorize_cloud`
+
+ ====== ======
+ |vi_7| Title: **Create a PCL visualizer in Qt to colorize clouds**
+
+ Author: *Victor Lamoine*
+
+ Compatibility: > PCL 1.5
+
+ This tutorial shows you how to color point clouds within a Qt application.
+ ====== ======
+
+ .. |vi_7| image:: images/qt_visualizer/qt.png
+ :height: 128px
+
+
.. _applications_tutorial:
Applications
.. |ap_5| image:: images/ground_based_rgbd_people_detection/Index_photo.jpg
:height: 120px
+
+.. _gpu:
+
+GPU
+---
+
+ * :ref:`gpu_install`
+
+ ====== ======
+ |gp_1| Title: **GPU Installation**
+
+ Author: *Koen Buys*
+
+ Compatibility: PCL git master
+
+ This tutorial explains how to configure PCL to use with a Nvidia GPU
+ ====== ======
+
+ .. |gp_1| image:: images/PCD_icon.png
+ :height: 100px
+
+ * :ref:`using_kinfu_large_scale`
+
+ ====== ======
+ |ap_4| Title: **Using Kinfu Large Scale to generate a textured mesh**
+
+ Author: *Francisco Heredia and Raphael Favier*
+
+ Compatibility: PCL git master
+
+ This tutorial demonstrates how to use KinFu Large Scale to produce a mesh from a room, and apply texture information in post-processing for a more appealing visual result.
+ ====== ======
+
+ .. |ap_4| image:: images/using_kinfu_large_scale.jpg
+ :height: 100px
+
+ * :ref:`gpu_people`
+
+ ====== ======
+ |gp_2| Title: **People Detection**
+
+ Author: *Koen Buys*
+
+ Compatibility: PCL git master
+
+ This tutorial presents a method for people and pose detection.
+ ====== ======
+
+ .. |gp_2| image:: images/gpu/people/c2_100.jpg
+ :height: 100px
+
..
* :ref:`normal_estimation_integral_images`
Surface normal estimation
Using the formula
=================
-The PCL formula is not in Homebrew official repositories yet, but it will be after 1.7.2.
-For now it resides at `Fran6co's repository <https://github.com/fran6co/homebrew-cv>`_ and can be tapped from there.
+The PCL formula is in the Homebrew official repositories.
This will automatically install all necessary dependencies and provides options for controlling
which parts of PCL are installed.
#. Install Homebrew. See the Homebrew website for instructions.
#. Execute ``brew update``.
- #. Execute ``brew tap homebrew/versions``.
#. Execute ``brew tap homebrew/science``.
- #. Execute ``brew tap fran6co/cv``.
To install the latest version using the formula, execute the following command::
- $ brew install pcl --HEAD
+ $ brew install pcl
You can specify options to control which parts of PCL are installed. For
example, to build just the libraries without extra dependencies, execute the following command::
- $ brew install pcl --HEAD --without-apps --without-tools --without-vtk --without-qvtk --without-qt
+ $ brew install pcl --without-apps --without-tools --without-vtk --without-qt
For a full list of the available options, see the formula's help::
rendering window with your mouse etc. A very simple example of such things
is found in the ``interactionCustomizationVis`` method.
+.. note:: In Mac platforms and if using a VTK version prior to 7.0, the executable is required to be built as an Application Bundle, in order to have proper mouse and keyboard interaction support. For more instructions on how to do that, please consult the `Cocoa VTK Wiki <http://www.vtk.org/Wiki/Cocoa_VTK#Use_Application_Bundles>`_.
+
In this part of the tutorial you will be shown how to catch mouse and keyboard
events. By right clicking on the window, a 2D text will appear and you can
erase all the text instances by pressing 'r'. The result should look something
--- /dev/null
+.. _qt_colorize_cloud:
+
+==================================================
+Create a PCL visualizer in Qt to colorize clouds
+==================================================
+
+Please read and do the `PCL + Qt tutorial <http://www.pointclouds.org/documentation/tutorials/qt_visualizer.php>`_ first;
+only the coloring part is explained in details here.
+
+In this tutorial we will learn how to color clouds by computing a `Look Up Table (LUT) <https://en.wikipedia.org/wiki/Lookup_table>`_,
+compared to the first tutorial this tutorial shows you how to connect multiple slots to one function. It also showcases how to load and save
+files within the Qt interface.
+
+| The tutorial was tested on Linux Ubuntu 12.04 and 14.04. It also seems to be working fine on Windows 8.1 x64.
+| Feel free to push modifications into the git repo to make this code/tutorial compatible with your platform !
+
+.. contents::
+
+The project
+===========
+
+As for the other tutorial, we use `cmake <https://en.wikipedia.org/wiki/CMake>`_ instead of `qmake <http://qt-project.org/doc/qt-4.8/qmake-manual.html>`_.
+This is how I organized the project: the build folder contains all built files and the src folder holds all sources files ::
+
+ .
+ ├── build
+ └── src
+ ├── CMakeLists.txt
+ ├── main.cpp
+ ├── pclviewer.cpp
+ ├── pclviewer.h
+ ├── pclviewer.ui
+ └── pcl_visualizer.pro
+
+If you want to change this layout you will have to do minor modifications in the code, especially line 2 of ``pclviewer.cpp``
+Create the folder tree and download the sources files from `github <https://github.com/PointCloudLibrary/pcl/tree/master/doc/tutorials/content/sources/qt_colorize_cloud>`_.
+
+.. note::
+ File paths should not contain any special character or the compilation might fail with a ``moc: Cannot open options file specified with @`` error message.
+
+
+User interface (UI)
+===================
+
+The UI looks like this:
+
+.. image:: images/qt_colorize_cloud/ui.png
+ :height: 499
+
+The vertical spacers are here to make sure everything moves fine when you re-size the window; the QVTK widget size has been set to a minimum size of
+640 x 480 pixel, the layout makes sure that the QVTK widget expands when you re-size the application window.
+
+The code
+========
+
+Now, let's break down the code piece by piece.
+
+pclviewer.h
+-----------
+
+.. literalinclude:: sources/qt_colorize_cloud/pclviewer.h
+ :language: cpp
+ :lines: 42-57
+
+These are the public slots triggered by the buttons in the UI.
+
+.. literalinclude:: sources/qt_colorize_cloud/pclviewer.h
+ :language: cpp
+ :lines: 59-86
+
+These are the protected members of our class;
+ * ``viewer_`` is the visualizer object
+ * ``cloud_`` holds the point cloud displayed
+ * ``filtering_axis_`` stores on which axis we want to filter the point cloud. We need this variable because we only have one slot for 3 axes.
+ * ``color_mode_`` stores the color mode for the colorization, we need this variable for the same reason we need ``filtering_axis_``
+ * ``colorCloudDistances ()`` is the member function that actually colorize the point cloud.
+
+pclviewer.cpp
+-------------
+
+.. literalinclude:: sources/qt_colorize_cloud/pclviewer.cpp
+ :language: cpp
+ :lines: 4-8
+
+We initialize the members of our class to default values (note that theses values should match with the UI buttons ticked)
+
+.. literalinclude:: sources/qt_colorize_cloud/pclviewer.cpp
+ :language: cpp
+ :lines: 9-24
+
+Here we initialize the UI, window title and generate a random point cloud (500 points), note we don't care about the color for now.
+
+.. literalinclude:: sources/qt_colorize_cloud/pclviewer.cpp
+ :language: cpp
+ :lines: 26-31
+
+Here we set up the QVTK window.
+
+.. literalinclude:: sources/qt_colorize_cloud/pclviewer.cpp
+ :language: cpp
+ :lines: 33-46
+
+At this point we connect SLOTS and their functions to ensure that each UI elements has an use.
+
+.. literalinclude:: sources/qt_colorize_cloud/pclviewer.cpp
+ :language: cpp
+ :lines: 48-52
+
+We call the coloring function, add the point cloud and refresh the QVTK viewer.
+
+.. literalinclude:: sources/qt_colorize_cloud/pclviewer.cpp
+ :language: cpp
+ :lines: 60-98
+
+This functions deals with opening files, it supports both ``pcd`` and ``ply`` files.
+The LUT computing will only work if the point cloud is dense (only finite values) so we remove NaN values from the point cloud if needed.
+
+.. literalinclude:: sources/qt_colorize_cloud/pclviewer.cpp
+ :language: cpp
+ :lines: 100-127
+
+| This functions deals with saving the displayed file, it supports both ``pcd`` and ``ply`` files.
+| As said before, if the user doesn't append an extension to the file name, ``ply`` will be automatically added.
+
+.. literalinclude:: sources/qt_colorize_cloud/pclviewer.cpp
+ :language: cpp
+ :lines: 129-152
+
+This function is called whenever one of the three radio buttons X,Y,Z are clicked, it determines which radio button is clicked and changes
+the ``filtering_axis_`` member accordingly.
+
+.. literalinclude:: sources/qt_colorize_cloud/pclviewer.cpp
+ :language: cpp
+ :lines: 154-187
+
+This function is called whenever one of the radio buttons in the color list is clicked, the ``color_mode_`` member is modified accordingly.
+We also call the coloring function and update the cloud / QVTK widget.
+
+.. literalinclude:: sources/qt_colorize_cloud/pclviewer.cpp
+ :language: cpp
+ :lines: 189-209
+
+| This is the core function of the application. We are going to color the cloud following a color scheme.
+The point cloud is going to be colored following one direction, we first need to know where it starts and where it ends
+(the minimum & maximum point values along the chosen axis). We first set the initial minimal value to the first point value
+(this is safe because we removed NaN points from the point clouds). The switch case allows us to deal with the 3 different axes.
+
+.. literalinclude:: sources/qt_colorize_cloud/pclviewer.cpp
+ :language: cpp
+ :lines: 211-238
+
+We then loop through the whole cloud to find the minimum and maximum values.
+
+.. literalinclude:: sources/qt_colorize_cloud/pclviewer.cpp
+ :language: cpp
+ :lines: 239-244
+
+Here we compute the scaling, RGB values are coded from 0 to 255 (as integers), we need to scale our distances so that the
+minimum distance equals 0 (in RGB scale) and the maximum distance 255 (in RGB scale).
+The ``if`` condition is here in case of a perfectly flat point cloud and avoids exceptions thrown by boost.
+
+.. literalinclude:: sources/qt_colorize_cloud/pclviewer.cpp
+ :language: cpp
+ :lines: 246-260
+
+We have computed how much we need to scale the distances to fit the RGB scale, we first need to round the ``double`` values to the closest ``integer``
+because colors are coded as integers.
+
+.. literalinclude:: sources/qt_colorize_cloud/pclviewer.cpp
+ :language: cpp
+ :lines: 262-270
+
+This is where we apply the color level we have computed to the point cloud R,G,B values.
+You can do whatever you want here, the simplest option is to apply the 3 channels (R,G,B) to the ``value`` computed, this means that the
+minimum distance will translate into dark (black = 0,0,0) points and maximal distances into white (255,255,255) points.
+
+.. literalinclude:: sources/qt_colorize_cloud/pclviewer.cpp
+ :language: cpp
+ :lines: 271-305
+
+These are examples of coloring schemes, if you are wondering how it works, simply plot the computed values into a spreadsheet software.
+
+Compiling and running
+=====================
+
+There are two options here :
+ * You have configured the Qt project (see `Qt visualizer tutorial <http://www.pointclouds.org/documentation/tutorials/qt_visualizer.php#qt-configuration>`_) and you can compile/run just by clicking on the bottom left "Play" button.
+ * You didn't configure the Qt project; just go to the build folder an run ``cmake ../src && make -j2 && ./pcl_visualizer``
+
+Note that if you don't specify a extension when saving the file, the file will be saved as a binary PLY file.
+
+.. image:: images/qt_colorize_cloud/colorize_cloud.gif
+ :height: 526
+
├── pclviewer.cpp
├── pclviewer.h
├── pclviewer.ui
- ├── pcl_visualizer.pro
- └── pcl_visualizer.pro.user
+ └── pcl_visualizer.pro
If you want to change this layout you will have to do minor modifications in the code, especially line 2 of ``pclviewer.cpp``
Create the folder tree and download the sources files from `github <https://github.com/PointCloudLibrary/pcl/tree/master/doc/tutorials/content/sources/qt_visualizer>`_.
.. literalinclude:: sources/remove_outliers/remove_outliers.cpp
:language: cpp
- :lines: 38-52
+ :lines: 38-53
Basically, we create the condition which a given point must satisfy for it to remain in our PointCloud. In this example, we use add two comparisons to the conditon: greater than (GT) 0.0 and less than (LT) 0.8. This condition is then used to build the filter.
.. literalinclude:: sources/remove_outliers/remove_outliers.cpp
:language: cpp
- :lines: 57-67
+ :lines: 58-68
Compiling and running remove_outliers.cpp
---------------------------------------------
You will see something similar to (depending on which filter you are using)::
- Cloud before filtering:
- 0.352222 -0.151883 -0.106395
- -0.397406 -0.473106 0.292602
- -0.731898 0.667105 0.441304
- -0.734766 0.854581 -0.0361733
- -0.4607 -0.277468 -0.916762
- Cloud after filtering:
- -0.397406 -0.473106 0.292602
- -0.731898 0.667105 0.441304
+ Cloud before filtering:
+ 0.0080142 0.694695 -0.26015
+ -0.342265 -0.446349 0.214207
+ 0.173687 -0.84253 -0.400481
+ -0.874475 0.706127 -0.117635
+ 0.908514 -0.598159 0.744714
+ Cloud after filtering:
+ nan nan nan
+ -0.342265 -0.446349 0.214207
+ nan nan nan
+ nan nan nan
+ 0.908514 -0.598159 0.744714
align.setSourceFeatures (object_features);
align.setInputTarget (scene);
align.setTargetFeatures (scene_features);
- align.setMaximumIterations (10000); // Number of RANSAC iterations
+ align.setMaximumIterations (50000); // Number of RANSAC iterations
align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose
- align.setCorrespondenceRandomness (2); // Number of nearest features to use
+ align.setCorrespondenceRandomness (5); // Number of nearest features to use
align.setSimilarityThreshold (0.9f); // Polygonal edge length similarity threshold
- align.setMaxCorrespondenceDistance (1.5f * leaf); // Inlier threshold
+ align.setMaxCorrespondenceDistance (2.5f * leaf); // Inlier threshold
align.setInlierFraction (0.25f); // Required inlier fraction for accepting a pose hypothesis
{
pcl::ScopeTime t("Alignment");
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
- for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
+ for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/shot_omp.h>
#include <pcl/features/board.h>
-#include <pcl/keypoints/uniform_sampling.h>
+#include <pcl/filters/uniform_sampling.h>
#include <pcl/recognition/cg/hough_3d.h>
#include <pcl/recognition/cg/geometric_consistency.h>
#include <pcl/visualization/pcl_visualizer.h>
//
// Downsample Clouds to Extract keypoints
//
- pcl::PointCloud<int> sampled_indices;
pcl::UniformSampling<PointType> uniform_sampling;
uniform_sampling.setInputCloud (model);
uniform_sampling.setRadiusSearch (model_ss_);
- uniform_sampling.compute (sampled_indices);
- pcl::copyPointCloud (*model, sampled_indices.points, *model_keypoints);
+ uniform_sampling.filter (*model_keypoints);
std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl;
uniform_sampling.setInputCloud (scene);
uniform_sampling.setRadiusSearch (scene_ss_);
- uniform_sampling.compute (sampled_indices);
- pcl::copyPointCloud (*scene, sampled_indices.points, *scene_keypoints);
+ uniform_sampling.filter (*scene_keypoints);
std::cout << "Scene total points: " << scene->size () << "; Selected Keypoints: " << scene_keypoints->size () << std::endl;
--- /dev/null
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)\r
+\r
+project(pcl-davidSDK_images_viewer)\r
+\r
+find_package(PCL 1.8.0 REQUIRED)\r
+find_package(OpenCV REQUIRED)\r
+\r
+include_directories(${PCL_INCLUDE_DIRS})\r
+link_directories(${PCL_LIBRARY_DIRS})\r
+add_definitions(${PCL_DEFINITIONS})\r
+\r
+add_executable(davidsdk_images_viewer davidsdk_images_viewer.cpp)\r
+target_link_libraries(davidsdk_images_viewer ${PCL_LIBRARIES} ${OpenCV_LIBS})\r
--- /dev/null
+#include <iostream>
+#include <pcl/io/davidsdk_grabber.h>
+#include <pcl/console/print.h>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+/** @brief A pointer to the PCL DavidSDKGrabber object */
+pcl::DavidSDKGrabber::Ptr davidsdk_ptr;
+
+/** @brief Get OpenCV image type corresponding to the parameters given
+ * @param channels number of channels in the image
+ * @param bpe bytes per element
+ * @param isFlt is float
+ * @returns the OpenCV type
+ */
+int
+getOpenCVType (std::string type)
+{
+ if (type == "CV_32FC1")
+ return CV_32FC1;
+ else if (type == "CV_32FC2")
+ return CV_32FC2;
+ else if (type == "CV_32FC3")
+ return CV_32FC3;
+ else if (type == "CV_32FC4")
+ return CV_32FC4;
+ else if (type == "CV_64FC1")
+ return CV_64FC1;
+ else if (type == "CV_64FC2")
+ return CV_64FC2;
+ else if (type == "CV_64FC3")
+ return CV_64FC3;
+ else if (type == "CV_64FC4")
+ return CV_64FC4;
+ else if (type == "CV_8UC1")
+ return CV_8UC1;
+ else if (type == "CV_8UC2")
+ return CV_8UC2;
+ else if (type == "CV_8UC3")
+ return CV_8UC3;
+ else if (type == "CV_8UC4")
+ return CV_8UC4;
+ else if (type == "CV_16UC1")
+ return CV_16UC1;
+ else if (type == "CV_16UC2")
+ return CV_16UC2;
+ else if (type == "CV_16UC3")
+ return CV_16UC3;
+ else if (type == "CV_16UC4")
+ return CV_16UC4;
+ else if (type == "CV_32SC1")
+ return CV_32SC1;
+ else if (type == "CV_32SC2")
+ return CV_32SC2;
+ else if (type == "CV_32SC3")
+ return CV_32SC3;
+ else if (type == "CV_32SC4")
+ return CV_32SC4;
+
+ return (-1);
+}
+
+/** @brief Process and/or display DavidSDKGrabber image
+ * @param[in] image davidSDK image */
+void
+grabberCallback (const boost::shared_ptr<pcl::PCLImage>& image)
+{
+ unsigned char *image_array = reinterpret_cast<unsigned char *> (&image->data[0]);
+
+ int type = getOpenCVType (image->encoding);
+ cv::Mat cv_image (image->height, image->width, type, image_array);
+
+ cv::imshow ("davidSDK images", cv_image);
+ cv::waitKey (5);
+}
+
+/** @brief Asks the user to press enter to continue
+ * @param[in] str Message to display */
+void
+waitForUser (std::string str = "Press enter to continue")
+{
+ PCL_WARN (str.c_str ());
+ std::cout.flush ();
+ getc (stdin);
+}
+
+/** @brief Main function
+ * @param argc
+ * @param argv
+ * @return Exit status */
+int
+main (int argc,
+ char *argv[])
+{
+ if (argc != 2)
+ {
+ PCL_ERROR ("Usage:\n%s 192.168.100.65\n", argv[0]);
+ return (-1);
+ }
+
+ davidsdk_ptr.reset (new pcl::DavidSDKGrabber);
+ davidsdk_ptr->connect (argv[1]);
+
+ if (!davidsdk_ptr->isConnected ())
+ return (-1);
+ PCL_WARN ("davidSDK connected\n");
+
+ boost::function<void
+ (const boost::shared_ptr<pcl::PCLImage> &)> f = boost::bind (&grabberCallback, _1);
+ davidsdk_ptr->registerCallback (f);
+ davidsdk_ptr->start ();
+ waitForUser ("Press enter to quit");
+
+ return (0);
+}
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it, j++)
{
pcl::PointCloud<PointNormal>::Ptr cloud_cluster_don (new pcl::PointCloud<PointNormal>);
- for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
+ for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
{
cloud_cluster_don->points.push_back (doncloud->points[*pit]);
}
--- /dev/null
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+project(pcl-ensenso_cloud_images_viewer)
+
+find_package(PCL 1.8.0 REQUIRED)
+find_package(OpenCV REQUIRED)
+
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+add_executable(ensenso_cloud_images_viewer ensenso_cloud_images_viewer.cpp)
+target_link_libraries(ensenso_cloud_images_viewer ${PCL_LIBRARIES} ${OpenCV_LIBS})
--- /dev/null
+/**
+ * @file pcl_ensenso_cloud_images_viewer.cpp
+ * @brief Display both Ensenso images & cloud using the PCL Ensenso grabber (and OpenCV)
+ * @author Victor Lamoine
+ * @date November 2014
+ */
+
+#include <iostream>
+#include <vector>
+#include <algorithm>
+
+#include <pcl/common/common.h>
+#include <pcl/console/print.h>
+#include <pcl/io/ensenso_grabber.h>
+#include <pcl/console/time.h>
+#include <pcl/visualization/cloud_viewer.h>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+/** @brief Convenience typedef */
+typedef pcl::PointXYZ PointT;
+
+/** @brief Convenience typedef */
+typedef pcl::PointCloud<PointT> PointCloudT;
+
+/** @brief Convenience typdef for the Ensenso grabber callback */
+typedef std::pair<pcl::PCLImage, pcl::PCLImage> PairOfImages;
+
+/** @brief CloudViewer pointer */
+boost::shared_ptr<pcl::visualization::CloudViewer> viewer_ptr;
+
+/** @brief Pair of Ensenso images */
+pcl::EnsensoGrabber::Ptr ensenso_ptr;
+
+/** @brief Get OpenCV image type corresponding to the parameters given
+ * @param channels number of channels in the image
+ * @param bpe bytes per element
+ * @param isFlt is float
+ * @returns the OpenCV type
+ */
+int
+getOpenCVType (std::string type)
+{
+ if (type == "CV_32FC1")
+ return CV_32FC1;
+ else if (type == "CV_32FC2")
+ return CV_32FC2;
+ else if (type == "CV_32FC3")
+ return CV_32FC3;
+ else if (type == "CV_32FC4")
+ return CV_32FC4;
+ else if (type == "CV_64FC1")
+ return CV_64FC1;
+ else if (type == "CV_64FC2")
+ return CV_64FC2;
+ else if (type == "CV_64FC3")
+ return CV_64FC3;
+ else if (type == "CV_64FC4")
+ return CV_64FC4;
+ else if (type == "CV_8UC1")
+ return CV_8UC1;
+ else if (type == "CV_8UC2")
+ return CV_8UC2;
+ else if (type == "CV_8UC3")
+ return CV_8UC3;
+ else if (type == "CV_8UC4")
+ return CV_8UC4;
+ else if (type == "CV_16UC1")
+ return CV_16UC1;
+ else if (type == "CV_16UC2")
+ return CV_16UC2;
+ else if (type == "CV_16UC3")
+ return CV_16UC3;
+ else if (type == "CV_16UC4")
+ return CV_16UC4;
+ else if (type == "CV_32SC1")
+ return CV_32SC1;
+ else if (type == "CV_32SC2")
+ return CV_32SC2;
+ else if (type == "CV_32SC3")
+ return CV_32SC3;
+ else if (type == "CV_32SC4")
+ return CV_32SC4;
+
+ return (-1);
+}
+
+/** @brief Process and/or display Ensenso grabber images
+ * @param[in] cloud The Ensenso point cloud
+ * @param[in] images Pair of Ensenso images (raw or with overlay)
+ * @warning Image type changes if a calibration pattern is discovered/lost;
+ * check @c images->first.encoding */
+void
+grabberCallback (const boost::shared_ptr<PointCloudT>& cloud,
+ const boost::shared_ptr<PairOfImages>& images)
+{
+ viewer_ptr->showCloud (cloud);
+ unsigned char *l_image_array = reinterpret_cast<unsigned char *> (&images->first.data[0]);
+ unsigned char *r_image_array = reinterpret_cast<unsigned char *> (&images->second.data[0]);
+
+ std::cout << "Encoding: " << images->first.encoding << std::endl;
+ int type = getOpenCVType (images->first.encoding);
+ cv::Mat l_image (images->first.height, images->first.width, type, l_image_array);
+ cv::Mat r_image (images->first.height, images->first.width, type, r_image_array);
+ cv::Mat im (images->first.height, images->first.width * 2, type);
+
+ im.adjustROI (0, 0, 0, -images->first.width);
+ l_image.copyTo (im);
+ im.adjustROI (0, 0, -images->first.width, images->first.width);
+ r_image.copyTo (im);
+ im.adjustROI (0, 0, images->first.width, 0);
+ cv::imshow ("Ensenso images", im);
+ cv::waitKey (10);
+}
+
+/** @brief Main function
+ * @return Exit status */
+int
+main (void)
+{
+ viewer_ptr.reset (new pcl::visualization::CloudViewer ("3D Viewer"));
+ ensenso_ptr.reset (new pcl::EnsensoGrabber);
+ ensenso_ptr->openDevice (0);
+ ensenso_ptr->openTcpPort ();
+ //ensenso_ptr->initExtrinsicCalibration (5); // Disable projector if you want good looking images.
+ // You won't be able to detect a calibration pattern with the projector enabled!
+
+ boost::function<void
+ (const boost::shared_ptr<PointCloudT>&,
+ const boost::shared_ptr<PairOfImages>&)> f = boost::bind (&grabberCallback, _1, _2);
+ ensenso_ptr->registerCallback (f);
+
+ cv::namedWindow ("Ensenso images", cv::WINDOW_AUTOSIZE);
+ ensenso_ptr->start ();
+
+ while (!viewer_ptr->wasStopped ())
+ {
+ PCL_INFO("FPS: %f\n", ensenso_ptr->getFramesPerSecond ());
+ boost::this_thread::sleep (boost::posix_time::milliseconds (500));
+ }
+
+ ensenso_ptr->stop ();
+ //cv::destroyAllWindows (); // Doesn't work
+
+ ensenso_ptr->closeDevice ();
+ ensenso_ptr->closeTcpPort ();
+ return 0;
+}
--- /dev/null
+cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
+
+project(global_hypothesis_verification)
+
+#Pcl
+find_package(PCL 1.7 REQUIRED)
+
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+add_executable (global_hypothesis_verification global_hypothesis_verification.cpp)
+target_link_libraries (global_hypothesis_verification ${PCL_LIBRARIES})
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/correspondence.h>
+#include <pcl/features/normal_3d_omp.h>
+#include <pcl/features/shot_omp.h>
+#include <pcl/features/board.h>
+#include <pcl/filters/uniform_sampling.h>
+#include <pcl/recognition/cg/hough_3d.h>
+#include <pcl/recognition/cg/geometric_consistency.h>
+#include <pcl/recognition/hv/hv_go.h>
+#include <pcl/registration/icp.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/kdtree/impl/kdtree_flann.hpp>
+#include <pcl/common/transforms.h>
+#include <pcl/console/parse.h>
+
+typedef pcl::PointXYZRGBA PointType;
+typedef pcl::Normal NormalType;
+typedef pcl::ReferenceFrame RFType;
+typedef pcl::SHOT352 DescriptorType;
+
+struct CloudStyle
+{
+ double r;
+ double g;
+ double b;
+ double size;
+
+ CloudStyle (double r,
+ double g,
+ double b,
+ double size) :
+ r (r),
+ g (g),
+ b (b),
+ size (size)
+ {
+ }
+};
+
+CloudStyle style_white (255.0, 255.0, 255.0, 4.0);
+CloudStyle style_red (255.0, 0.0, 0.0, 3.0);
+CloudStyle style_green (0.0, 255.0, 0.0, 5.0);
+CloudStyle style_cyan (93.0, 200.0, 217.0, 4.0);
+CloudStyle style_violet (255.0, 0.0, 255.0, 8.0);
+
+std::string model_filename_;
+std::string scene_filename_;
+
+//Algorithm params
+bool show_keypoints_ (false);
+bool use_hough_ (true);
+float model_ss_ (0.02f);
+float scene_ss_ (0.02f);
+float rf_rad_ (0.015f);
+float descr_rad_ (0.02f);
+float cg_size_ (0.01f);
+float cg_thresh_ (5.0f);
+int icp_max_iter_ (5);
+float icp_corr_distance_ (0.005f);
+float hv_clutter_reg_ (5.0f);
+float hv_inlier_th_ (0.005f);
+float hv_occlusion_th_ (0.01f);
+float hv_rad_clutter_ (0.03f);
+float hv_regularizer_ (3.0f);
+float hv_rad_normals_ (0.05);
+bool hv_detect_clutter_ (true);
+
+/**
+ * Prints out Help message
+ * @param filename Runnable App Name
+ */
+void
+showHelp (char *filename)
+{
+ std::cout << std::endl;
+ std::cout << "***************************************************************************" << std::endl;
+ std::cout << "* *" << std::endl;
+ std::cout << "* Global Hypothese Verification Tutorial - Usage Guide *" << std::endl;
+ std::cout << "* *" << std::endl;
+ std::cout << "***************************************************************************" << std::endl << std::endl;
+ std::cout << "Usage: " << filename << " model_filename.pcd scene_filename.pcd [Options]" << std::endl << std::endl;
+ std::cout << "Options:" << std::endl;
+ std::cout << " -h: Show this help." << std::endl;
+ std::cout << " -k: Show keypoints." << std::endl;
+ std::cout << " --algorithm (Hough|GC): Clustering algorithm used (default Hough)." << std::endl;
+ std::cout << " --model_ss val: Model uniform sampling radius (default " << model_ss_ << ")" << std::endl;
+ std::cout << " --scene_ss val: Scene uniform sampling radius (default " << scene_ss_ << ")" << std::endl;
+ std::cout << " --rf_rad val: Reference frame radius (default " << rf_rad_ << ")" << std::endl;
+ std::cout << " --descr_rad val: Descriptor radius (default " << descr_rad_ << ")" << std::endl;
+ std::cout << " --cg_size val: Cluster size (default " << cg_size_ << ")" << std::endl;
+ std::cout << " --cg_thresh val: Clustering threshold (default " << cg_thresh_ << ")" << std::endl << std::endl;
+ std::cout << " --icp_max_iter val: ICP max iterations number (default " << icp_max_iter_ << ")" << std::endl;
+ std::cout << " --icp_corr_distance val: ICP correspondence distance (default " << icp_corr_distance_ << ")" << std::endl << std::endl;
+ std::cout << " --hv_clutter_reg val: Clutter Regularizer (default " << hv_clutter_reg_ << ")" << std::endl;
+ std::cout << " --hv_inlier_th val: Inlier threshold (default " << hv_inlier_th_ << ")" << std::endl;
+ std::cout << " --hv_occlusion_th val: Occlusion threshold (default " << hv_occlusion_th_ << ")" << std::endl;
+ std::cout << " --hv_rad_clutter val: Clutter radius (default " << hv_rad_clutter_ << ")" << std::endl;
+ std::cout << " --hv_regularizer val: Regularizer value (default " << hv_regularizer_ << ")" << std::endl;
+ std::cout << " --hv_rad_normals val: Normals radius (default " << hv_rad_normals_ << ")" << std::endl;
+ std::cout << " --hv_detect_clutter val: TRUE if clutter detect enabled (default " << hv_detect_clutter_ << ")" << std::endl << std::endl;
+}
+
+/**
+ * Parses Command Line Arguments (Argc,Argv)
+ * @param argc
+ * @param argv
+ */
+void
+parseCommandLine (int argc,
+ char *argv[])
+{
+ //Show help
+ if (pcl::console::find_switch (argc, argv, "-h"))
+ {
+ showHelp (argv[0]);
+ exit (0);
+ }
+
+ //Model & scene filenames
+ std::vector<int> filenames;
+ filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
+ if (filenames.size () != 2)
+ {
+ std::cout << "Filenames missing.\n";
+ showHelp (argv[0]);
+ exit (-1);
+ }
+
+ model_filename_ = argv[filenames[0]];
+ scene_filename_ = argv[filenames[1]];
+
+ //Program behavior
+ if (pcl::console::find_switch (argc, argv, "-k"))
+ {
+ show_keypoints_ = true;
+ }
+
+ std::string used_algorithm;
+ if (pcl::console::parse_argument (argc, argv, "--algorithm", used_algorithm) != -1)
+ {
+ if (used_algorithm.compare ("Hough") == 0)
+ {
+ use_hough_ = true;
+ }
+ else if (used_algorithm.compare ("GC") == 0)
+ {
+ use_hough_ = false;
+ }
+ else
+ {
+ std::cout << "Wrong algorithm name.\n";
+ showHelp (argv[0]);
+ exit (-1);
+ }
+ }
+
+ //General parameters
+ pcl::console::parse_argument (argc, argv, "--model_ss", model_ss_);
+ pcl::console::parse_argument (argc, argv, "--scene_ss", scene_ss_);
+ pcl::console::parse_argument (argc, argv, "--rf_rad", rf_rad_);
+ pcl::console::parse_argument (argc, argv, "--descr_rad", descr_rad_);
+ pcl::console::parse_argument (argc, argv, "--cg_size", cg_size_);
+ pcl::console::parse_argument (argc, argv, "--cg_thresh", cg_thresh_);
+ pcl::console::parse_argument (argc, argv, "--icp_max_iter", icp_max_iter_);
+ pcl::console::parse_argument (argc, argv, "--icp_corr_distance", icp_corr_distance_);
+ pcl::console::parse_argument (argc, argv, "--hv_clutter_reg", hv_clutter_reg_);
+ pcl::console::parse_argument (argc, argv, "--hv_inlier_th", hv_inlier_th_);
+ pcl::console::parse_argument (argc, argv, "--hv_occlusion_th", hv_occlusion_th_);
+ pcl::console::parse_argument (argc, argv, "--hv_rad_clutter", hv_rad_clutter_);
+ pcl::console::parse_argument (argc, argv, "--hv_regularizer", hv_regularizer_);
+ pcl::console::parse_argument (argc, argv, "--hv_rad_normals", hv_rad_normals_);
+ pcl::console::parse_argument (argc, argv, "--hv_detect_clutter", hv_detect_clutter_);
+}
+
+int
+main (int argc,
+ char *argv[])
+{
+ parseCommandLine (argc, argv);
+
+ pcl::PointCloud<PointType>::Ptr model (new pcl::PointCloud<PointType> ());
+ pcl::PointCloud<PointType>::Ptr model_keypoints (new pcl::PointCloud<PointType> ());
+ pcl::PointCloud<PointType>::Ptr scene (new pcl::PointCloud<PointType> ());
+ pcl::PointCloud<PointType>::Ptr scene_keypoints (new pcl::PointCloud<PointType> ());
+ pcl::PointCloud<NormalType>::Ptr model_normals (new pcl::PointCloud<NormalType> ());
+ pcl::PointCloud<NormalType>::Ptr scene_normals (new pcl::PointCloud<NormalType> ());
+ pcl::PointCloud<DescriptorType>::Ptr model_descriptors (new pcl::PointCloud<DescriptorType> ());
+ pcl::PointCloud<DescriptorType>::Ptr scene_descriptors (new pcl::PointCloud<DescriptorType> ());
+
+ /**
+ * Load Clouds
+ */
+ if (pcl::io::loadPCDFile (model_filename_, *model) < 0)
+ {
+ std::cout << "Error loading model cloud." << std::endl;
+ showHelp (argv[0]);
+ return (-1);
+ }
+ if (pcl::io::loadPCDFile (scene_filename_, *scene) < 0)
+ {
+ std::cout << "Error loading scene cloud." << std::endl;
+ showHelp (argv[0]);
+ return (-1);
+ }
+
+ /**
+ * Compute Normals
+ */
+ pcl::NormalEstimationOMP<PointType, NormalType> norm_est;
+ norm_est.setKSearch (10);
+ norm_est.setInputCloud (model);
+ norm_est.compute (*model_normals);
+
+ norm_est.setInputCloud (scene);
+ norm_est.compute (*scene_normals);
+
+ /**
+ * Downsample Clouds to Extract keypoints
+ */
+ pcl::UniformSampling<PointType> uniform_sampling;
+ uniform_sampling.setInputCloud (model);
+ uniform_sampling.setRadiusSearch (model_ss_);
+ uniform_sampling.filter (*model_keypoints);
+ std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl;
+
+ uniform_sampling.setInputCloud (scene);
+ uniform_sampling.setRadiusSearch (scene_ss_);
+ uniform_sampling.filter (*scene_keypoints);
+ std::cout << "Scene total points: " << scene->size () << "; Selected Keypoints: " << scene_keypoints->size () << std::endl;
+
+ /**
+ * Compute Descriptor for keypoints
+ */
+ pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est;
+ descr_est.setRadiusSearch (descr_rad_);
+
+ descr_est.setInputCloud (model_keypoints);
+ descr_est.setInputNormals (model_normals);
+ descr_est.setSearchSurface (model);
+ descr_est.compute (*model_descriptors);
+
+ descr_est.setInputCloud (scene_keypoints);
+ descr_est.setInputNormals (scene_normals);
+ descr_est.setSearchSurface (scene);
+ descr_est.compute (*scene_descriptors);
+
+ /**
+ * Find Model-Scene Correspondences with KdTree
+ */
+ pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
+ pcl::KdTreeFLANN<DescriptorType> match_search;
+ match_search.setInputCloud (model_descriptors);
+ std::vector<int> model_good_keypoints_indices;
+ std::vector<int> scene_good_keypoints_indices;
+
+ for (size_t i = 0; i < scene_descriptors->size (); ++i)
+ {
+ std::vector<int> neigh_indices (1);
+ std::vector<float> neigh_sqr_dists (1);
+ if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs
+ {
+ continue;
+ }
+ int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
+ if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25f)
+ {
+ pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
+ model_scene_corrs->push_back (corr);
+ model_good_keypoints_indices.push_back (corr.index_query);
+ scene_good_keypoints_indices.push_back (corr.index_match);
+ }
+ }
+ pcl::PointCloud<PointType>::Ptr model_good_kp (new pcl::PointCloud<PointType> ());
+ pcl::PointCloud<PointType>::Ptr scene_good_kp (new pcl::PointCloud<PointType> ());
+ pcl::copyPointCloud (*model_keypoints, model_good_keypoints_indices, *model_good_kp);
+ pcl::copyPointCloud (*scene_keypoints, scene_good_keypoints_indices, *scene_good_kp);
+
+ std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;
+
+ /**
+ * Clustering
+ */
+ std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
+ std::vector < pcl::Correspondences > clustered_corrs;
+
+ if (use_hough_)
+ {
+ pcl::PointCloud<RFType>::Ptr model_rf (new pcl::PointCloud<RFType> ());
+ pcl::PointCloud<RFType>::Ptr scene_rf (new pcl::PointCloud<RFType> ());
+
+ pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est;
+ rf_est.setFindHoles (true);
+ rf_est.setRadiusSearch (rf_rad_);
+
+ rf_est.setInputCloud (model_keypoints);
+ rf_est.setInputNormals (model_normals);
+ rf_est.setSearchSurface (model);
+ rf_est.compute (*model_rf);
+
+ rf_est.setInputCloud (scene_keypoints);
+ rf_est.setInputNormals (scene_normals);
+ rf_est.setSearchSurface (scene);
+ rf_est.compute (*scene_rf);
+
+ // Clustering
+ pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
+ clusterer.setHoughBinSize (cg_size_);
+ clusterer.setHoughThreshold (cg_thresh_);
+ clusterer.setUseInterpolation (true);
+ clusterer.setUseDistanceWeight (false);
+
+ clusterer.setInputCloud (model_keypoints);
+ clusterer.setInputRf (model_rf);
+ clusterer.setSceneCloud (scene_keypoints);
+ clusterer.setSceneRf (scene_rf);
+ clusterer.setModelSceneCorrespondences (model_scene_corrs);
+
+ clusterer.recognize (rototranslations, clustered_corrs);
+ }
+ else
+ {
+ pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
+ gc_clusterer.setGCSize (cg_size_);
+ gc_clusterer.setGCThreshold (cg_thresh_);
+
+ gc_clusterer.setInputCloud (model_keypoints);
+ gc_clusterer.setSceneCloud (scene_keypoints);
+ gc_clusterer.setModelSceneCorrespondences (model_scene_corrs);
+
+ gc_clusterer.recognize (rototranslations, clustered_corrs);
+ }
+
+ /**
+ * Stop if no instances
+ */
+ if (rototranslations.size () <= 0)
+ {
+ cout << "*** No instances found! ***" << endl;
+ return (0);
+ }
+ else
+ {
+ cout << "Recognized Instances: " << rototranslations.size () << endl << endl;
+ }
+
+ /**
+ * Generates clouds for each instances found
+ */
+ std::vector<pcl::PointCloud<PointType>::ConstPtr> instances;
+
+ for (size_t i = 0; i < rototranslations.size (); ++i)
+ {
+ pcl::PointCloud<PointType>::Ptr rotated_model (new pcl::PointCloud<PointType> ());
+ pcl::transformPointCloud (*model, *rotated_model, rototranslations[i]);
+ instances.push_back (rotated_model);
+ }
+
+ /**
+ * ICP
+ */
+ std::vector<pcl::PointCloud<PointType>::ConstPtr> registered_instances;
+ if (true)
+ {
+ cout << "--- ICP ---------" << endl;
+
+ for (size_t i = 0; i < rototranslations.size (); ++i)
+ {
+ pcl::IterativeClosestPoint<PointType, PointType> icp;
+ icp.setMaximumIterations (icp_max_iter_);
+ icp.setMaxCorrespondenceDistance (icp_corr_distance_);
+ icp.setInputTarget (scene);
+ icp.setInputSource (instances[i]);
+ pcl::PointCloud<PointType>::Ptr registered (new pcl::PointCloud<PointType>);
+ icp.align (*registered);
+ registered_instances.push_back (registered);
+ cout << "Instance " << i << " ";
+ if (icp.hasConverged ())
+ {
+ cout << "Aligned!" << endl;
+ }
+ else
+ {
+ cout << "Not Aligned!" << endl;
+ }
+ }
+
+ cout << "-----------------" << endl << endl;
+ }
+
+ /**
+ * Hypothesis Verification
+ */
+ cout << "--- Hypotheses Verification ---" << endl;
+ std::vector<bool> hypotheses_mask; // Mask Vector to identify positive hypotheses
+
+ pcl::GlobalHypothesesVerification<PointType, PointType> GoHv;
+
+ GoHv.setSceneCloud (scene); // Scene Cloud
+ GoHv.addModels (registered_instances, true); //Models to verify
+
+ GoHv.setInlierThreshold (hv_inlier_th_);
+ GoHv.setOcclusionThreshold (hv_occlusion_th_);
+ GoHv.setRegularizer (hv_regularizer_);
+ GoHv.setRadiusClutter (hv_rad_clutter_);
+ GoHv.setClutterRegularizer (hv_clutter_reg_);
+ GoHv.setDetectClutter (hv_detect_clutter_);
+ GoHv.setRadiusNormals (hv_rad_normals_);
+
+ GoHv.verify ();
+ GoHv.getMask (hypotheses_mask); // i-element TRUE if hvModels[i] verifies hypotheses
+
+ for (int i = 0; i < hypotheses_mask.size (); i++)
+ {
+ if (hypotheses_mask[i])
+ {
+ cout << "Instance " << i << " is GOOD! <---" << endl;
+ }
+ else
+ {
+ cout << "Instance " << i << " is bad!" << endl;
+ }
+ }
+ cout << "-------------------------------" << endl;
+
+ /**
+ * Visualization
+ */
+ pcl::visualization::PCLVisualizer viewer ("Hypotheses Verification");
+ viewer.addPointCloud (scene, "scene_cloud");
+
+ pcl::PointCloud<PointType>::Ptr off_scene_model (new pcl::PointCloud<PointType> ());
+ pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints (new pcl::PointCloud<PointType> ());
+
+ pcl::PointCloud<PointType>::Ptr off_model_good_kp (new pcl::PointCloud<PointType> ());
+ pcl::transformPointCloud (*model, *off_scene_model, Eigen::Vector3f (-1, 0, 0), Eigen::Quaternionf (1, 0, 0, 0));
+ pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (-1, 0, 0), Eigen::Quaternionf (1, 0, 0, 0));
+ pcl::transformPointCloud (*model_good_kp, *off_model_good_kp, Eigen::Vector3f (-1, 0, 0), Eigen::Quaternionf (1, 0, 0, 0));
+
+ if (show_keypoints_)
+ {
+ CloudStyle modelStyle = style_white;
+ pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler (off_scene_model, modelStyle.r, modelStyle.g, modelStyle.b);
+ viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, modelStyle.size, "off_scene_model");
+ }
+
+ if (show_keypoints_)
+ {
+ CloudStyle goodKeypointStyle = style_violet;
+ pcl::visualization::PointCloudColorHandlerCustom<PointType> model_good_keypoints_color_handler (off_model_good_kp, goodKeypointStyle.r, goodKeypointStyle.g,
+ goodKeypointStyle.b);
+ viewer.addPointCloud (off_model_good_kp, model_good_keypoints_color_handler, "model_good_keypoints");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, goodKeypointStyle.size, "model_good_keypoints");
+
+ pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_good_keypoints_color_handler (scene_good_kp, goodKeypointStyle.r, goodKeypointStyle.g,
+ goodKeypointStyle.b);
+ viewer.addPointCloud (scene_good_kp, scene_good_keypoints_color_handler, "scene_good_keypoints");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, goodKeypointStyle.size, "scene_good_keypoints");
+ }
+
+ for (size_t i = 0; i < instances.size (); ++i)
+ {
+ std::stringstream ss_instance;
+ ss_instance << "instance_" << i;
+
+ CloudStyle clusterStyle = style_red;
+ pcl::visualization::PointCloudColorHandlerCustom<PointType> instance_color_handler (instances[i], clusterStyle.r, clusterStyle.g, clusterStyle.b);
+ viewer.addPointCloud (instances[i], instance_color_handler, ss_instance.str ());
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, clusterStyle.size, ss_instance.str ());
+
+ CloudStyle registeredStyles = hypotheses_mask[i] ? style_green : style_cyan;
+ ss_instance << "_registered" << endl;
+ pcl::visualization::PointCloudColorHandlerCustom<PointType> registered_instance_color_handler (registered_instances[i], registeredStyles.r,
+ registeredStyles.g, registeredStyles.b);
+ viewer.addPointCloud (registered_instances[i], registered_instance_color_handler, ss_instance.str ());
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, registeredStyles.size, ss_instance.str ());
+ }
+
+ while (!viewer.wasStopped ())
+ {
+ viewer.spinOnce ();
+ }
+
+ return (0);
+}
--- /dev/null
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+project(people_detect)
+
+find_package(PCL 1.7 REQUIRED)
+
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+#Searching CUDA
+FIND_PACKAGE(CUDA)
+
+#Include the FindCUDA script
+INCLUDE(FindCUDA)
+
+cuda_add_executable (people_detect src/people_detect.cpp)
+target_link_libraries (people_detect ${PCL_LIBRARIES})
--- /dev/null
+if(NOT VTK_FOUND)
+ set(DEFAULT FALSE)
+ set(REASON "VTK was not found.")
+else(NOT VTK_FOUND)
+ set(DEFAULT TRUE)
+ set(REASON)
+ set(VTK_USE_FILE ${VTK_USE_FILE} CACHE INTERNAL "VTK_USE_FILE")
+ include (${VTK_USE_FILE})
+ include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include)
+endif(NOT VTK_FOUND)
+
+set(the_target people_tracking)
+
+include_directories(${VTK_INCLUDE_DIRS})
+
+PCL_ADD_EXECUTABLE(people_detect ${SUBSYS_NAME} src/people_detect.cpp)
+target_link_libraries (people_detect pcl_common pcl_filters pcl_kdtree pcl_segmentation pcl_kdtree pcl_gpu_people pcl_filters pcl_io pcl_visualization ${Boost_LIBRARIES})
+
--- /dev/null
+/**
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * @brief This file is the execution node of the Human Tracking
+ * @authors Koen Buys, Anatoly Baksheev
+ **/
+
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/common/time.h>
+#include <pcl/exceptions.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
+#include <pcl/gpu/containers/initialization.h>
+#include <pcl/gpu/people/people_detector.h>
+#include <pcl/gpu/people/colormap.h>
+#include <pcl/visualization/image_viewer.h>
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/oni_grabber.h>
+#include <pcl/io/pcd_grabber.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/png_io.h>
+#include <boost/filesystem.hpp>
+
+#include <iostream>
+
+namespace pc = pcl::console;
+using namespace pcl::visualization;
+using namespace pcl::gpu;
+using namespace pcl;
+using namespace std;
+
+struct SampledScopeTime : public StopWatch
+{
+ enum { EACH = 33 };
+ SampledScopeTime(int& time_ms) : time_ms_(time_ms) {}
+ ~SampledScopeTime()
+ {
+ static int i_ = 0;
+ time_ms_ += getTime ();
+ if (i_ % EACH == 0 && i_)
+ {
+ cout << "Average frame time = " << time_ms_ / EACH << "ms ( " << 1000.f * EACH / time_ms_ << "fps )" << endl;
+ time_ms_ = 0;
+ }
+ ++i_;
+ }
+ private:
+ int& time_ms_;
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+string
+make_name(int counter, const char* suffix)
+{
+ char buf[4096];
+ sprintf (buf, "./people%04d_%s.png", counter, suffix);
+ return buf;
+}
+
+template<typename T> void
+savePNGFile(const std::string& filename, const pcl::gpu::DeviceArray2D<T>& arr)
+{
+ int c;
+ pcl::PointCloud<T> cloud(arr.cols(), arr.rows());
+ arr.download(cloud.points, c);
+ pcl::io::savePNGFile(filename, cloud);
+}
+
+template <typename T> void
+savePNGFile (const std::string& filename, const pcl::PointCloud<T>& cloud)
+{
+ pcl::io::savePNGFile(filename, cloud);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+class PeoplePCDApp
+{
+ public:
+ typedef pcl::gpu::people::PeopleDetector PeopleDetector;
+
+ enum { COLS = 640, ROWS = 480 };
+
+ PeoplePCDApp (pcl::Grabber& capture) : capture_(capture), exit_(false), time_ms_(0), cloud_cb_(true), counter_(0), final_view_("Final labeling"), depth_view_("Depth")
+ {
+ final_view_.setSize (COLS, ROWS);
+ depth_view_.setSize (COLS, ROWS);
+
+ final_view_.setPosition (0, 0);
+ depth_view_.setPosition (650, 0);
+
+ cmap_device_.create(ROWS, COLS);
+ cmap_host_.points.resize(COLS * ROWS);
+ depth_device_.create(ROWS, COLS);
+ image_device_.create(ROWS, COLS);
+
+ depth_host_.points.resize(COLS * ROWS);
+
+ rgba_host_.points.resize(COLS * ROWS);
+ rgb_host_.resize(COLS * ROWS * 3);
+
+ people::uploadColorMap(color_map_);
+
+ }
+
+ void
+ visualizeAndWrite(bool write = false)
+ {
+ const PeopleDetector::Labels& labels = people_detector_.rdf_detector_->getLabels();
+ people::colorizeLabels(color_map_, labels, cmap_device_);
+
+ int c;
+ cmap_host_.width = cmap_device_.cols();
+ cmap_host_.height = cmap_device_.rows();
+ cmap_host_.points.resize(cmap_host_.width * cmap_host_.height);
+ cmap_device_.download(cmap_host_.points, c);
+
+ final_view_.showRGBImage<pcl::RGB>(cmap_host_);
+ final_view_.spinOnce(1, true);
+
+ if (cloud_cb_)
+ {
+ depth_host_.width = people_detector_.depth_device1_.cols();
+ depth_host_.height = people_detector_.depth_device1_.rows();
+ depth_host_.points.resize(depth_host_.width * depth_host_.height);
+ people_detector_.depth_device1_.download(depth_host_.points, c);
+ }
+
+ depth_view_.showShortImage(&depth_host_.points[0], depth_host_.width, depth_host_.height, 0, 5000, true);
+ depth_view_.spinOnce(1, true);
+
+ if (write)
+ {
+ if (cloud_cb_)
+ savePNGFile(make_name(counter_, "ii"), cloud_host_);
+ else
+ savePNGFile(make_name(counter_, "ii"), rgba_host_);
+ savePNGFile(make_name(counter_, "c2"), cmap_host_);
+ savePNGFile(make_name(counter_, "s2"), labels);
+ savePNGFile(make_name(counter_, "d1"), people_detector_.depth_device1_);
+ savePNGFile(make_name(counter_, "d2"), people_detector_.depth_device2_);
+ }
+ }
+
+ void source_cb1(const boost::shared_ptr<const PointCloud<PointXYZRGBA> >& cloud)
+ {
+ {
+ boost::mutex::scoped_lock lock(data_ready_mutex_);
+ if (exit_)
+ return;
+
+ pcl::copyPointCloud(*cloud, cloud_host_);
+ }
+ data_ready_cond_.notify_one();
+ }
+
+ void source_cb2(const boost::shared_ptr<openni_wrapper::Image>& image_wrapper, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_wrapper, float)
+ {
+ {
+ boost::mutex::scoped_try_lock lock(data_ready_mutex_);
+
+ if (exit_ || !lock)
+ return;
+
+ //getting depth
+ int w = depth_wrapper->getWidth();
+ int h = depth_wrapper->getHeight();
+ int s = w * PeopleDetector::Depth::elem_size;
+ const unsigned short *data = depth_wrapper->getDepthMetaData().Data();
+ depth_device_.upload(data, s, h, w);
+
+ depth_host_.points.resize(w *h);
+ depth_host_.width = w;
+ depth_host_.height = h;
+ std::copy(data, data + w * h, &depth_host_.points[0]);
+
+ //getting image
+ w = image_wrapper->getWidth();
+ h = image_wrapper->getHeight();
+ s = w * PeopleDetector::Image::elem_size;
+
+ //fill rgb array
+ rgb_host_.resize(w * h * 3);
+ image_wrapper->fillRGB(w, h, (unsigned char*)&rgb_host_[0]);
+
+ // convert to rgba, TODO image_wrapper should be updated to support rgba directly
+ rgba_host_.points.resize(w * h);
+ rgba_host_.width = w;
+ rgba_host_.height = h;
+ for(int i = 0; i < rgba_host_.size(); ++i)
+ {
+ const unsigned char *pixel = &rgb_host_[i * 3];
+ RGB& rgba = rgba_host_.points[i];
+ rgba.r = pixel[0];
+ rgba.g = pixel[1];
+ rgba.b = pixel[2];
+ }
+ image_device_.upload(&rgba_host_.points[0], s, h, w);
+ }
+ data_ready_cond_.notify_one();
+ }
+
+ void
+ startMainLoop ()
+ {
+ cloud_cb_ = false;
+
+ PCDGrabberBase* ispcd = dynamic_cast<pcl::PCDGrabberBase*>(&capture_);
+ if (ispcd)
+ cloud_cb_= true;
+
+ typedef boost::shared_ptr<openni_wrapper::DepthImage> DepthImagePtr;
+ typedef boost::shared_ptr<openni_wrapper::Image> ImagePtr;
+
+ boost::function<void (const boost::shared_ptr<const PointCloud<PointXYZRGBA> >&)> func1 = boost::bind (&PeoplePCDApp::source_cb1, this, _1);
+ boost::function<void (const ImagePtr&, const DepthImagePtr&, float constant)> func2 = boost::bind (&PeoplePCDApp::source_cb2, this, _1, _2, _3);
+ boost::signals2::connection c = cloud_cb_ ? capture_.registerCallback (func1) : capture_.registerCallback (func2);
+
+ {
+ boost::unique_lock<boost::mutex> lock(data_ready_mutex_);
+
+ try
+ {
+ capture_.start ();
+ while (!exit_ && !final_view_.wasStopped())
+ {
+ bool has_data = data_ready_cond_.timed_wait(lock, boost::posix_time::millisec(100));
+ if(has_data)
+ {
+ SampledScopeTime fps(time_ms_);
+
+ if (cloud_cb_)
+ process_return_ = people_detector_.process(cloud_host_.makeShared());
+ else
+ process_return_ = people_detector_.process(depth_device_, image_device_);
+
+ ++counter_;
+ }
+
+ if(has_data && (process_return_ == 2))
+ visualizeAndWrite();
+ }
+ final_view_.spinOnce (3);
+ }
+ catch (const std::bad_alloc& /*e*/) { cout << "Bad alloc" << endl; }
+ catch (const std::exception& /*e*/) { cout << "Exception" << endl; }
+
+ capture_.stop ();
+ }
+ c.disconnect();
+ }
+
+ boost::mutex data_ready_mutex_;
+ boost::condition_variable data_ready_cond_;
+
+ pcl::Grabber& capture_;
+
+ bool cloud_cb_;
+ bool exit_;
+ int time_ms_;
+ int counter_;
+ int process_return_;
+ PeopleDetector people_detector_;
+ PeopleDetector::Image cmap_device_;
+ pcl::PointCloud<pcl::RGB> cmap_host_;
+
+ PeopleDetector::Depth depth_device_;
+ PeopleDetector::Image image_device_;
+
+ pcl::PointCloud<unsigned short> depth_host_;
+ pcl::PointCloud<pcl::RGB> rgba_host_;
+ std::vector<unsigned char> rgb_host_;
+
+ PointCloud<PointXYZRGBA> cloud_host_;
+
+ ImageViewer final_view_;
+ ImageViewer depth_view_;
+
+ DeviceArray<pcl::RGB> color_map_;
+};
+
+int main(int argc, char** argv)
+{
+ // selecting GPU and prining info
+ int device = 0;
+ pc::parse_argument (argc, argv, "-gpu", device);
+ pcl::gpu::setDevice (device);
+ pcl::gpu::printShortCudaDeviceInfo (device);
+
+ // selecting data source
+ boost::shared_ptr<pcl::Grabber> capture;
+ capture.reset( new pcl::OpenNIGrabber() );
+
+ //selecting tree files
+ vector<string> tree_files;
+ tree_files.push_back("Data/forest1/tree_20.txt");
+ tree_files.push_back("Data/forest2/tree_20.txt");
+ tree_files.push_back("Data/forest3/tree_20.txt");
+ tree_files.push_back("Data/forest4/tree_20.txt");
+
+ pc::parse_argument (argc, argv, "-tree0", tree_files[0]);
+ pc::parse_argument (argc, argv, "-tree1", tree_files[1]);
+ pc::parse_argument (argc, argv, "-tree2", tree_files[2]);
+ pc::parse_argument (argc, argv, "-tree3", tree_files[3]);
+
+ int num_trees = (int)tree_files.size();
+ pc::parse_argument (argc, argv, "-numTrees", num_trees);
+
+ tree_files.resize(num_trees);
+ if (num_trees == 0 || num_trees > 4)
+ return cout << "Invalid number of trees" << endl, -1;
+
+ try
+ {
+ // loading trees
+ typedef pcl::gpu::people::RDFBodyPartsDetector RDFBodyPartsDetector;
+ RDFBodyPartsDetector::Ptr rdf(new RDFBodyPartsDetector(tree_files));
+ PCL_INFO("Loaded files into rdf");
+
+ // Create the app
+ PeoplePCDApp app(*capture);
+ app.people_detector_.rdf_detector_ = rdf;
+
+ // executing
+ app.startMainLoop ();
+ }
+ catch (const pcl::PCLException& e) { cout << "PCLException: " << e.detailedMessage() << endl; }
+ catch (const std::runtime_error& e) { cout << e.what() << endl; }
+ catch (const std::bad_alloc& /*e*/) { cout << "Bad alloc" << endl; }
+ catch (const std::exception& /*e*/) { cout << "Exception" << endl; }
+
+ return 0;
+}
+
+
+
// Define a translation of 2.5 meters on the x axis.
transform_2.translation() << 2.5, 0.0, 0.0;
- // The same rotation matrix as before; tetha radians arround Z axis
+ // The same rotation matrix as before; theta radians arround Z axis
transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));
// Print the transformation
--- /dev/null
+cmake_minimum_required (VERSION 2.6 FATAL_ERROR)
+
+project (pcl-colorize_cloud)
+find_package (Qt4 REQUIRED)
+find_package (VTK REQUIRED)
+find_package (PCL 1.7.1 REQUIRED)
+
+include_directories (${PCL_INCLUDE_DIRS})
+link_directories (${PCL_LIBRARY_DIRS})
+add_definitions (${PCL_DEFINITIONS})
+
+set (project_SOURCES main.cpp pclviewer.cpp)
+set (project_HEADERS pclviewer.h)
+set (project_FORMS pclviewer.ui)
+set (VTK_LIBRARIES vtkRendering vtkGraphics vtkHybrid QVTK)
+
+QT4_WRAP_CPP (project_HEADERS_MOC ${project_HEADERS})
+QT4_WRAP_UI (project_FORMS_HEADERS ${project_FORMS})
+
+INCLUDE (${QT_USE_FILE})
+ADD_DEFINITIONS (${QT_DEFINITIONS})
+
+ADD_EXECUTABLE (colorize_cloud ${project_SOURCES}
+ ${project_FORMS_HEADERS}
+ ${project_HEADERS_MOC})
+
+TARGET_LINK_LIBRARIES (colorize_cloud ${QT_LIBRARIES} ${PCL_LIBRARIES} ${VTK_LIBRARIES})
+
--- /dev/null
+#include "pclviewer.h"
+#include <QApplication>
+#include <QMainWindow>
+
+int
+main (int argc,
+ char *argv[])
+{
+ QApplication a (argc, argv);
+ PCLViewer w;
+ w.show ();
+
+ return a.exec ();
+}
--- /dev/null
+#-------------------------------------------------
+#
+# Project created by QtCreator 2014-11-11T14:00:00
+#
+#-------------------------------------------------
+
+QT += core gui
+
+greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
+
+TARGET = colorize_cloud
+TEMPLATE = app
+
+
+SOURCES += main.cpp\
+ pclviewer.cpp
+
+HEADERS += pclviewer.h
+
+FORMS += pclviewer.ui
--- /dev/null
+#include "pclviewer.h"
+#include "../build/ui_pclviewer.h"
+
+PCLViewer::PCLViewer (QWidget *parent) :
+ QMainWindow (parent),
+ ui (new Ui::PCLViewer),
+ filtering_axis_ (1), // = y
+ color_mode_ (4) // = Rainbow
+{
+ ui->setupUi (this);
+ this->setWindowTitle ("PCL viewer");
+
+ // Setup the cloud pointer
+ cloud_.reset (new PointCloudT);
+ // The number of points in the cloud
+ cloud_->resize (500);
+
+ // Fill the cloud with random points
+ for (size_t i = 0; i < cloud_->points.size (); ++i)
+ {
+ cloud_->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
+ cloud_->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
+ cloud_->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
+ }
+
+ // Set up the QVTK window
+ viewer_.reset (new pcl::visualization::PCLVisualizer ("viewer", false));
+ viewer_->setBackgroundColor (0.1, 0.1, 0.1);
+ ui->qvtkWidget->SetRenderWindow (viewer_->getRenderWindow ());
+ viewer_->setupInteractor (ui->qvtkWidget->GetInteractor (), ui->qvtkWidget->GetRenderWindow ());
+ ui->qvtkWidget->update ();
+
+ // Connect "Load" and "Save" buttons and their functions
+ connect (ui->pushButton_load, SIGNAL(clicked ()), this, SLOT(loadFileButtonPressed ()));
+ connect (ui->pushButton_save, SIGNAL(clicked ()), this, SLOT(saveFileButtonPressed ()));
+
+ // Connect X,Y,Z radio buttons and their functions
+ connect (ui->radioButton_x, SIGNAL(clicked ()), this, SLOT(axisChosen ()));
+ connect (ui->radioButton_y, SIGNAL(clicked ()), this, SLOT(axisChosen ()));
+ connect (ui->radioButton_z, SIGNAL(clicked ()), this, SLOT(axisChosen ()));
+
+ connect (ui->radioButton_BlueRed, SIGNAL(clicked ()), this, SLOT(lookUpTableChosen()));
+ connect (ui->radioButton_GreenMagenta, SIGNAL(clicked ()), this, SLOT(lookUpTableChosen()));
+ connect (ui->radioButton_WhiteRed, SIGNAL(clicked ()), this, SLOT(lookUpTableChosen()));
+ connect (ui->radioButton_GreyRed, SIGNAL(clicked ()), this, SLOT(lookUpTableChosen()));
+ connect (ui->radioButton_Rainbow, SIGNAL(clicked ()), this, SLOT(lookUpTableChosen()));
+
+ // Color the randomly generated cloud
+ colorCloudDistances ();
+ viewer_->addPointCloud (cloud_, "cloud");
+ viewer_->resetCamera ();
+ ui->qvtkWidget->update ();
+}
+
+PCLViewer::~PCLViewer ()
+{
+ delete ui;
+}
+
+void
+PCLViewer::loadFileButtonPressed ()
+{
+ // You might want to change "/home/" if you're not on an *nix platform
+ QString filename = QFileDialog::getOpenFileName (this, tr ("Open point cloud"), "/home/", tr ("Point cloud data (*.pcd *.ply)"));
+
+ PCL_INFO("File chosen: %s\n", filename.toStdString ().c_str ());
+ PointCloudT::Ptr cloud_tmp (new PointCloudT);
+
+ if (filename.isEmpty ())
+ return;
+
+ int return_status;
+ if (filename.endsWith (".pcd", Qt::CaseInsensitive))
+ return_status = pcl::io::loadPCDFile (filename.toStdString (), *cloud_tmp);
+ else
+ return_status = pcl::io::loadPLYFile (filename.toStdString (), *cloud_tmp);
+
+ if (return_status != 0)
+ {
+ PCL_ERROR("Error reading point cloud %s\n", filename.toStdString ().c_str ());
+ return;
+ }
+
+ // If point cloud contains NaN values, remove them before updating the visualizer point cloud
+ if (cloud_tmp->is_dense)
+ pcl::copyPointCloud (*cloud_tmp, *cloud_);
+ else
+ {
+ PCL_WARN("Cloud is not dense! Non finite points will be removed\n");
+ std::vector<int> vec;
+ pcl::removeNaNFromPointCloud (*cloud_tmp, *cloud_, vec);
+ }
+
+ colorCloudDistances ();
+ viewer_->updatePointCloud (cloud_, "cloud");
+ viewer_->resetCamera ();
+ ui->qvtkWidget->update ();
+}
+
+void
+PCLViewer::saveFileButtonPressed ()
+{
+ // You might want to change "/home/" if you're not on an *nix platform
+ QString filename = QFileDialog::getSaveFileName(this, tr ("Open point cloud"), "/home/", tr ("Point cloud data (*.pcd *.ply)"));
+
+ PCL_INFO("File chosen: %s\n", filename.toStdString ().c_str ());
+
+ if (filename.isEmpty ())
+ return;
+
+ int return_status;
+ if (filename.endsWith (".pcd", Qt::CaseInsensitive))
+ return_status = pcl::io::savePCDFileBinary (filename.toStdString (), *cloud_);
+ else if (filename.endsWith (".ply", Qt::CaseInsensitive))
+ return_status = pcl::io::savePLYFileBinary (filename.toStdString (), *cloud_);
+ else
+ {
+ filename.append(".ply");
+ return_status = pcl::io::savePLYFileBinary (filename.toStdString (), *cloud_);
+ }
+
+ if (return_status != 0)
+ {
+ PCL_ERROR("Error writing point cloud %s\n", filename.toStdString ().c_str ());
+ return;
+ }
+}
+
+void
+PCLViewer::axisChosen ()
+{
+ // Only 1 of the button can be checked at the time (mutual exclusivity) in a group of radio buttons
+ if (ui->radioButton_x->isChecked ())
+ {
+ PCL_INFO("x filtering chosen\n");
+ filtering_axis_ = 0;
+ }
+ else if (ui->radioButton_y->isChecked ())
+ {
+ PCL_INFO("y filtering chosen\n");
+ filtering_axis_ = 1;
+ }
+ else
+ {
+ PCL_INFO("z filtering chosen\n");
+ filtering_axis_ = 2;
+ }
+
+ colorCloudDistances ();
+ viewer_->updatePointCloud (cloud_, "cloud");
+ ui->qvtkWidget->update ();
+}
+
+void
+PCLViewer::lookUpTableChosen ()
+{
+ // Only 1 of the button can be checked at the time (mutual exclusivity) in a group of radio buttons
+ if (ui->radioButton_BlueRed->isChecked ())
+ {
+ PCL_INFO("Blue -> Red LUT chosen\n");
+ color_mode_ = 0;
+ }
+ else if (ui->radioButton_GreenMagenta->isChecked ())
+ {
+ PCL_INFO("Green -> Magenta LUT chosen\n");
+ color_mode_ = 1;
+ }
+ else if (ui->radioButton_WhiteRed->isChecked ())
+ {
+ PCL_INFO("White -> Red LUT chosen\n");
+ color_mode_ = 2;
+ }
+ else if (ui->radioButton_GreyRed->isChecked ())
+ {
+ PCL_INFO("Grey / Red LUT chosen\n");
+ color_mode_ = 3;
+ }
+ else
+ {
+ PCL_INFO("Rainbow LUT chosen\n");
+ color_mode_ = 4;
+ }
+
+ colorCloudDistances ();
+ viewer_->updatePointCloud (cloud_, "cloud");
+ ui->qvtkWidget->update ();
+}
+
+void
+PCLViewer::colorCloudDistances ()
+{
+ // Find the minimum and maximum values along the selected axis
+ double min, max;
+ // Set an initial value
+ switch (filtering_axis_)
+ {
+ case 0: // x
+ min = cloud_->points[0].x;
+ max = cloud_->points[0].x;
+ break;
+ case 1: // y
+ min = cloud_->points[0].y;
+ max = cloud_->points[0].y;
+ break;
+ default: // z
+ min = cloud_->points[0].z;
+ max = cloud_->points[0].z;
+ break;
+ }
+
+ // Search for the minimum/maximum
+ for (PointCloudT::iterator cloud_it = cloud_->begin (); cloud_it != cloud_->end (); ++cloud_it)
+ {
+ switch (filtering_axis_)
+ {
+ case 0: // x
+ if (min > cloud_it->x)
+ min = cloud_it->x;
+
+ if (max < cloud_it->x)
+ max = cloud_it->x;
+ break;
+ case 1: // y
+ if (min > cloud_it->y)
+ min = cloud_it->y;
+
+ if (max < cloud_it->y)
+ max = cloud_it->y;
+ break;
+ default: // z
+ if (min > cloud_it->z)
+ min = cloud_it->z;
+
+ if (max < cloud_it->z)
+ max = cloud_it->z;
+ break;
+ }
+ }
+
+ // Compute LUT scaling to fit the full histogram spectrum
+ double lut_scale = 255.0 / (max - min); // max is 255, min is 0
+
+ if (min == max) // In case the cloud is flat on the chosen direction (x,y or z)
+ lut_scale = 1.0; // Avoid rounding error in boost
+
+ for (PointCloudT::iterator cloud_it = cloud_->begin (); cloud_it != cloud_->end (); ++cloud_it)
+ {
+ int value;
+ switch (filtering_axis_)
+ {
+ case 0: // x
+ value = boost::math::iround ( (cloud_it->x - min) * lut_scale); // Round the number to the closest integer
+ break;
+ case 1: // y
+ value = boost::math::iround ( (cloud_it->y - min) * lut_scale);
+ break;
+ default: // z
+ value = boost::math::iround ( (cloud_it->z - min) * lut_scale);
+ break;
+ }
+
+ // Apply color to the cloud
+ switch (color_mode_)
+ {
+ case 0:
+ // Blue (= min) -> Red (= max)
+ cloud_it->r = value;
+ cloud_it->g = 0;
+ cloud_it->b = 255 - value;
+ break;
+ case 1:
+ // Green (= min) -> Magenta (= max)
+ cloud_it->r = value;
+ cloud_it->g = 255 - value;
+ cloud_it->b = value;
+ break;
+ case 2:
+ // White (= min) -> Red (= max)
+ cloud_it->r = 255;
+ cloud_it->g = 255 - value;
+ cloud_it->b = 255 - value;
+ break;
+ case 3:
+ // Grey (< 128) / Red (> 128)
+ if (value > 128)
+ {
+ cloud_it->r = 255;
+ cloud_it->g = 0;
+ cloud_it->b = 0;
+ }
+ else
+ {
+ cloud_it->r = 128;
+ cloud_it->g = 128;
+ cloud_it->b = 128;
+ }
+ break;
+ default:
+ // Blue -> Green -> Red (~ rainbow)
+ cloud_it->r = value > 128 ? (value - 128) * 2 : 0; // r[128] = 0, r[255] = 255
+ cloud_it->g = value < 128 ? 2 * value : 255 - ( (value - 128) * 2); // g[0] = 0, g[128] = 255, g[255] = 0
+ cloud_it->b = value < 128 ? 255 - (2 * value) : 0; // b[0] = 255, b[128] = 0
+ }
+ }
+}
--- /dev/null
+#ifndef PCLVIEWER_H
+#define PCLVIEWER_H
+
+// Qt
+#include <QMainWindow>
+#include <QFileDialog>
+
+// Point Cloud Library
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/filters/filter.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+// Boost
+#include <boost/math/special_functions/round.hpp>
+
+// Visualization Toolkit (VTK)
+#include <vtkRenderWindow.h>
+
+typedef pcl::PointXYZRGBA PointT;
+typedef pcl::PointCloud<PointT> PointCloudT;
+
+namespace Ui
+{
+ class PCLViewer;
+}
+
+class PCLViewer : public QMainWindow
+{
+ Q_OBJECT
+
+ public:
+ /** @brief Constructor */
+ explicit
+ PCLViewer (QWidget *parent = 0);
+
+ /** @brief Destructor */
+ ~PCLViewer ();
+
+ public slots:
+ /** @brief Triggered whenever the "Save file" button is clicked */
+ void
+ saveFileButtonPressed ();
+
+ /** @brief Triggered whenever the "Load file" button is clicked */
+ void
+ loadFileButtonPressed ();
+
+ /** @brief Triggered whenever a button in the "Color on axis" group is clicked */
+ void
+ axisChosen ();
+
+ /** @brief Triggered whenever a button in the "Color mode" group is clicked */
+ void
+ lookUpTableChosen ();
+
+ protected:
+ /** @brief The PCL visualizer object */
+ boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_;
+
+ /** @brief The point cloud displayed */
+ PointCloudT::Ptr cloud_;
+
+ /** @brief 0 = x | 1 = y | 2 = z */
+ int filtering_axis_;
+
+ /** @brief Holds the color mode for @ref colorCloudDistances */
+ int color_mode_;
+
+ /** @brief Color point cloud on X,Y or Z axis using a Look-Up Table (LUT)
+ * Computes a LUT and color the cloud accordingly, available color palettes are :
+ *
+ * Values are on a scale from 0 to 255:
+ * 0. Blue (= 0) -> Red (= 255), this is the default value
+ * 1. Green (= 0) -> Magenta (= 255)
+ * 2. White (= 0) -> Red (= 255)
+ * 3. Grey (< 128) / Red (> 128)
+ * 4. Blue -> Green -> Red (~ rainbow)
+ *
+ * @warning If there's an outlier in the data the color may seem uniform because of this outlier!
+ * @note A boost rounding exception error will be thrown if used with a non dense point cloud
+ */
+ void
+ colorCloudDistances ();
+
+ private:
+ /** @brief ui pointer */
+ Ui::PCLViewer *ui;
+};
+
+#endif // PCLVIEWER_H
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>PCLViewer</class>
+ <widget class="QMainWindow" name="PCLViewer">
+ <property name="windowModality">
+ <enum>Qt::NonModal</enum>
+ </property>
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>896</width>
+ <height>498</height>
+ </rect>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>5000</width>
+ <height>5000</height>
+ </size>
+ </property>
+ <property name="windowTitle">
+ <string>PCLViewer</string>
+ </property>
+ <widget class="QWidget" name="centralwidget">
+ <layout class="QHBoxLayout" name="horizontalLayout_3">
+ <item>
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <item>
+ <widget class="QGroupBox" name="groupBox_Axis">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="title">
+ <string>Color on axis</string>
+ </property>
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <widget class="QRadioButton" name="radioButton_x">
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>X</string>
+ </property>
+ <property name="checked">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QRadioButton" name="radioButton_y">
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Y</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QRadioButton" name="radioButton_z">
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Z</string>
+ </property>
+ <property name="checked">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <spacer name="verticalSpacer">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item>
+ <widget class="QGroupBox" name="groupBox_ColorMode">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>230</width>
+ <height>180</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="title">
+ <string>Color mode</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QRadioButton" name="radioButton_BlueRed">
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Blue to red</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QRadioButton" name="radioButton_GreenMagenta">
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Green to magenta</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QRadioButton" name="radioButton_WhiteRed">
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>White to red</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QRadioButton" name="radioButton_GreyRed">
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Grey / red</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QRadioButton" name="radioButton_Rainbow">
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Rainbow</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="verticalSpacer_3">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QPushButton" name="pushButton_load">
+ <property name="minimumSize">
+ <size>
+ <width>50</width>
+ <height>40</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Load file</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="pushButton_save">
+ <property name="minimumSize">
+ <size>
+ <width>50</width>
+ <height>40</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Save file</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <widget class="QVTKWidget" name="qvtkWidget" native="true">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+ <horstretch>50</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>640</width>
+ <height>480</height>
+ </size>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </widget>
+ <customwidgets>
+ <customwidget>
+ <class>QVTKWidget</class>
+ <extends>QWidget</extends>
+ <header>QVTKWidget.h</header>
+ </customwidget>
+ </customwidgets>
+ <tabstops>
+ <tabstop>pushButton_load</tabstop>
+ <tabstop>radioButton_z</tabstop>
+ </tabstops>
+ <resources/>
+ <connections/>
+</ui>
+++ /dev/null
-<?xml version="1.0" encoding="UTF-8"?>
-<!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 3.0.1, 2014-05-01T16:44:18. -->
-<qtcreator>
- <data>
- <variable>ProjectExplorer.Project.ActiveTarget</variable>
- <value type="int">0</value>
- </data>
- <data>
- <variable>ProjectExplorer.Project.EditorSettings</variable>
- <valuemap type="QVariantMap">
- <value type="bool" key="EditorConfiguration.AutoIndent">true</value>
- <value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
- <value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
- <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
- <value type="QString" key="language">Cpp</value>
- <valuemap type="QVariantMap" key="value">
- <value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
- </valuemap>
- </valuemap>
- <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
- <value type="QString" key="language">QmlJS</value>
- <valuemap type="QVariantMap" key="value">
- <value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
- </valuemap>
- </valuemap>
- <value type="int" key="EditorConfiguration.CodeStyle.Count">2</value>
- <value type="QByteArray" key="EditorConfiguration.Codec">UTF-8</value>
- <value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
- <value type="int" key="EditorConfiguration.IndentSize">4</value>
- <value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
- <value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
- <value type="int" key="EditorConfiguration.PaddingMode">1</value>
- <value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
- <value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
- <value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
- <value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
- <value type="int" key="EditorConfiguration.TabSize">8</value>
- <value type="bool" key="EditorConfiguration.UseGlobal">true</value>
- <value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
- <value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
- <value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
- <value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
- <value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
- </valuemap>
- </data>
- <data>
- <variable>ProjectExplorer.Project.PluginSettings</variable>
- <valuemap type="QVariantMap"/>
- </data>
- <data>
- <variable>ProjectExplorer.Project.Target.0</variable>
- <valuemap type="QVariantMap">
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{7fcc8410-2f07-4a1b-8f7e-75865afc8bab}</value>
- <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
- <value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
- <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
- <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">../build</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProcessStep.Arguments">../src</value>
- <value type="QString" key="ProjectExplorer.ProcessStep.Command">cmake</value>
- <value type="QString" key="ProjectExplorer.ProcessStep.WorkingDirectory">%{buildDir}/../build</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Custom Process Step</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.ProcessStep</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
- <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
- <value type="QString">-w</value>
- <value type="QString">-r</value>
- </valuelist>
- <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">-j2</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
- <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
- <value type="QString">-w</value>
- <value type="QString">-r</value>
- </valuelist>
- <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
- <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
- <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Release</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
- <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">0</value>
- <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy locally</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.PluginSettings"/>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
- <valuelist type="QVariantList" key="Analyzer.Valgrind.AddedSuppressionFiles"/>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectBusEvents">false</value>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectSystime">false</value>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableBranchSim">false</value>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableCacheSim">false</value>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableEventToolTips">true</value>
- <value type="double" key="Analyzer.Valgrind.Callgrind.MinimumCostRatio">0.01</value>
- <value type="double" key="Analyzer.Valgrind.Callgrind.VisualisationMinimumCostRatio">10</value>
- <value type="bool" key="Analyzer.Valgrind.FilterExternalIssues">true</value>
- <value type="int" key="Analyzer.Valgrind.LeakCheckOnFinish">1</value>
- <value type="int" key="Analyzer.Valgrind.NumCallers">25</value>
- <valuelist type="QVariantList" key="Analyzer.Valgrind.RemovedSuppressionFiles"/>
- <value type="int" key="Analyzer.Valgrind.SelfModifyingCodeDetection">1</value>
- <value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
- <value type="bool" key="Analyzer.Valgrind.ShowReachable">false</value>
- <value type="bool" key="Analyzer.Valgrind.TrackOrigins">true</value>
- <value type="QString" key="Analyzer.Valgrind.ValgrindExecutable">valgrind</value>
- <valuelist type="QVariantList" key="Analyzer.Valgrind.VisibleErrorKinds">
- <value type="int">0</value>
- <value type="int">1</value>
- <value type="int">2</value>
- <value type="int">3</value>
- <value type="int">4</value>
- <value type="int">5</value>
- <value type="int">6</value>
- <value type="int">7</value>
- <value type="int">8</value>
- <value type="int">9</value>
- <value type="int">10</value>
- <value type="int">11</value>
- <value type="int">12</value>
- <value type="int">13</value>
- <value type="int">14</value>
- </valuelist>
- <value type="int" key="PE.EnvironmentAspect.Base">2</value>
- <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">pcl_visualizer</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4RunConfiguration:/home/victor/Copy/qt_visualizer/src/pcl_visualizer.pro</value>
- <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.CommandLineArguments"></value>
- <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.ProFile">pcl_visualizer.pro</value>
- <value type="bool" key="Qt4ProjectManager.Qt4RunConfiguration.UseDyldImageSuffix">false</value>
- <value type="bool" key="Qt4ProjectManager.Qt4RunConfiguration.UseTerminal">false</value>
- <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.UserWorkingDirectory"></value>
- <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
- <value type="bool" key="RunConfiguration.UseCppDebugger">true</value>
- <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">false</value>
- <value type="bool" key="RunConfiguration.UseMultiProcess">false</value>
- <value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>
- <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
- </valuemap>
- </data>
- <data>
- <variable>ProjectExplorer.Project.TargetCount</variable>
- <value type="int">1</value>
- </data>
- <data>
- <variable>ProjectExplorer.Project.Updater.EnvironmentId</variable>
- <value type="QByteArray">{8d8c9016-62ac-401a-82aa-bc1e1c77c433}</value>
- </data>
- <data>
- <variable>ProjectExplorer.Project.Updater.FileVersion</variable>
- <value type="int">15</value>
- </data>
-</qtcreator>
#include <pcl/io/pcd_io.h>
#include <pcl/console/time.h>
-#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/conversions.h>
-#include <pcl/keypoints/uniform_sampling.h>
+#include <pcl/filters/uniform_sampling.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/registration/correspondence_estimation.h>
PointCloud<PointXYZ> &keypoints_src,
PointCloud<PointXYZ> &keypoints_tgt)
{
- PointCloud<int> keypoints_src_idx, keypoints_tgt_idx;
// Get an uniform grid of keypoints
UniformSampling<PointXYZ> uniform;
uniform.setRadiusSearch (1); // 1m
uniform.setInputCloud (src);
- uniform.compute (keypoints_src_idx);
- copyPointCloud<PointXYZ, PointXYZ> (*src, keypoints_src_idx.points, keypoints_src);
+ uniform.filter (keypoints_src);
uniform.setInputCloud (tgt);
- uniform.compute (keypoints_tgt_idx);
- copyPointCloud<PointXYZ, PointXYZ> (*tgt, keypoints_tgt_idx.points, keypoints_tgt);
+ uniform.filter (keypoints_tgt);
// For debugging purposes only: uncomment the lines below and use pcl_viewer to view the results, i.e.:
// pcl_viewer source_pcd keypoints_src.pcd -ps 1 -ps 10
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("z", pcl::ComparisonOps::LT, 0.8)));
// build the filter
- pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond);
+ pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
+ condrem.setCondition (range_cond);
condrem.setInputCloud (cloud);
condrem.setKeepOrganized(true);
// apply filter
project(supervoxel_clustering)
-find_package(PCL 1.7 REQUIRED)
+find_package(PCL 1.8 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/segmentation/supervoxel_clustering.h>
+//VTK include needed for drawing graph lines
+#include <vtkPolyLine.h>
+
// Types
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
}
- PointCloudT::Ptr cloud = boost::make_shared <PointCloudT> ();
+ PointCloudT::Ptr cloud = boost::shared_ptr <PointCloudT> (new PointCloudT ());
pcl::console::print_highlight ("Loading point cloud...\n");
if (pcl::io::loadPCDFile<PointT> (argv[1], *cloud))
{
}
- bool use_transform = ! pcl::console::find_switch (argc, argv, "--NT");
+ bool disable_transform = pcl::console::find_switch (argc, argv, "--NT");
float voxel_resolution = 0.008f;
bool voxel_res_specified = pcl::console::find_switch (argc, argv, "-v");
////// This is how to use supervoxels
////////////////////////////// //////////////////////////////
- pcl::SupervoxelClustering<PointT> super (voxel_resolution, seed_resolution, use_transform);
+ pcl::SupervoxelClustering<PointT> super (voxel_resolution, seed_resolution);
+ if (disable_transform)
+ super.setUseSingleCameraTransform (false);
super.setInputCloud (cloud);
super.setColorImportance (color_importance);
super.setSpatialImportance (spatial_importance);
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2.0, "voxel centroids");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.95, "voxel centroids");
- PointCloudT::Ptr colored_voxel_cloud = super.getColoredVoxelCloud ();
- viewer->addPointCloud (colored_voxel_cloud, "colored voxels");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.8, "colored voxels");
+ PointLCloudT::Ptr labeled_voxel_cloud = super.getLabeledVoxelCloud ();
+ viewer->addPointCloud (labeled_voxel_cloud, "labeled voxels");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.8, "labeled voxels");
PointNCloudT::Ptr sv_normal_cloud = super.makeSupervoxelNormalCloud (supervoxel_clusters);
//We have this disabled so graph is easy to see, uncomment to see supervoxel normals
if ((int)cloud.width * cloud.height != 1)
return (false);
}
- catch (pcl::InvalidConversionException e)
+ catch (const pcl::InvalidConversionException&)
{
return (false);
}
if ((int)cloud.width * cloud.height != 1)
return (false);
}
- catch (pcl::InvalidConversionException e)
+ catch (const pcl::InvalidConversionException&)
{
return (false);
}
.. literalinclude:: sources/supervoxel_clustering/supervoxel_clustering.cpp
:language: cpp
- :lines: 9-14
+ :lines: 12-17
Then we load the input cloud based on the input argument
.. literalinclude:: sources/supervoxel_clustering/supervoxel_clustering.cpp
:language: cpp
- :lines: 36-42
+ :lines: 39-45
Next we check the input arguments and set default values. You can play with the various parameters to see how they affect the supervoxels, but briefly:
-- ``--NT`` Disables the single-view transform (this is necessary if you are loading a cloud constructed from more than one viewpoint)
+- ``--NT`` Disables the single-view transform (this is the default for unorganized clouds, only affects organized clouds)
- ``-v`` Sets the voxel size, which determines the leaf size of the underlying octree structure (in meters)
- ``-s`` Sets the seeding size, which determines how big the supervoxels will be (in meters)
- ``-c`` Sets the weight for color - how much color will influence the shape of the supervoxels
.. literalinclude:: sources/supervoxel_clustering/supervoxel_clustering.cpp
:language: cpp
- :lines: 45-67
+ :lines: 48-70
We are now ready to setup the supervoxel clustering. We use the class :pcl:`SupervoxelClustering <pcl::SupervoxelClustering>`, which implements the clustering process and give it the parameters.
.. important::
- You MUST set use_transform to false if you are using a cloud which doesn't have the camera at (0,0,0). The transform is specifically designed to help improve Kinect data by increasing voxel bin size as distance from the camera increases. If your data is artificial, made from combining multiple clouds from cameras at different viewpoints, or doesn't have the camera at (0,0,0), the transform MUST be set to false.
+ By default, the algorithm will use a special tranform compressing the depth in Z if your input cloud is organized (eg, from an RGBD sensor like the Kinect). You MUST set use_transform to false if you are using an organized cloud which doesn't have the camera at (0,0,0) and depth in positive Z. The transform is specifically designed to help improve Kinect data by increasing voxel bin size as distance from the camera increases. If your cloud is unorganized, this transform will not be used by default, but can be enabled by using setUseSingleCameraTransform(true).
.. literalinclude:: sources/supervoxel_clustering/supervoxel_clustering.cpp
:language: cpp
- :lines: 73-77
+ :lines: 76-82
Then we initialize the data structure which will be used to extract the supervoxels, and run the algorithm. The data structure is a map from labels to shared pointers of :pcl:`Supervoxel <pcl::Supervoxel>` templated on the input point type. Supervoxels have the following fields:
.. literalinclude:: sources/supervoxel_clustering/supervoxel_clustering.cpp
:language: cpp
- :lines: 79-83
+ :lines: 84-88
We then load a viewer and use some of the getter functions of :pcl:`SupervoxelClustering <pcl::SupervoxelClustering>` to pull out clouds to display. ``voxel_centroid_cloud`` contains the voxel centroids coming out of the octree (basically the downsampled original cloud), and ``colored_voxel_cloud`` are the voxels colored according to their supervoxel labels (random colors). ``sv_normal_cloud`` contains a cloud of the supervoxel normals, but we don't display it here so that the graph is visible.
.. literalinclude:: sources/supervoxel_clustering/supervoxel_clustering.cpp
:language: cpp
- :lines: 85-99
+ :lines: 90-104
Finally, we extract the supervoxel adjacency list (in the form of a multimap of label adjacencies).
.. literalinclude:: sources/supervoxel_clustering/supervoxel_clustering.cpp
:language: cpp
- :lines: 101-103
+ :lines: 106-108
Then we iterate through the multimap, creating a point cloud of the centroids of each supervoxel's neighbors.
.. literalinclude:: sources/supervoxel_clustering/supervoxel_clustering.cpp
:language: cpp
- :lines: 105-120
+ :lines: 110-125
Then we create a string label for the supervoxel graph we will draw and call ``addSupervoxelConnectionsToViewer``, a drawing helper function implemented later in the tutorial code. The details of ``addSupervoxelConnectionsToViewer`` are beyond the scope of this tutorial, but all it does is draw a star polygon mesh of the supervoxel centroid to all of its neighbors centroids. We need to do this like this because adding individual lines using the ``addLine`` functionality of ``pcl_visualizer`` is too slow for large numbers of lines.
.. literalinclude:: sources/supervoxel_clustering/supervoxel_clustering.cpp
:language: cpp
- :lines: 121-127
+ :lines: 126-132
This results in a supervoxel graph that looks like this for seed size of 0.1m (top) and 0.05m (middle). The bottom is the original cloud, given for reference.:
After you have made the executable, you can run it like so, assuming the pcd file is in the same folder as the executable::
- $ ./supervoxel_clustering milk_cartoon_all_small_clorox.pcd
+ $ ./supervoxel_clustering milk_cartoon_all_small_clorox.pcd --NT
Don't be afraid to play around with the parameters (especially the seed size, -s) to see what happens. The pcd file name should always be the first parameter!
.. math::
- L_j = L_distance ( \times L_color )
+ L_j = L_{distance} ( \times L_{color} )
- w = \sum_ L_j
+ w = \sum{}^{} L_j
More Advanced
-------------
-If you want to see more flexible and useful tracking code which starts tracking without preparing to make segemented model beforehand, you should refer a tracking code https://github.com/aginika/pcl/blob/master/apps/src/openni_tracking.cpp. It will show you better and more legible code. The above Figures are windows when you implement that code.
+If you want to see more flexible and useful tracking code which starts tracking without preparing to make segemented model beforehand, you should refer a tracking code https://github.com/PointCloudLibrary/pcl/blob/master/apps/src/openni_tracking.cpp. It will show you better and more legible code. The above Figures are windows when you implement that code.
.. image:: images/pcl_with_eclipse/lrun_obj.gif
:height: 16
+The Eclipse console doesn't manage ANSI colours, you could use `an ANSI console plugin <http://www.mihai-nita.net/eclipse/>`_ to get rid of the "[0m" characters in the output.
+
Where to get more information
=============================
In the case of the search method, we can either do the same, or be clever and
provide a default option for the user. The best default options are:
- * use an organized search method via :pcl:`pcl::OrganizedDataIndex<pcl::OrganizedDataIndex>` if the point cloud is organized;
+ * use an organized search method via :pcl:`pcl::OrganizedNeighbor<pcl::OrganizedNeighbor>` if the point cloud is organized;
* use a general purpose kdtree via :pcl:`pcl::KdTreeFLANN<pcl::KdTreeFLANN>` if the point cloud is unorganized.
.. code-block:: cpp
if (!tree_)
{
if (input_->isOrganized ())
- tree_.reset (new pcl::Organized
DataIndex<PointT> ());
+ tree_.reset (new pcl::Organized
Neighbor<PointT> ());
else
tree_.reset (new pcl::KdTreeFLANN<PointT> (false));
}
if (!tree_)
{
if (input_->isOrganized ())
- tree_.reset (new pcl::Organized
DataIndex<PointT> ());
+ tree_.reset (new pcl::Organized
Neighbor<PointT> ());
else
tree_.reset (new pcl::KdTreeFLANN<PointT> (false));
}
if (!tree_)
{
if (input_->isOrganized ())
- tree_.reset (new pcl::Organized
DataIndex<PointT> ());
+ tree_.reset (new pcl::Organized
Neighbor<PointT> ());
else
tree_.reset (new pcl::KdTreeFLANN<PointT> (false));
}
if (!tree_)
{
if (input_->isOrganized ())
- tree_.reset (new pcl::Organized
DataIndex<PointT> ());
+ tree_.reset (new pcl::Organized
Neighbor<PointT> ());
else
tree_.reset (new pcl::KdTreeFLANN<PointT> (false));
}
// In case a search method has not been given, initialize it using some defaults
if (!tree_)
{
- // For organized datasets, use an OrganizedDataIndex
+ // For organized datasets, use an OrganizedNeighbor
if (input_->isOrganized ())
- tree_.reset (new pcl::Organized
DataIndex<PointT> ());
+ tree_.reset (new pcl::Organized
Neighbor<PointT> ());
// For unorganized data, use a FLANN kdtree
else
tree_.reset (new pcl::KdTreeFLANN<PointT> (false));
set(SUBSYS_NAME examples)
set(SUBSYS_DESC "PCL examples")
-set(SUBSYS_DEPS common io features search kdtree octree filters keypoints segmentation sample_consensus outofcore geometry)
+set(SUBSYS_DEPS common io features search kdtree octree filters keypoints segmentation sample_consensus outofcore stereo geometry)
set(DEFAULT FALSE)
set(REASON "Code examples are disabled by default.")
#include <pcl/features/don.h>
+#ifdef PCL_ONLY_CORE_POINT_TYPES
+#include <pcl/features/impl/normal_3d_omp.hpp>
+#include <pcl/segmentation/impl/extract_clusters.hpp>
+#endif
+
using namespace pcl;
using namespace std;
int main (int argc, char *argv[])
{
- ///The smallest scale to use in the DoN filter.
- double scale1 = 0.2;
+ ///The smallest scale to use in the DoN filter.
+ double scale1 = 0.2;
- ///The largest scale to use in the DoN filter.
- double scale2 = 2.0;
+ ///The largest scale to use in the DoN filter.
+ double scale2 = 2.0;
- ///The minimum DoN magnitude to threshold by
- double threshold = 0.25;
+ ///The minimum DoN magnitude to threshold by
+ double threshold = 0.25;
- ///segment scene into clusters with given distance tolerance using euclidean clustering
- double segradius = 0.2;
+ ///segment scene into clusters with given distance tolerance using euclidean clustering
+ double segradius = 0.2;
- //voxelization factor of pointcloud to use in approximation of normals
- bool approx = false;
- double decimation = 100;
+ //voxelization factor of pointcloud to use in approximation of normals
+ bool approx = false;
+ double decimation = 100;
- if(argc < 2){
- cerr << "Expected 2 arguments: inputfile outputfile" << endl;
- }
+ if(argc < 2){
+ cerr << "Expected 2 arguments: inputfile outputfile" << endl;
+ }
- ///The file to read from.
- string infile = argv[1];
+ ///The file to read from.
+ string infile = argv[1];
- ///The file to output to.
- string outfile = argv[2];
+ ///The file to output to.
+ string outfile = argv[2];
- // Load cloud in blob format
- pcl::PCLPointCloud2 blob;
- pcl::io::loadPCDFile (infile.c_str(), blob);
+ // Load cloud in blob format
+ pcl::PCLPointCloud2 blob;
+ pcl::io::loadPCDFile (infile.c_str(), blob);
-
pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
+ pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
cout << "Loading point cloud...";
pcl::fromPCLPointCloud2 (blob, *cloud);
cout << "done." << endl;
- SearchPtr tree;
+ SearchPtr tree;
- if (cloud->isOrganized ())
- {
-
tree.reset (new pcl::search::OrganizedNeighbor<PointT> ());
- }
- else
- {
+ if (cloud->isOrganized ())
+ {
+ tree.reset (new pcl::search::OrganizedNeighbor<PointT> ());
+ }
+ else
+ {
tree.reset (new pcl::search::KdTree<PointT> (false));
- }
-
- tree->setInputCloud (cloud);
-
-
PointCloud<PointT>::Ptr small_cloud_downsampled;
-
PointCloud<PointT>::Ptr large_cloud_downsampled;
-
- // If we are using approximation
- if(approx){
- cout << "Downsampling point cloud for approximation" << endl;
-
- // Create the downsampling filtering object
-
pcl::VoxelGrid<PointT> sor;
- sor.setDownsampleAllData (false);
- sor.setInputCloud (cloud);
-
- // Create downsampled point cloud for DoN NN search with small scale
-
small_cloud_downsampled = PointCloud<PointT>::Ptr(new pcl::PointCloud<PointT>);
- float smalldownsample = static_cast<float> (scale1 / decimation);
- sor.setLeafSize (smalldownsample, smalldownsample, smalldownsample);
- sor.filter (*small_cloud_downsampled);
- cout << "Using leaf size of " << smalldownsample << " for small scale, " << small_cloud_downsampled->size() << " points" << endl;
-
- // Create downsampled point cloud for DoN NN search with large scale
-
large_cloud_downsampled = PointCloud<PointT>::Ptr(new pcl::PointCloud<PointT>);
- const float largedownsample = float (scale2/decimation);
- sor.setLeafSize (largedownsample, largedownsample, largedownsample);
- sor.filter (*large_cloud_downsampled);
- cout << "Using leaf size of " << largedownsample << " for large scale, " << large_cloud_downsampled->size() << " points" << endl;
- }
-
- // Compute normals using both small and large scales at each point
-
pcl::NormalEstimationOMP<PointT, PointNT> ne;
- ne.setInputCloud (cloud);
+ }
+
+ tree->setInputCloud (cloud);
+
+ PointCloud<PointT>::Ptr small_cloud_downsampled;
+ PointCloud<PointT>::Ptr large_cloud_downsampled;
+
+ // If we are using approximation
+ if(approx){
+ cout << "Downsampling point cloud for approximation" << endl;
+
+ // Create the downsampling filtering object
+ pcl::VoxelGrid<PointT> sor;
+ sor.setDownsampleAllData (false);
+ sor.setInputCloud (cloud);
+
+ // Create downsampled point cloud for DoN NN search with small scale
+ small_cloud_downsampled = PointCloud<PointT>::Ptr(new pcl::PointCloud<PointT>);
+ float smalldownsample = static_cast<float> (scale1 / decimation);
+ sor.setLeafSize (smalldownsample, smalldownsample, smalldownsample);
+ sor.filter (*small_cloud_downsampled);
+ cout << "Using leaf size of " << smalldownsample << " for small scale, " << small_cloud_downsampled->size() << " points" << endl;
+
+ // Create downsampled point cloud for DoN NN search with large scale
+ large_cloud_downsampled = PointCloud<PointT>::Ptr(new pcl::PointCloud<PointT>);
+ const float largedownsample = float (scale2/decimation);
+ sor.setLeafSize (largedownsample, largedownsample, largedownsample);
+ sor.filter (*large_cloud_downsampled);
+ cout << "Using leaf size of " << largedownsample << " for large scale, " << large_cloud_downsampled->size() << " points" << endl;
+ }
+
+ // Compute normals using both small and large scales at each point
+ pcl::NormalEstimationOMP<PointT, PointNT> ne;
+ ne.setInputCloud (cloud);
ne.setSearchMethod (tree);
- /**
- * NOTE: setting viewpoint is very important, so that we can ensure
- * normals are all pointed in the same direction!
- */
- ne.setViewPoint(std::numeric_limits<float>::max(),std::numeric_limits<float>::max(),std::numeric_limits<float>::max());
+ /**
+ * NOTE: setting viewpoint is very important, so that we can ensure
+ * normals are all pointed in the same direction!
+ */
+ ne.setViewPoint(std::numeric_limits<float>::max(),std::numeric_limits<float>::max(),std::numeric_limits<float>::max());
- if(scale1 >= scale2){
- cerr << "Error: Large scale must be > small scale!" << endl;
- exit(EXIT_FAILURE);
- }
+ if(scale1 >= scale2){
+ cerr << "Error: Large scale must be > small scale!" << endl;
+ exit(EXIT_FAILURE);
+ }
- //the normals calculated with the small scale
- cout << "Calculating normals for scale..." << scale1 << endl;
-
pcl::PointCloud<PointNT>::Ptr normals_small_scale (new pcl::PointCloud<PointNT>);
+ //the normals calculated with the small scale
+ cout << "Calculating normals for scale..." << scale1 << endl;
+ pcl::PointCloud<PointNT>::Ptr normals_small_scale (new pcl::PointCloud<PointNT>);
- if(approx){
- ne.setSearchSurface(small_cloud_downsampled);
+ if(approx){
+ ne.setSearchSurface(small_cloud_downsampled);
}
- ne.setRadiusSearch (scale1);
- ne.compute (*normals_small_scale);
+ ne.setRadiusSearch (scale1);
+ ne.compute (*normals_small_scale);
- cout << "Calculating normals for scale..." << scale2 << endl;
- //the normals calculated with the large scale
-
pcl::PointCloud<PointNT>::Ptr normals_large_scale (new pcl::PointCloud<PointNT>);
+ cout << "Calculating normals for scale..." << scale2 << endl;
+ //the normals calculated with the large scale
+ pcl::PointCloud<PointNT>::Ptr normals_large_scale (new pcl::PointCloud<PointNT>);
- if(approx){
- ne.setSearchSurface(large_cloud_downsampled);
- }
- ne.setRadiusSearch (scale2);
- ne.compute (*normals_large_scale);
+ if(approx){
+ ne.setSearchSurface(large_cloud_downsampled);
+ }
+ ne.setRadiusSearch (scale2);
+ ne.compute (*normals_large_scale);
- // Create output cloud for DoN results
-
PointCloud<PointOutT>::Ptr doncloud (new pcl::PointCloud<PointOutT>);
-
copyPointCloud<PointT, PointOutT>(*cloud, *doncloud);
+ // Create output cloud for DoN results
+ PointCloud<PointOutT>::Ptr doncloud (new pcl::PointCloud<PointOutT>);
+ copyPointCloud<PointT, PointOutT>(*cloud, *doncloud);
- cout << "Calculating DoN... " << endl;
- // Create DoN operator
-
pcl::DifferenceOfNormalsEstimation<PointT, PointNT, PointOutT> don;
- don.setInputCloud (cloud);
- don.setNormalScaleLarge(normals_large_scale);
- don.setNormalScaleSmall(normals_small_scale);
+ cout << "Calculating DoN... " << endl;
+ // Create DoN operator
+ pcl::DifferenceOfNormalsEstimation<PointT, PointNT, PointOutT> don;
+ don.setInputCloud (cloud);
+ don.setNormalScaleLarge(normals_large_scale);
+ don.setNormalScaleSmall(normals_small_scale);
- if(!don.initCompute ()){
- std::cerr << "Error: Could not intialize DoN feature operator" << std::endl;
- exit(EXIT_FAILURE);
- }
+ if(!don.initCompute ()){
+ std::cerr << "Error: Could not intialize DoN feature operator" << std::endl;
+ exit(EXIT_FAILURE);
+ }
- //Compute DoN
- don.computeFeature(*doncloud);
+ //Compute DoN
+ don.computeFeature(*doncloud);
pcl::PCDWriter writer;
// Save DoN features
writer.write<PointOutT> (outfile.c_str (), *doncloud, false);
- //Filter by magnitude
- cout << "Filtering out DoN mag <= "<< threshold << "..." << endl;
-
- // build the condition
- pcl::ConditionOr<PointOutT>::Ptr range_cond (new
- pcl::ConditionOr<PointOutT> ());
- range_cond->addComparison (pcl::FieldComparison<PointOutT>::ConstPtr (new
- pcl::FieldComparison<PointOutT> ("curvature", pcl::ComparisonOps::GT, threshold)));
- // build the filter
- pcl::ConditionalRemoval<PointOutT> condrem (range_cond);
- condrem.setInputCloud (doncloud);
-
- pcl::PointCloud<PointOutT>::Ptr doncloud_filtered (new pcl::PointCloud<PointOutT>);
-
- // apply filter
- condrem.filter (*doncloud_filtered);
-
- doncloud = doncloud_filtered;
-
- // Save filtered output
- std::cout << "Filtered Pointcloud: " << doncloud->points.size () << " data points." << std::endl;
- std::stringstream ss;
- ss << outfile.substr(0,outfile.length()-4) << "_threshold_"<< threshold << "_.pcd";
- writer.write<PointOutT> (ss.str (), *doncloud, false);
-
- //Filter by magnitude
- cout << "Clustering using EuclideanClusterExtraction with tolerance <= "<< segradius << "..." << endl;
-
- pcl::search::KdTree<PointOutT>::Ptr segtree (new pcl::search::KdTree<PointOutT>);
- segtree->setInputCloud (doncloud);
-
- std::vector<pcl::PointIndices> cluster_indices;
- pcl::EuclideanClusterExtraction<PointOutT> ec;
-
- ec.setClusterTolerance (segradius);
- ec.setMinClusterSize (50);
- ec.setMaxClusterSize (100000);
- ec.setSearchMethod (segtree);
- ec.setInputCloud (doncloud);
- ec.extract (cluster_indices);
-
- int j = 0;
- for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it, j++)
- {
- pcl::PointCloud<PointOutT>::Ptr cloud_cluster_don (new pcl::PointCloud<PointOutT>);
- for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++){
- cloud_cluster_don->points.push_back (doncloud->points[*pit]);
- }
-
- cloud_cluster_don->width = int (cloud_cluster_don->points.size ());
- cloud_cluster_don->height = 1;
- cloud_cluster_don->is_dense = true;
-
- std::cout << "PointCloud representing the Cluster: " << cloud_cluster_don->points.size () << " data points." << std::endl;
- std::stringstream ss;
- ss << outfile.substr(0,outfile.length()-4) << "_threshold_"<< threshold << "_cluster_" << j << ".pcd";
- writer.write<PointOutT> (ss.str (), *cloud_cluster_don, false);
- }
+ //Filter by magnitude
+ cout << "Filtering out DoN mag <= "<< threshold << "..." << endl;
+
+ // build the condition
+ pcl::ConditionOr<PointOutT>::Ptr range_cond (new
+ pcl::ConditionOr<PointOutT> ());
+ range_cond->addComparison (pcl::FieldComparison<PointOutT>::ConstPtr (new
+ pcl::FieldComparison<PointOutT> ("curvature", pcl::ComparisonOps::GT, threshold)));
+ // build the filter
+ pcl::ConditionalRemoval<PointOutT> condrem;
+ condrem.setCondition (range_cond);
+ condrem.setInputCloud (doncloud);
+
+ pcl::PointCloud<PointOutT>::Ptr doncloud_filtered (new pcl::PointCloud<PointOutT>);
+
+ // apply filter
+ condrem.filter (*doncloud_filtered);
+
+ doncloud = doncloud_filtered;
+
+ // Save filtered output
+ std::cout << "Filtered Pointcloud: " << doncloud->points.size () << " data points." << std::endl;
+ std::stringstream ss;
+ ss << outfile.substr(0,outfile.length()-4) << "_threshold_"<< threshold << "_.pcd";
+ writer.write<PointOutT> (ss.str (), *doncloud, false);
+
+ //Filter by magnitude
+ cout << "Clustering using EuclideanClusterExtraction with tolerance <= "<< segradius << "..." << endl;
+
+ pcl::search::KdTree<PointOutT>::Ptr segtree (new pcl::search::KdTree<PointOutT>);
+ segtree->setInputCloud (doncloud);
+
+ std::vector<pcl::PointIndices> cluster_indices;
+ pcl::EuclideanClusterExtraction<PointOutT> ec;
+
+ ec.setClusterTolerance (segradius);
+ ec.setMinClusterSize (50);
+ ec.setMaxClusterSize (100000);
+ ec.setSearchMethod (segtree);
+ ec.setInputCloud (doncloud);
+ ec.extract (cluster_indices);
+
+ int j = 0;
+ for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it, j++)
+ {
+ pcl::PointCloud<PointOutT>::Ptr cloud_cluster_don (new pcl::PointCloud<PointOutT>);
+ for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit){
+ cloud_cluster_don->points.push_back (doncloud->points[*pit]);
+ }
+
+ cloud_cluster_don->width = int (cloud_cluster_don->points.size ());
+ cloud_cluster_don->height = 1;
+ cloud_cluster_don->is_dense = true;
+
+ std::cout << "PointCloud representing the Cluster: " << cloud_cluster_don->points.size () << " data points." << std::endl;
+ std::stringstream ss;
+ ss << outfile.substr(0,outfile.length()-4) << "_threshold_"<< threshold << "_cluster_" << j << ".pcd";
+ writer.write<PointOutT> (ss.str (), *cloud_cluster_don, false);
+ }
}
+
include (${VTK_USE_FILE})
PCL_ADD_EXAMPLE(pcl_example_supervoxels FILES example_supervoxels.cpp
LINK_WITH pcl_common pcl_features pcl_segmentation pcl_octree pcl_kdtree pcl_visualization)
+ PCL_ADD_EXAMPLE(pcl_example_lccp_segmentation FILES example_lccp_segmentation.cpp
+ LINK_WITH pcl_common pcl_io pcl_segmentation pcl_visualization)
+ PCL_ADD_EXAMPLE(pcl_example_cpc_segmentation FILES example_cpc_segmentation.cpp
+ LINK_WITH pcl_common pcl_io pcl_segmentation pcl_visualization pcl_sample_consensus)
endif(VTK_FOUND)
endif(BUILD_visualization)
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+// Stdlib
+#include <stdlib.h>
+#include <cmath>
+#include <limits.h>
+
+#include <boost/format.hpp>
+#include <boost/filesystem.hpp>
+
+// PCL input/output
+#include <pcl/console/parse.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/visualization/point_cloud_color_handlers.h>
+
+//PCL other
+#include <pcl/filters/passthrough.h>
+#include <pcl/segmentation/supervoxel_clustering.h>
+
+// The segmentation class this example is for
+#include <pcl/segmentation/cpc_segmentation.h>
+
+// VTK
+#include <vtkImageReader2Factory.h>
+#include <vtkImageReader2.h>
+#include <vtkImageData.h>
+#include <vtkImageFlip.h>
+#include <vtkPolyLine.h>
+
+/// ***** Type Definitions ***** ///
+
+typedef pcl::PointXYZRGBA PointT; // The point type used for input
+typedef pcl::LCCPSegmentation<PointT>::SupervoxelAdjacencyList SuperVoxelAdjacencyList;
+
+/// Callback and variables
+
+bool show_normals = false, normals_changed = false;
+bool show_adjacency = false, line_changed = false;
+bool show_supervoxels = false;
+bool show_segmentation = true;
+bool show_help = true;
+bool bg_white = false;
+float normals_scale;
+float line_width = 2.0f;
+float textcolor;
+
+/** \brief Callback for setting options in the visualizer via keyboard.
+ * \param[in] event_arg Registered keyboard event */
+void
+keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event_arg,
+ void*)
+{
+ int key = event_arg.getKeyCode ();
+
+ if (event_arg.keyUp ())
+ switch (key)
+ {
+ case (int) '1':
+ show_normals = !show_normals;
+ normals_changed = true;
+ break;
+ case (int) '2':
+ show_adjacency = !show_adjacency;
+ break;
+ case (int) '3':
+ show_supervoxels = !show_supervoxels;
+ break;
+ case (int) '4':
+ show_segmentation = !show_segmentation;
+ break;
+ case (int) '5':
+ normals_scale *= 1.25;
+ normals_changed = true;
+ break;
+ case (int) '6':
+ normals_scale *= 0.8;
+ normals_changed = true;
+ break;
+ case (int) '7':
+ line_width += 0.5;
+ line_changed = true;
+ break;
+ case (int) '8':
+ if (line_width <= 1)
+ break;
+ line_width -= 0.5;
+ line_changed = true;
+ break;
+ case (int) 'd':
+ case (int) 'D':
+ show_help = !show_help;
+ break;
+ default:
+ break;
+ }
+}
+
+/// ***** Prototypes helper functions***** ///
+
+/** \brief Displays info text in the specified PCLVisualizer
+ * \param[in] viewer_arg The PCLVisualizer to modify */
+void
+printText (boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_arg);
+
+/** \brief Removes info text in the specified PCLVisualizer
+ * \param[in] viewer_arg The PCLVisualizer to modify */
+void
+removeText (boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_arg);
+
+/// ---- main ---- ///
+int
+main (int argc,
+ char ** argv)
+{
+ if (argc < 2) /// Print Info
+ {
+ pcl::console::print_info (
+\
+ "\n\
+-- pcl::CPCSegmentation example -- :\n\
+\n\
+Syntax: %s input.pcd [Options] \n\
+\n\
+Output:\n\
+ -o <outname> \n\
+ Write segmented point cloud to disk (Type XYZL). If this option is specified without giving a name, the <outputname> defaults to <inputfilename>_out.pcd.\n\
+ The content of the file can be changed with the -add and -bin flags\n\
+ -novis Disable visualization\n\
+Output options:\n\
+ -add Instead of XYZL, append a label field to the input point cloud which holds the segmentation results (<input_cloud_type>+L)\n\
+ If a label field already exists in the input point cloud it will be overwritten by the segmentation\n\
+ -bin Save a binary pcd-file instead of an ascii file \n\
+ -so Additionally write the colored supervoxel image to <outfilename>_svcloud.pcd\n\
+ -white Use white background instead of black \n\
+ \n\
+Supervoxel Parameters: \n\
+ -v <voxel resolution> (default 0.0075) \n\
+ -s <seed resolution> (default 0.03)\n\
+ -c <color weight> (default 0)\n\
+ -z <spatial weight> (default 1)\n\
+ -n <normal_weight> (default 4)\n\
+ -tvoxel - Use single-camera-transform for voxels (Depth-Dependent-Voxel-Grid)\n\
+ -refine - Use supervoxel refinement\n\
+ -nonormals - Ignore the normals from the input pcd file\n\
+ \n\
+LCCPSegmentation Parameters: \n\
+ -ct <concavity tolerance angle> - Angle threshold in degrees for concave edges to be treated as convex (default 10) \n\
+ -st <smoothness threshold> - Invalidate steps. Value from the interval [0,1], where 0 is the strictest and 1 equals 'no smoothness check' (default 0.1)\n\
+ -ec - Use extended (less local) convexity check\n\
+ -sc - Use sanity criterion to invalidate singular connected patches\n\
+ -smooth <mininmal segment size> - Merge small segments which have fewer points than minimal segment size (default 0)\n\
+ \n\
+CPCSegmentation Parameters: \n\
+ -cut <max_cuts>,<cutting_min_segments>,<min_cut_score> - Plane cutting parameters for splitting of segments\n\
+ <max_cuts> - Perform cuts up to this recursion level. Cuts are performed in each segment separately (default 25)\n\
+ <cutting_min_segments> - Minumum number of supervoxels in the segment to perform cutting (default 400).\n\
+ <min_cut_score> - Minumum score a proposed cut needs to have for being cut (default 0.16)\n\
+ -clocal - Use locally constrained cuts (recommended flag)\n\
+ -cdir - Use directed weigths (recommended flag) \n\
+ -cclean - Use clean cuts. \n\
+ Flag set: Only split edges with supervoxels on opposite sites of the cutting-plane \n\
+ Flag not set: Split all edges whose centroid is within the seed resolution distance to the cutting-plane\n\
+ -citer <num_interations> - Sets the maximum number of iterations for the RANSAC algorithm (default 10000) \n\
+ \n",
+ argv[0]);
+ return (1);
+ }
+
+ /// -----------------------------------| Preparations |-----------------------------------
+
+ bool sv_output_specified = pcl::console::find_switch (argc, argv, "-so");
+ bool show_visualization = (!pcl::console::find_switch (argc, argv, "-novis"));
+ bool ignore_provided_normals = pcl::console::find_switch (argc, argv, "-nonormals");
+ bool add_label_field = pcl::console::find_switch (argc, argv, "-add");
+ bool save_binary_pcd = pcl::console::find_switch (argc, argv, "-bin");
+
+ /// Create variables needed for preparations
+ std::string outputname ("");
+ pcl::PointCloud<PointT>::Ptr input_cloud_ptr (new pcl::PointCloud<PointT>);
+ pcl::PointCloud<pcl::Normal>::Ptr input_normals_ptr (new pcl::PointCloud<pcl::Normal>);
+ bool has_normals = false;
+
+ /// Get pcd path from command line
+ std::string pcd_filename = argv[1];
+ PCL_INFO ("Loading pointcloud\n");
+
+ /// check if the provided pcd file contains normals
+ pcl::PCLPointCloud2 input_pointcloud2;
+ if (pcl::io::loadPCDFile (pcd_filename, input_pointcloud2))
+ {
+ PCL_ERROR ("ERROR: Could not read input point cloud %s.\n", pcd_filename.c_str ());
+ return (3);
+ }
+ pcl::fromPCLPointCloud2 (input_pointcloud2, *input_cloud_ptr);
+ if (!ignore_provided_normals)
+ {
+ if (pcl::getFieldIndex (input_pointcloud2, "normal_x") >= 0)
+ {
+ pcl::fromPCLPointCloud2 (input_pointcloud2, *input_normals_ptr);
+ has_normals = true;
+
+ //NOTE Supposedly there was a bug in old PCL versions that the orientation was not set correctly when recording clouds. This is just a workaround.
+ if (input_normals_ptr->sensor_orientation_.w () == 0)
+ {
+ input_normals_ptr->sensor_orientation_.w () = 1;
+ input_normals_ptr->sensor_orientation_.x () = 0;
+ input_normals_ptr->sensor_orientation_.y () = 0;
+ input_normals_ptr->sensor_orientation_.z () = 0;
+ }
+ }
+ else
+ PCL_WARN ("Could not find normals in pcd file. Normals will be calculated. This only works for single-camera-view pointclouds.\n");
+ }
+ PCL_INFO ("Done making cloud\n");
+
+ /// Create outputname if not given
+ bool output_specified = pcl::console::find_switch (argc, argv, "-o");
+ if (output_specified)
+ {
+ pcl::console::parse (argc, argv, "-o", outputname);
+
+ // If no filename is given, get output filename from inputname (strip seperators and file extension)
+ if (outputname.empty () || (outputname.at (0) == '-'))
+ {
+ outputname = pcd_filename;
+ size_t sep = outputname.find_last_of ('/');
+ if (sep != std::string::npos)
+ outputname = outputname.substr (sep + 1, outputname.size () - sep - 1);
+
+ size_t dot = outputname.find_last_of ('.');
+ if (dot != std::string::npos)
+ outputname = outputname.substr (0, dot);
+ }
+ }
+
+/// -----------------------------------| Main Computation |-----------------------------------
+
+ /// Default values of parameters before parsing
+ // Supervoxel Stuff
+ float voxel_resolution = 0.0075f;
+ float seed_resolution = 0.03f;
+ float color_importance = 0.0f;
+ float spatial_importance = 1.0f;
+ float normal_importance = 4.0f;
+ bool use_single_cam_transform ;
+ bool use_supervoxel_refinement;
+
+ // LCCPSegmentation Stuff
+ float concavity_tolerance_threshold = 10;
+ float smoothness_threshold = 0.1;
+ uint32_t min_segment_size = 0;
+ bool use_extended_convexity;
+ bool use_sanity_criterion;
+
+ // CPCSegmentation Stuff
+ float min_cut_score = 0.16;
+ unsigned int max_cuts = 25;
+ unsigned int cutting_min_segments = 400;
+ bool use_local_constrain;
+ bool use_directed_cutting;
+ bool use_clean_cutting;
+ unsigned int ransac_iterations = 10000;
+
+ /// Parse Arguments needed for computation
+ //Supervoxel Stuff
+ use_single_cam_transform = pcl::console::find_switch (argc, argv, "-tvoxel");
+ use_supervoxel_refinement = pcl::console::find_switch (argc, argv, "-refine");
+
+ pcl::console::parse (argc, argv, "-v", voxel_resolution);
+ pcl::console::parse (argc, argv, "-s", seed_resolution);
+ pcl::console::parse (argc, argv, "-c", color_importance);
+ pcl::console::parse (argc, argv, "-z", spatial_importance);
+ pcl::console::parse (argc, argv, "-n", normal_importance);
+
+ normals_scale = seed_resolution / 2.0;
+
+ // Segmentation Stuff
+ pcl::console::parse (argc, argv, "-ct", concavity_tolerance_threshold);
+ pcl::console::parse (argc, argv, "-st", smoothness_threshold);
+ use_extended_convexity = pcl::console::find_switch (argc, argv, "-ec");
+ unsigned int k_factor = 0;
+ if (use_extended_convexity)
+ k_factor = 1;
+ use_sanity_criterion = pcl::console::find_switch (argc, argv, "-sc");
+
+ if (pcl::console::find_switch (argc, argv, "-cut"))
+ {
+ std::vector<float> a;
+ pcl::console::parse_x_arguments (argc, argv, "-cut", a);
+ max_cuts = a[0];
+ if (a.size () > 1)
+ {
+ cutting_min_segments = a[1];
+ if (a.size () > 2)
+ {
+ min_cut_score = a[2];
+ }
+ }
+ }
+ else
+ {
+ PCL_WARN ("Warning you did not specify the cut argument. No cutting is being done (Fallback to LCCP preprocessing). \nUsage:\n-cut <max_cuts>,<cutting_min_segments>,<min_cut_score> optional: cdir, clocal, citer, cclean\n");
+ max_cuts = 0;
+ }
+
+ use_local_constrain = pcl::console::find_switch (argc, argv, "-clocal");
+ use_directed_cutting = pcl::console::find_switch (argc, argv, "-cdir");
+ use_clean_cutting = pcl::console::find_switch (argc, argv, "-cclean");
+ pcl::console::parse (argc, argv, "-citer",ransac_iterations);
+
+ bg_white = pcl::console::find_switch (argc, argv, "-white");
+ textcolor = bg_white?0:1;
+
+ pcl::console::print_info ("Maximum cuts: %d\n", max_cuts);
+ pcl::console::print_info ("Minumum segment siz: %d\n", cutting_min_segments);
+ pcl::console::print_info ("Use local constrain: %d\n", use_local_constrain);
+ pcl::console::print_info ("Use directed weights: %d\n", use_directed_cutting);
+ pcl::console::print_info ("Use clean cuts: %d\n", use_clean_cutting);
+ pcl::console::print_info ("RANSAC iterations: %d\n", ransac_iterations);
+
+ pcl::console::parse (argc, argv, "-smooth", min_segment_size);
+ /// Preparation of Input: Supervoxel Oversegmentation
+
+ pcl::SupervoxelClustering<PointT> super (voxel_resolution, seed_resolution);
+ super.setUseSingleCameraTransform (use_single_cam_transform);
+ super.setInputCloud (input_cloud_ptr);
+ if (has_normals)
+ super.setNormalCloud (input_normals_ptr);
+ super.setColorImportance (color_importance);
+ super.setSpatialImportance (spatial_importance);
+ super.setNormalImportance (normal_importance);
+ std::map<uint32_t, pcl::Supervoxel<PointT>::Ptr> supervoxel_clusters;
+
+ PCL_INFO ("Extracting supervoxels\n");
+ super.extract (supervoxel_clusters);
+
+ if (use_supervoxel_refinement)
+ {
+ PCL_INFO ("Refining supervoxels\n");
+ super.refineSupervoxels (2, supervoxel_clusters);
+ }
+ std::stringstream temp;
+ temp << " Nr. Supervoxels: " << supervoxel_clusters.size () << "\n";
+ PCL_INFO (temp.str ().c_str ());
+
+ PCL_INFO ("Getting supervoxel adjacency\n");
+ std::multimap<uint32_t, uint32_t>supervoxel_adjacency;
+ super.getSupervoxelAdjacency (supervoxel_adjacency);
+
+ /// Get the cloud of supervoxel centroid with normals and the colored cloud with supervoxel coloring (this is used for visulization)
+ pcl::PointCloud<pcl::PointNormal>::Ptr sv_centroid_normal_cloud = pcl::SupervoxelClustering<PointT>::makeSupervoxelNormalCloud (supervoxel_clusters);
+
+ /// Set paramters for LCCP preprocessing and CPC (CPC inherits from LCCP, thus it includes LCCP's functionality)
+
+ PCL_INFO ("Starting Segmentation\n");
+ pcl::CPCSegmentation<PointT> cpc;
+ cpc.setConcavityToleranceThreshold (concavity_tolerance_threshold);
+ cpc.setSanityCheck (use_sanity_criterion);
+ cpc.setCutting (max_cuts, cutting_min_segments, min_cut_score, use_local_constrain, use_directed_cutting, use_clean_cutting);
+ cpc.setRANSACIterations (ransac_iterations);
+ cpc.setSmoothnessCheck (true, voxel_resolution, seed_resolution, smoothness_threshold);
+ cpc.setKFactor (k_factor);
+ cpc.setInputSupervoxels (supervoxel_clusters, supervoxel_adjacency);
+ cpc.setMinSegmentSize (min_segment_size);
+ cpc.segment ();
+
+ PCL_INFO ("Interpolation voxel cloud -> input cloud and relabeling\n");
+ pcl::PointCloud<pcl::PointXYZL>::Ptr sv_labeled_cloud = super.getLabeledCloud ();
+ pcl::PointCloud<pcl::PointXYZL>::Ptr cpc_labeled_cloud = sv_labeled_cloud->makeShared ();
+ cpc.relabelCloud (*cpc_labeled_cloud);
+ SuperVoxelAdjacencyList sv_adjacency_list;
+ cpc.getSVAdjacencyList (sv_adjacency_list); // Needed for visualization
+
+ /// Creating Colored Clouds and Output
+ if (cpc_labeled_cloud->size () == input_cloud_ptr->size ())
+ {
+ if (output_specified)
+ {
+ PCL_INFO ("Saving output\n");
+ if (add_label_field)
+ {
+ if (pcl::getFieldIndex (input_pointcloud2, "label") >= 0)
+ PCL_WARN ("Input cloud already has a label field. It will be overwritten by the cpc segmentation output.\n");
+ pcl::PCLPointCloud2 output_label_cloud2, output_concat_cloud2;
+ pcl::toPCLPointCloud2 (*cpc_labeled_cloud, output_label_cloud2);
+ pcl::concatenateFields (input_pointcloud2, output_label_cloud2, output_concat_cloud2);
+ pcl::io::savePCDFile (outputname + "_out.pcd", output_concat_cloud2, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), save_binary_pcd);
+ }
+ else
+ pcl::io::savePCDFile (outputname + "_out.pcd", *cpc_labeled_cloud, save_binary_pcd);
+
+ if (sv_output_specified)
+ {
+ pcl::io::savePCDFile (outputname + "_svcloud.pcd", *sv_centroid_normal_cloud, save_binary_pcd);
+ }
+ }
+ }
+ else
+ {
+ PCL_ERROR ("ERROR:: Sizes of input cloud and labeled supervoxel cloud do not match. No output is produced.\n");
+ }
+
+ /// -----------------------------------| Visualization |-----------------------------------
+
+ if (show_visualization)
+ {
+ /// Calculate visualization of adjacency graph
+ // Using lines this would be VERY slow right now, because one actor is created for every line (may be fixed in future versions of PCL)
+ // Currently this is a work-around creating a polygon mesh consisting of two triangles for each edge
+ using namespace pcl;
+
+ typedef LCCPSegmentation<PointT>::VertexIterator VertexIterator;
+ typedef LCCPSegmentation<PointT>::AdjacencyIterator AdjacencyIterator;
+ typedef LCCPSegmentation<PointT>::EdgeID EdgeID;
+
+ std::set<EdgeID> edge_drawn;
+
+ const unsigned char black_color [3] = {0, 0, 0};
+ const unsigned char white_color [3] = {255, 255, 255};
+ const unsigned char concave_color [3] = {255, 0, 0};
+ const unsigned char cut_color [3] = { 0,255, 0};
+ const unsigned char* convex_color = bg_white ? black_color : white_color;
+ const unsigned char* color;
+
+ //The vertices in the supervoxel adjacency list are the supervoxel centroids
+ //This iterates through them, finding the edges
+ std::pair<VertexIterator, VertexIterator> vertex_iterator_range;
+ vertex_iterator_range = boost::vertices (sv_adjacency_list);
+
+ vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
+ vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New ();
+ vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+ colors->SetNumberOfComponents (3);
+ colors->SetName ("Colors");
+
+ // Create a polydata to store everything in
+ vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New ();
+ for (VertexIterator itr = vertex_iterator_range.first; itr != vertex_iterator_range.second; ++itr)
+ {
+ const uint32_t sv_label = sv_adjacency_list[*itr];
+ std::pair<AdjacencyIterator, AdjacencyIterator> neighbors = boost::adjacent_vertices (*itr, sv_adjacency_list);
+
+ for (AdjacencyIterator itr_neighbor = neighbors.first; itr_neighbor != neighbors.second; ++itr_neighbor)
+ {
+ EdgeID connecting_edge = boost::edge (*itr, *itr_neighbor, sv_adjacency_list).first; //Get the edge connecting these supervoxels
+ bool is_convex = sv_adjacency_list[connecting_edge].is_convex;
+ bool is_valid = sv_adjacency_list[connecting_edge].is_valid;
+ if (is_convex && is_valid)
+ color = convex_color;
+ else if (is_convex && !is_valid)
+ color = cut_color;
+ else if (!is_convex && !is_valid)
+ color = concave_color;
+
+ // two times since we add also two points per edge
+ colors->InsertNextTupleValue (color);
+ colors->InsertNextTupleValue (color);
+
+ pcl::Supervoxel<PointT>::Ptr supervoxel = supervoxel_clusters.at (sv_label);
+ pcl::PointXYZRGBA vert_curr = supervoxel->centroid_;
+
+ const uint32_t sv_neighbor_label = sv_adjacency_list[*itr_neighbor];
+ pcl::Supervoxel<PointT>::Ptr supervoxel_neigh = supervoxel_clusters.at (sv_neighbor_label);
+ pcl::PointXYZRGBA vert_neigh = supervoxel_neigh->centroid_;
+
+ points->InsertNextPoint (vert_curr.data);
+ points->InsertNextPoint (vert_neigh.data);
+
+ // Add the points to the dataset
+ vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New ();
+ polyLine->GetPointIds ()->SetNumberOfIds (2);
+ polyLine->GetPointIds ()->SetId (0, points->GetNumberOfPoints () - 2);
+ polyLine->GetPointIds ()->SetId (1, points->GetNumberOfPoints () - 1);
+ cells->InsertNextCell (polyLine);
+ }
+ }
+ polyData->SetPoints (points);
+ // Add the lines to the dataset
+ polyData->SetLines (cells);
+
+ polyData->GetPointData ()->SetScalars (colors);
+
+ /// END: Calculate visualization of adjacency graph
+
+ /// Configure Visualizer
+ pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
+ float bg_color = bg_white?1:0;
+ viewer->setBackgroundColor (bg_color, bg_color, bg_color);
+ viewer->registerKeyboardCallback (keyboardEventOccurred, 0);
+ viewer->addPointCloud (cpc_labeled_cloud, "cpc_cloud");
+ /// Visualization Loop
+ PCL_INFO ("Loading viewer\n");
+ while (!viewer->wasStopped ())
+ {
+ viewer->spinOnce (100);
+ /// Show Segmentation or Supervoxels
+ if (show_segmentation)
+ {
+ if (!viewer->contains ("cpc_cloud"))
+ viewer->addPointCloud (cpc_labeled_cloud, "cpc_cloud");
+ }
+ else
+ viewer->removePointCloud ("cpc_cloud");
+
+ if (show_supervoxels)
+ {
+ if (!viewer->contains ("sv_cloud"))
+ viewer->addPointCloud (sv_labeled_cloud, "sv_cloud");
+ }
+ else
+ viewer->removePointCloud ("sv_cloud");
+
+ /// Show Normals
+ if (normals_changed)
+ {
+ viewer->removePointCloud ("normals");
+ normals_changed = false;
+ }
+ if (show_normals)
+ {
+ if (!viewer->contains ("normals"))
+ viewer->addPointCloudNormals<pcl::PointNormal> (sv_centroid_normal_cloud, 1, normals_scale, "normals");
+ }
+ else
+ viewer->removePointCloud ("normals");
+ /// Show Adjacency
+ if (line_changed)
+ {
+ viewer->removeShape ("adjacency_graph");
+ line_changed = false;
+ }
+ if (show_adjacency)
+ {
+ if (!viewer->contains ("adjacency_graph"))
+ {
+ viewer->addModelFromPolyData (polyData, "adjacency_graph", 0);
+ viewer->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, line_width, "adjacency_graph");
+ }
+ }
+ else
+ {
+ viewer->removeShape ("adjacency_graph");
+ }
+
+ if (show_help)
+ {
+ viewer->removeShape ("help_text");
+ printText (viewer);
+ }
+ else
+ {
+ removeText (viewer);
+ if (!viewer->updateText ("Press d to show help", 5, 10, 12, textcolor, textcolor, textcolor, "help_text"))
+ viewer->addText ("Press d to show help", 5, 10, 12, textcolor, textcolor, textcolor, "help_text");
+ }
+
+ boost::this_thread::sleep (boost::posix_time::microseconds (100000));
+ }
+ } /// END if (show_visualization)
+
+ return (0);
+
+} /// END main
+
+/// -------------------------| Definitions of helper functions|-------------------------
+
+void
+printText (boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_arg)
+{
+ std::string on_str = "ON";
+ std::string off_str = "OFF";
+ int top = 100;
+ if (!viewer_arg->updateText ("Press (1-n) to show different elements (d) to disable this", 5, top, 12, textcolor, textcolor, textcolor, "hud_text"))
+ viewer_arg->addText ("Press (1-n) to show different elements", 5, top, 12, textcolor, textcolor, textcolor, "hud_text");
+
+ top -= 12;
+ std::string temp = "(1) Supervoxel Normals, currently " + ( (show_normals) ? on_str : off_str);
+ if (!viewer_arg->updateText (temp, 5, top, 10, textcolor, textcolor, textcolor, "normals_text"))
+ viewer_arg->addText (temp, 5, top, 10, textcolor, textcolor, textcolor, "normals_text");
+
+ top -= 24;
+ temp = "(2) Adjacency Graph, currently " + ( (show_adjacency) ? on_str : off_str) + "\n White: convex; Red: concave, Green: cut";
+ if (!viewer_arg->updateText (temp, 5, top, 10, textcolor, textcolor, textcolor, "graph_text"))
+ viewer_arg->addText (temp, 5, top, 10, textcolor, textcolor, textcolor, "graph_text");
+
+ top -= 12;
+ temp = "(3) Press to show SEGMENTATION";
+ if (!viewer_arg->updateText (temp, 5, top, 10, textcolor, textcolor, textcolor, "seg_text"))
+ viewer_arg->addText (temp, 5, top, 10, textcolor, textcolor, textcolor, "seg_text");
+
+ top -= 12;
+ temp = "(4) Press to show SUPERVOXELS";
+ if (!viewer_arg->updateText (temp, 5, top, 10, textcolor, textcolor, textcolor, "supervoxel_text"))
+ viewer_arg->addText (temp, 5, top, 10, textcolor, textcolor, textcolor, "supervoxel_text");
+
+ top -= 12;
+ temp = "(5/6) Press to increase/decrease normals scale, currently " + boost::str (boost::format ("%.3f") % normals_scale);
+ if (!viewer_arg->updateText (temp, 5, top, 10, textcolor, textcolor, textcolor, "normals_scale_text"))
+ viewer_arg->addText (temp, 5, top, 10, textcolor, textcolor, textcolor, "normals_scale_text");
+ top -= 12;
+ temp = "(7/8) Press to increase/decrease line width, currently " + boost::str (boost::format ("%.3f") % line_width);
+ if (!viewer_arg->updateText (temp, 5, top, 10, textcolor, textcolor, textcolor, "line_width_text"))
+ viewer_arg->addText (temp, 5, top, 10, textcolor, textcolor, textcolor, "line_width_text");
+}
+
+void
+removeText (boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_arg)
+{
+ viewer_arg->removeShape ("hud_text");
+ viewer_arg->removeShape ("normals_text");
+ viewer_arg->removeShape ("line_width_text");
+ viewer_arg->removeShape ("graph_text");
+ viewer_arg->removeShape ("supervoxel_text");
+ viewer_arg->removeShape ("seg_text");
+ viewer_arg->removeShape ("normals_scale_text");
+}
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
- for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
+ for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
cloud_cluster->points.push_back (cloud_ptr->points[*pit]);
cloud_cluster->width = static_cast<uint32_t> (cloud_cluster->points.size ());
cloud_cluster->height = 1;
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+// Stdlib
+#include <stdlib.h>
+#include <cmath>
+#include <limits.h>
+
+#include <boost/format.hpp>
+
+
+// PCL input/output
+#include <pcl/console/parse.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/visualization/point_cloud_color_handlers.h>
+
+//PCL other
+#include <pcl/filters/passthrough.h>
+#include <pcl/segmentation/supervoxel_clustering.h>
+
+// The segmentation class this example is for
+#include <pcl/segmentation/lccp_segmentation.h>
+
+// VTK
+#include <vtkImageReader2Factory.h>
+#include <vtkImageReader2.h>
+#include <vtkImageData.h>
+#include <vtkImageFlip.h>
+#include <vtkPolyLine.h>
+
+/// ***** Type Definitions ***** ///
+
+typedef pcl::PointXYZRGBA PointT; // The point type used for input
+typedef pcl::LCCPSegmentation<PointT>::SupervoxelAdjacencyList SuperVoxelAdjacencyList;
+
+/// Callback and variables
+
+bool show_normals = false, normals_changed = false;
+bool show_adjacency = false;
+bool show_supervoxels = false;
+bool show_help = true;
+float normals_scale;
+
+/** \brief Callback for setting options in the visualizer via keyboard.
+ * \param[in] event_arg Registered keyboard event */
+void
+keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event_arg,
+ void*)
+{
+ int key = event_arg.getKeyCode ();
+
+ if (event_arg.keyUp ())
+ switch (key)
+ {
+ case (int) '1':
+ show_normals = !show_normals;
+ normals_changed = true;
+ break;
+ case (int) '2':
+ show_adjacency = !show_adjacency;
+ break;
+ case (int) '3':
+ show_supervoxels = !show_supervoxels;
+ break;
+ case (int) '4':
+ normals_scale *= 1.25;
+ normals_changed = true;
+ break;
+ case (int) '5':
+ normals_scale *= 0.8;
+ normals_changed = true;
+ break;
+ case (int) 'd':
+ case (int) 'D':
+ show_help = !show_help;
+ break;
+ default:
+ break;
+ }
+}
+
+/// ***** Prototypes helper functions***** ///
+
+/** \brief Displays info text in the specified PCLVisualizer
+ * \param[in] viewer_arg The PCLVisualizer to modify */
+void
+printText (boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_arg);
+
+/** \brief Removes info text in the specified PCLVisualizer
+ * \param[in] viewer_arg The PCLVisualizer to modify */
+void
+removeText (boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_arg);
+
+
+/// ---- main ---- ///
+int
+main (int argc,
+ char ** argv)
+{
+ if (argc < 2) /// Print Info
+ {
+ pcl::console::print_info (
+\
+ "\n\
+-- pcl::LCCPSegmentation example -- :\n\
+\n\
+Syntax: %s input.pcd [Options] \n\
+\n\
+Output:\n\
+ -o <outname> \n\
+ Write segmented point cloud to disk (Type XYZL). If this option is specified without giving a name, the <outputname> defaults to <inputfilename>_out.pcd.\n\
+ The content of the file can be changed with the -add and -bin flags\n\
+ -novis Disable visualization\n\
+Output options:\n\
+ -add Instead of XYZL, append a label field to the input point cloud which holds the segmentation results (<input_cloud_type>+L)\n\
+ If a label field already exists in the input point cloud it will be overwritten by the segmentation\n\
+ -bin Save a binary pcd-file instead of an ascii file \n\
+ -so Additionally write the colored supervoxel image to <outfilename>_svcloud.pcd\n\
+ \n\
+Supervoxel Parameters: \n\
+ -v <voxel resolution> \n\
+ -s <seed resolution> \n\
+ -c <color weight> \n\
+ -z <spatial weight> \n\
+ -n <normal_weight> \n\
+ -tvoxel - Use single-camera-transform for voxels (Depth-Dependent-Voxel-Grid)\n\
+ -refine - Use supervoxel refinement\n\
+ -nonormals - Ignore the normals from the input pcd file\n\
+ \n\
+LCCPSegmentation Parameters: \n\
+ -ct <concavity tolerance angle> - Angle threshold for concave edges to be treated as convex. \n\
+ -st <smoothness threshold> - Invalidate steps. Value from the interval [0,1], where 0 is the strictest and 1 equals 'no smoothness check' \n\
+ -ec - Use extended (less local) convexity check\n\
+ -sc - Use sanity criterion to invalidate singular connected patches\n\
+ -smooth <mininmal segment size> - Merge small segments which have fewer points than minimal segment size\n\
+ \n",
+ argv[0]);
+ return (1);
+ }
+
+ /// -----------------------------------| Preparations |-----------------------------------
+
+ bool sv_output_specified = pcl::console::find_switch (argc, argv, "-so");
+ bool show_visualization = (!pcl::console::find_switch (argc, argv, "-novis"));
+ bool ignore_provided_normals = pcl::console::find_switch (argc, argv, "-nonormals");
+ bool add_label_field = pcl::console::find_switch (argc, argv, "-add");
+ bool save_binary_pcd = pcl::console::find_switch (argc, argv, "-bin");
+
+ /// Create variables needed for preparations
+ std::string outputname ("");
+ pcl::PointCloud<PointT>::Ptr input_cloud_ptr (new pcl::PointCloud<PointT>);
+ pcl::PointCloud<pcl::Normal>::Ptr input_normals_ptr (new pcl::PointCloud<pcl::Normal>);
+ bool has_normals = false;
+
+ /// Get pcd path from command line
+ std::string pcd_filename = argv[1];
+ PCL_INFO ("Loading pointcloud\n");
+
+ /// check if the provided pcd file contains normals
+ pcl::PCLPointCloud2 input_pointcloud2;
+ if (pcl::io::loadPCDFile (pcd_filename, input_pointcloud2))
+ {
+ PCL_ERROR ("ERROR: Could not read input point cloud %s.\n", pcd_filename.c_str ());
+ return (3);
+ }
+ pcl::fromPCLPointCloud2 (input_pointcloud2, *input_cloud_ptr);
+ if (!ignore_provided_normals)
+ {
+ if (pcl::getFieldIndex (input_pointcloud2,"normal_x") >= 0)
+ {
+ pcl::fromPCLPointCloud2 (input_pointcloud2, *input_normals_ptr);
+ has_normals = true;
+
+ //NOTE Supposedly there was a bug in old PCL versions that the orientation was not set correctly when recording clouds. This is just a workaround.
+ if (input_normals_ptr->sensor_orientation_.w () == 0)
+ {
+ input_normals_ptr->sensor_orientation_.w () = 1;
+ input_normals_ptr->sensor_orientation_.x () = 0;
+ input_normals_ptr->sensor_orientation_.y () = 0;
+ input_normals_ptr->sensor_orientation_.z () = 0;
+ }
+ }
+ else
+ PCL_WARN ("Could not find normals in pcd file. Normals will be calculated. This only works for single-camera-view pointclouds.\n");
+ }
+ PCL_INFO ("Done making cloud\n");
+
+ /// Create outputname if not given
+ bool output_specified = pcl::console::find_switch (argc, argv, "-o");
+ if (output_specified)
+ {
+ pcl::console::parse (argc, argv, "-o", outputname);
+
+ // If no filename is given, get output filename from inputname (strip seperators and file extension)
+ if (outputname.empty () || (outputname.at (0) == '-'))
+ {
+ outputname = pcd_filename;
+ size_t sep = outputname.find_last_of ('/');
+ if (sep != std::string::npos)
+ outputname = outputname.substr (sep + 1, outputname.size () - sep - 1);
+
+ size_t dot = outputname.find_last_of ('.');
+ if (dot != std::string::npos)
+ outputname = outputname.substr (0, dot);
+ }
+ }
+
+/// -----------------------------------| Main Computation |-----------------------------------
+
+ /// Default values of parameters before parsing
+ // Supervoxel Stuff
+ float voxel_resolution = 0.0075f;
+ float seed_resolution = 0.03f;
+ float color_importance = 0.0f;
+ float spatial_importance = 1.0f;
+ float normal_importance = 4.0f;
+ bool use_single_cam_transform = false;
+ bool use_supervoxel_refinement = false;
+
+ // LCCPSegmentation Stuff
+ float concavity_tolerance_threshold = 10;
+ float smoothness_threshold = 0.1;
+ uint32_t min_segment_size = 0;
+ bool use_extended_convexity = false;
+ bool use_sanity_criterion = false;
+
+ /// Parse Arguments needed for computation
+ //Supervoxel Stuff
+ use_single_cam_transform = pcl::console::find_switch (argc, argv, "-tvoxel");
+ use_supervoxel_refinement = pcl::console::find_switch (argc, argv, "-refine");
+
+ pcl::console::parse (argc, argv, "-v", voxel_resolution);
+ pcl::console::parse (argc, argv, "-s", seed_resolution);
+ pcl::console::parse (argc, argv, "-c", color_importance);
+ pcl::console::parse (argc, argv, "-z", spatial_importance);
+ pcl::console::parse (argc, argv, "-n", normal_importance);
+
+ normals_scale = seed_resolution / 2.0;
+
+ // Segmentation Stuff
+ pcl::console::parse (argc, argv, "-ct", concavity_tolerance_threshold);
+ pcl::console::parse (argc, argv, "-st", smoothness_threshold);
+ use_extended_convexity = pcl::console::find_switch (argc, argv, "-ec");
+ unsigned int k_factor = 0;
+ if (use_extended_convexity)
+ k_factor = 1;
+ use_sanity_criterion = pcl::console::find_switch (argc, argv, "-sc");
+ pcl::console::parse (argc, argv, "-smooth", min_segment_size);
+
+ /// Preparation of Input: Supervoxel Oversegmentation
+
+ pcl::SupervoxelClustering<PointT> super (voxel_resolution, seed_resolution);
+ super.setUseSingleCameraTransform (use_single_cam_transform);
+ super.setInputCloud (input_cloud_ptr);
+ if (has_normals)
+ super.setNormalCloud (input_normals_ptr);
+ super.setColorImportance (color_importance);
+ super.setSpatialImportance (spatial_importance);
+ super.setNormalImportance (normal_importance);
+ std::map<uint32_t, pcl::Supervoxel<PointT>::Ptr> supervoxel_clusters;
+
+ PCL_INFO ("Extracting supervoxels\n");
+ super.extract (supervoxel_clusters);
+
+ if (use_supervoxel_refinement)
+ {
+ PCL_INFO ("Refining supervoxels\n");
+ super.refineSupervoxels (2, supervoxel_clusters);
+ }
+ std::stringstream temp;
+ temp << " Nr. Supervoxels: " << supervoxel_clusters.size () << "\n";
+ PCL_INFO (temp.str ().c_str ());
+
+ PCL_INFO ("Getting supervoxel adjacency\n");
+ std::multimap<uint32_t, uint32_t> supervoxel_adjacency;
+ super.getSupervoxelAdjacency (supervoxel_adjacency);
+
+ /// Get the cloud of supervoxel centroid with normals and the colored cloud with supervoxel coloring (this is used for visulization)
+ pcl::PointCloud<pcl::PointNormal>::Ptr sv_centroid_normal_cloud = pcl::SupervoxelClustering<PointT>::makeSupervoxelNormalCloud (supervoxel_clusters);
+
+ /// The Main Step: Perform LCCPSegmentation
+
+ PCL_INFO ("Starting Segmentation\n");
+ pcl::LCCPSegmentation<PointT> lccp;
+ lccp.setConcavityToleranceThreshold (concavity_tolerance_threshold);
+ lccp.setSanityCheck (use_sanity_criterion);
+ lccp.setSmoothnessCheck (true, voxel_resolution, seed_resolution, smoothness_threshold);
+ lccp.setKFactor (k_factor);
+ lccp.setInputSupervoxels (supervoxel_clusters, supervoxel_adjacency);
+ lccp.setMinSegmentSize (min_segment_size);
+ lccp.segment ();
+
+ PCL_INFO ("Interpolation voxel cloud -> input cloud and relabeling\n");
+ pcl::PointCloud<pcl::PointXYZL>::Ptr sv_labeled_cloud = super.getLabeledCloud ();
+ pcl::PointCloud<pcl::PointXYZL>::Ptr lccp_labeled_cloud = sv_labeled_cloud->makeShared ();
+ lccp.relabelCloud (*lccp_labeled_cloud);
+ SuperVoxelAdjacencyList sv_adjacency_list;
+ lccp.getSVAdjacencyList (sv_adjacency_list); // Needed for visualization
+
+ /// Creating Colored Clouds and Output
+ if (lccp_labeled_cloud->size () == input_cloud_ptr->size ())
+ {
+ if (output_specified)
+ {
+ PCL_INFO ("Saving output\n");
+ if (add_label_field)
+ {
+ if (pcl::getFieldIndex (input_pointcloud2, "label") >= 0)
+ PCL_WARN ("Input cloud already has a label field. It will be overwritten by the lccp segmentation output.\n");
+ pcl::PCLPointCloud2 output_label_cloud2, output_concat_cloud2;
+ pcl::toPCLPointCloud2 (*lccp_labeled_cloud, output_label_cloud2);
+ pcl::concatenateFields (input_pointcloud2, output_label_cloud2, output_concat_cloud2);
+ pcl::io::savePCDFile (outputname + "_out.pcd", output_concat_cloud2, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), save_binary_pcd);
+ }
+ else
+ pcl::io::savePCDFile (outputname + "_out.pcd", *lccp_labeled_cloud, save_binary_pcd);
+
+ if (sv_output_specified)
+ {
+ pcl::io::savePCDFile (outputname + "_svcloud.pcd", *sv_centroid_normal_cloud, save_binary_pcd);
+ }
+ }
+ }
+ else
+ {
+ PCL_ERROR ("ERROR:: Sizes of input cloud and labeled supervoxel cloud do not match. No output is produced.\n");
+ }
+
+ /// -----------------------------------| Visualization |-----------------------------------
+
+ if (show_visualization)
+ {
+ /// Calculate visualization of adjacency graph
+ // Using lines this would be VERY slow right now, because one actor is created for every line (may be fixed in future versions of PCL)
+ // Currently this is a work-around creating a polygon mesh consisting of two triangles for each edge
+ using namespace pcl;
+
+ typedef LCCPSegmentation<PointT>::VertexIterator VertexIterator;
+ typedef LCCPSegmentation<PointT>::AdjacencyIterator AdjacencyIterator;
+ typedef LCCPSegmentation<PointT>::EdgeID EdgeID;
+
+ std::set<EdgeID> edge_drawn;
+
+ const unsigned char convex_color [3] = {255, 255, 255};
+ const unsigned char concave_color [3] = {255, 0, 0};
+ const unsigned char* color;
+
+ //The vertices in the supervoxel adjacency list are the supervoxel centroids
+ //This iterates through them, finding the edges
+ std::pair<VertexIterator, VertexIterator> vertex_iterator_range;
+ vertex_iterator_range = boost::vertices (sv_adjacency_list);
+
+ /// Create a cloud of the voxelcenters and map: VertexID in adjacency graph -> Point index in cloud
+
+ vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
+ vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New ();
+ vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+ colors->SetNumberOfComponents (3);
+ colors->SetName ("Colors");
+
+ // Create a polydata to store everything in
+ vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New ();
+ for (VertexIterator itr = vertex_iterator_range.first; itr != vertex_iterator_range.second; ++itr)
+ {
+ const uint32_t sv_label = sv_adjacency_list[*itr];
+ std::pair<AdjacencyIterator, AdjacencyIterator> neighbors = boost::adjacent_vertices (*itr, sv_adjacency_list);
+
+ for (AdjacencyIterator itr_neighbor = neighbors.first; itr_neighbor != neighbors.second; ++itr_neighbor)
+ {
+ EdgeID connecting_edge = boost::edge (*itr, *itr_neighbor, sv_adjacency_list).first; //Get the edge connecting these supervoxels
+ if (sv_adjacency_list[connecting_edge].is_convex)
+ color = convex_color;
+ else
+ color = concave_color;
+
+ // two times since we add also two points per edge
+ colors->InsertNextTupleValue (color);
+ colors->InsertNextTupleValue (color);
+
+ pcl::Supervoxel<PointT>::Ptr supervoxel = supervoxel_clusters.at (sv_label);
+ pcl::PointXYZRGBA vert_curr = supervoxel->centroid_;
+
+
+ const uint32_t sv_neighbor_label = sv_adjacency_list[*itr_neighbor];
+ pcl::Supervoxel<PointT>::Ptr supervoxel_neigh = supervoxel_clusters.at (sv_neighbor_label);
+ pcl::PointXYZRGBA vert_neigh = supervoxel_neigh->centroid_;
+
+ points->InsertNextPoint (vert_curr.data);
+ points->InsertNextPoint (vert_neigh.data);
+
+ // Add the points to the dataset
+ vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New ();
+ polyLine->GetPointIds ()->SetNumberOfIds (2);
+ polyLine->GetPointIds ()->SetId (0, points->GetNumberOfPoints ()-2);
+ polyLine->GetPointIds ()->SetId (1, points->GetNumberOfPoints ()-1);
+ cells->InsertNextCell (polyLine);
+ }
+ }
+ polyData->SetPoints (points);
+ // Add the lines to the dataset
+ polyData->SetLines (cells);
+
+ polyData->GetPointData ()->SetScalars (colors);
+
+ /// END: Calculate visualization of adjacency graph
+
+ /// Configure Visualizer
+ pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
+ viewer->setBackgroundColor (0, 0, 0);
+ viewer->registerKeyboardCallback (keyboardEventOccurred, 0);
+ viewer->addPointCloud (lccp_labeled_cloud, "maincloud");
+
+ /// Visualization Loop
+ PCL_INFO ("Loading viewer\n");
+ while (!viewer->wasStopped ())
+ {
+ viewer->spinOnce (100);
+
+ /// Show Segmentation or Supervoxels
+ viewer->updatePointCloud ( (show_supervoxels) ? sv_labeled_cloud : lccp_labeled_cloud, "maincloud");
+
+ /// Show Normals
+ if (normals_changed)
+ {
+ viewer->removePointCloud ("normals");
+ if (show_normals)
+ {
+ viewer->addPointCloudNormals<pcl::PointNormal> (sv_centroid_normal_cloud, 1, normals_scale, "normals");
+ normals_changed = false;
+ }
+ }
+ /// Show Adjacency
+ if (show_adjacency)
+ {
+ viewer->removeShape ("adjacency_graph");
+ viewer->addModelFromPolyData (polyData, "adjacency_graph");
+ }
+ else
+ {
+ viewer->removeShape ("adjacency_graph");
+ }
+
+ if (show_help)
+ {
+ viewer->removeShape ("help_text");
+ printText (viewer);
+ }
+ else
+ {
+ removeText (viewer);
+ if (!viewer->updateText ("Press d to show help", 5, 10, 12, 1.0, 1.0, 1.0, "help_text"))
+ viewer->addText ("Press d to show help", 5, 10, 12, 1.0, 1.0, 1.0, "help_text");
+ }
+
+ boost::this_thread::sleep (boost::posix_time::microseconds (100000));
+ }
+ } /// END if (show_visualization)
+
+ return (0);
+
+} /// END main
+
+/// -------------------------| Definitions of helper functions|-------------------------
+
+void
+printText (boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_arg)
+{
+ std::string on_str = "ON";
+ std::string off_str = "OFF";
+ if (!viewer_arg->updateText ("Press (1-n) to show different elements (d) to disable this", 5, 72, 12, 1.0, 1.0, 1.0, "hud_text"))
+ viewer_arg->addText ("Press (1-n) to show different elements", 5, 72, 12, 1.0, 1.0, 1.0, "hud_text");
+
+ std::string temp = "(1) Supervoxel Normals, currently " + ( (show_normals) ? on_str : off_str);
+ if (!viewer_arg->updateText (temp, 5, 60, 10, 1.0, 1.0, 1.0, "normals_text"))
+ viewer_arg->addText (temp, 5, 60, 10, 1.0, 1.0, 1.0, "normals_text");
+
+ temp = "(2) Adjacency Graph, currently " + ( (show_adjacency) ? on_str : off_str) + "\n White: convex; Red: concave";
+ if (!viewer_arg->updateText (temp, 5, 38, 10, 1.0, 1.0, 1.0, "graph_text"))
+ viewer_arg->addText (temp, 5, 38, 10, 1.0, 1.0, 1.0, "graph_text");
+
+ temp = "(3) Press to show " + ( (show_supervoxels) ? std::string ("SEGMENTATION") : std::string ("SUPERVOXELS"));
+ if (!viewer_arg->updateText (temp, 5, 26, 10, 1.0, 1.0, 1.0, "supervoxel_text"))
+ viewer_arg->addText (temp, 5, 26, 10, 1.0, 1.0, 1.0, "supervoxel_text");
+
+ temp = "(4/5) Press to increase/decrease normals scale, currently " + boost::str (boost::format ("%.3f") % normals_scale);
+ if (!viewer_arg->updateText (temp, 5, 14, 10, 1.0, 1.0, 1.0, "normals_scale_text"))
+ viewer_arg->addText (temp, 5, 14, 10, 1.0, 1.0, 1.0, "normals_scale_text");
+}
+
+void
+removeText (boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_arg)
+{
+ viewer_arg->removeShape ("hud_text");
+ viewer_arg->removeShape ("normals_text");
+ viewer_arg->removeShape ("graph_text");
+ viewer_arg->removeShape ("supervoxel_text");
+ viewer_arg->removeShape ("normals_scale_text");
+}
typedef pcl::PointCloud<PointNT> PointNCloudT;
typedef pcl::PointXYZL PointLT;
typedef pcl::PointCloud<PointLT> PointLCloudT;
+typedef pcl::Normal NormalT;
+typedef pcl::PointCloud<NormalT> NormalCloudT;
bool show_voxel_centroids = true;
bool show_supervoxels = true;
bool show_supervoxel_normals = false;
-bool show_graph = true;
+bool show_graph = false;
bool show_normals = false;
bool show_refined = false;
bool show_help = true;
+/** \brief Callback for setting options in the visualizer via keyboard.
+ * \param[in] event Registered keyboard event */
void
keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
{
std::string supervoxel_name,
boost::shared_ptr<pcl::visualization::PCLVisualizer> & viewer);
+/** \brief Displays info text in the specified PCLVisualizer
+ * \param[in] viewer_arg The PCLVisualizer to modify */
void printText (boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer);
+
+/** \brief Removes info text in the specified PCLVisualizer
+ * \param[in] viewer_arg The PCLVisualizer to modify */
void removeText (boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer);
+/** \brief Checks if the PCLPointCloud2 pc2 has the field named field_name
+ * \param[in] pc2 PCLPointCloud2 to check
+ * \param[in] field_name Fieldname to check
+ * \return True if field has been found, false otherwise */
+bool
+hasField (const pcl::PCLPointCloud2 &pc2, const std::string field_name);
+
+
using namespace pcl;
int
pcl::console::parse (argc, argv, "-d", depth_path);
PointCloudT::Ptr cloud = boost::make_shared < PointCloudT >();
+ NormalCloudT::Ptr input_normals = boost::make_shared < NormalCloudT > ();
+
bool pcd_file_specified = pcl::console::find_switch (argc, argv, "-p");
std::string pcd_path;
if (!depth_file_specified || !rgb_file_specified)
std::cout << "Using point cloud\n";
if (!pcd_file_specified)
{
- std::cout << "No cloud specified!";
+ std::cout << "No cloud specified!\n";
return (1);
}else
{
}
}
-
bool disable_transform = pcl::console::find_switch (argc, argv, "--NT");
+ bool ignore_provided_normals = pcl::console::find_switch (argc, argv, "--nonormals");
+ bool has_normals = false;
- std::string out_path;
- bool output_file_specified = pcl::console::find_switch (argc, argv, "-o");
- if (output_file_specified)
- pcl::console::parse (argc, argv, "-o", out_path);
- else
- out_path = "test_output.png";
+ std::string out_path = "test_output.png";;
+ pcl::console::parse (argc, argv, "-o", out_path);
- std::string out_label_path;
- bool output_label_file_specified = pcl::console::find_switch (argc, argv, "-l");
- if (output_label_file_specified)
- pcl::console::parse (argc, argv, "-l", out_label_path);
- else
- out_label_path = "test_output_labels.png";
+ std::string out_label_path = "test_output_labels.png";
+ pcl::console::parse (argc, argv, "-l", out_label_path);
- std::string refined_out_path;
- bool refined_output_file_specified = pcl::console::find_switch (argc, argv, "-O");
- if (refined_output_file_specified)
- pcl::console::parse (argc, argv, "-O", refined_out_path);
- else
- refined_out_path = "refined_test_output.png";
-
- std::string refined_out_label_path;
- bool refined_output_label_file_specified = pcl::console::find_switch (argc, argv, "-L");
- if (refined_output_label_file_specified)
- pcl::console::parse (argc, argv, "-L", refined_out_label_path);
- else
- refined_out_label_path = "refined_test_output_labels.png";
+ std::string refined_out_path = "refined_test_output.png";
+ pcl::console::parse (argc, argv, "-O", refined_out_path);
+ std::string refined_out_label_path = "refined_test_output_labels.png";;
+ pcl::console::parse (argc, argv, "-L", refined_out_label_path);
+
float voxel_resolution = 0.008f;
- bool voxel_res_specified = pcl::console::find_switch (argc, argv, "-v");
- if (voxel_res_specified)
- pcl::console::parse (argc, argv, "-v", voxel_resolution);
+ pcl::console::parse (argc, argv, "-v", voxel_resolution);
float seed_resolution = 0.08f;
- bool seed_res_specified = pcl::console::find_switch (argc, argv, "-s");
- if (seed_res_specified)
- pcl::console::parse (argc, argv, "-s", seed_resolution);
+ pcl::console::parse (argc, argv, "-s", seed_resolution);
float color_importance = 0.2f;
- if (pcl::console::find_switch (argc, argv, "-c"))
- pcl::console::parse (argc, argv, "-c", color_importance);
+ pcl::console::parse (argc, argv, "-c", color_importance);
float spatial_importance = 0.4f;
- if (pcl::console::find_switch (argc, argv, "-z"))
- pcl::console::parse (argc, argv, "-z", spatial_importance);
+ pcl::console::parse (argc, argv, "-z", spatial_importance);
float normal_importance = 1.0f;
- if (pcl::console::find_switch (argc, argv, "-n"))
- pcl::console::parse (argc, argv, "-n", normal_importance);
+ pcl::console::parse (argc, argv, "-n", normal_importance);
if (!pcd_file_specified)
{
}
else
{
- std::cout << "Loading pointcloud...\n";
- pcl::io::loadPCDFile (pcd_path, *cloud);
- for (PointCloudT::iterator cloud_itr = cloud->begin (); cloud_itr != cloud->end (); ++cloud_itr)
- if (cloud_itr->z < 0)
- cloud_itr->z = std::abs (cloud_itr->z);
+ /// check if the provided pcd file contains normals
+ pcl::PCLPointCloud2 input_pointcloud2;
+ if (pcl::io::loadPCDFile (pcd_path, input_pointcloud2))
+ {
+ PCL_ERROR ("ERROR: Could not read input point cloud %s.\n", pcd_path.c_str ());
+ return (3);
+ }
+ pcl::fromPCLPointCloud2 (input_pointcloud2, *cloud);
+ if (!ignore_provided_normals)
+ {
+ if (hasField (input_pointcloud2,"normal_x"))
+ {
+ std::cout << "Using normals contained in file. Set --nonormals option to disable this.\n";
+ pcl::fromPCLPointCloud2 (input_pointcloud2, *input_normals);
+ has_normals = true;
+ }
+ }
}
-
std::cout << "Done making cloud!\n";
/////////////////////////////// //////////////////////////////
////////////////////////////// //////////////////////////////
////////////////////////////// //////////////////////////////
- pcl::SupervoxelClustering<PointT> super (voxel_resolution, seed_resolution,!disable_transform);
+ // If the cloud is organized and we haven't disabled the transform we need to
+ // check that there are no negative z values, since we use log(z)
+ if (cloud->isOrganized () && !disable_transform)
+ {
+ for (PointCloudT::iterator cloud_itr = cloud->begin (); cloud_itr != cloud->end (); ++cloud_itr)
+ if (cloud_itr->z < 0)
+ {
+ PCL_ERROR ("Points found with negative Z values, this is not compatible with the single camera transform!\n");
+ PCL_ERROR ("Set the --NT option to disable the single camera transform!\n");
+ return 1;
+ }
+ std::cout <<"You have the single camera transform enabled - this should be used with point clouds captured from a single camera.\n";
+ std::cout <<"You can disable the transform with the --NT flag\n";
+ }
+
+ pcl::SupervoxelClustering<PointT> super (voxel_resolution, seed_resolution);
+ //If we manually disabled the transform then do so, otherwise the default
+ //behavior will take place (true for organized, false for unorganized)
+ if (disable_transform)
+ super.setUseSingleCameraTransform (false);
super.setInputCloud (cloud);
+ if (has_normals)
+ super.setNormalCloud (input_normals);
super.setColorImportance (color_importance);
super.setSpatialImportance (spatial_importance);
super.setNormalImportance (normal_importance);
std::cout << "Extracting supervoxels!\n";
super.extract (supervoxel_clusters);
std::cout << "Found " << supervoxel_clusters.size () << " Supervoxels!\n";
- PointCloudT::Ptr colored_voxel_cloud = super.getColoredVoxelCloud ();
+ PointLCloudT::Ptr labeled_voxel_cloud = super.getLabeledVoxelCloud ();
PointCloudT::Ptr voxel_centroid_cloud = super.getVoxelCentroidCloud ();
- PointCloudT::Ptr full_colored_cloud = super.getColoredCloud ();
PointNCloudT::Ptr sv_normal_cloud = super.makeSupervoxelNormalCloud (supervoxel_clusters);
PointLCloudT::Ptr full_labeled_cloud = super.getLabeledCloud ();
std::cout << "Refining supervoxels \n";
super.refineSupervoxels (3, refined_supervoxel_clusters);
- PointCloudT::Ptr refined_colored_voxel_cloud = super.getColoredVoxelCloud ();
+ PointLCloudT::Ptr refined_labeled_voxel_cloud = super.getLabeledVoxelCloud ();
PointNCloudT::Ptr refined_sv_normal_cloud = super.makeSupervoxelNormalCloud (refined_supervoxel_clusters);
PointLCloudT::Ptr refined_full_labeled_cloud = super.getLabeledCloud ();
- PointCloudT::Ptr refined_full_colored_cloud = super.getColoredCloud ();
// THESE ONLY MAKE SENSE FOR ORGANIZED CLOUDS
- pcl::io::savePNGFile (out_path, *full_colored_cloud, "rgb");
- pcl::io::savePNGFile (refined_out_path, *refined_full_colored_cloud, "rgb");
- pcl::io::savePNGFile (out_label_path, *full_labeled_cloud, "label");
- pcl::io::savePNGFile (refined_out_label_path, *refined_full_labeled_cloud, "label");
+ if (cloud->isOrganized ())
+ {
+ pcl::io::savePNGFile (out_label_path, *full_labeled_cloud, "label");
+ pcl::io::savePNGFile (refined_out_label_path, *refined_full_labeled_cloud, "label");
+ //Save RGB from labels
+ pcl::io::PointCloudImageExtractorFromLabelField<PointLT> pcie (pcl::io::PointCloudImageExtractorFromLabelField<PointLT>::COLORS_RGB_GLASBEY);
+ //We need to set this to account for NAN points in the organized cloud
+ pcie.setPaintNaNsWithBlack (true);
+ pcl::PCLImage image;
+ pcie.extract (*full_labeled_cloud, image);
+ pcl::io::savePNGFile (out_path, image);
+ pcie.extract (*refined_full_labeled_cloud, image);
+ pcl::io::savePNGFile (refined_out_path, image);
+ }
std::cout << "Constructing Boost Graph Library Adjacency List...\n";
typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, uint32_t, float> VoxelAdjacencyList;
- typedef VoxelAdjacencyList::vertex_descriptor VoxelID;
- typedef VoxelAdjacencyList::edge_descriptor EdgeID;
VoxelAdjacencyList supervoxel_adjacency_list;
super.getSupervoxelAdjacencyList (supervoxel_adjacency_list);
std::cout << "Loading viewer...\n";
while (!viewer->wasStopped ())
{
- if (show_voxel_centroids)
+ if (show_supervoxels)
{
- if (!viewer->updatePointCloud (voxel_centroid_cloud, "voxel centroids"))
- viewer->addPointCloud (voxel_centroid_cloud, "voxel centroids");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2.0, "voxel centroids");
- if (show_supervoxels)
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.5, "voxel centroids");
- else
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,1.0, "voxel centroids");
+ if (!viewer->updatePointCloud ((show_refined)?refined_labeled_voxel_cloud:labeled_voxel_cloud, "colored voxels"))
+ {
+ viewer->addPointCloud ((show_refined)?refined_labeled_voxel_cloud:labeled_voxel_cloud, "colored voxels");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3.0, "colored voxels");
+ }
}
else
{
- viewer->removePointCloud ("voxel centroids");
+ viewer->removePointCloud ("colored voxels");
}
- if (show_supervoxels)
+ if (show_voxel_centroids)
{
- if (!viewer->updatePointCloud ((show_refined)?refined_colored_voxel_cloud:colored_voxel_cloud, "colored voxels"))
- viewer->addPointCloud ((show_refined)?refined_colored_voxel_cloud:colored_voxel_cloud, "colored voxels");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.9, "colored voxels");
+ if (!viewer->updatePointCloud (voxel_centroid_cloud, "voxel centroids"))
+ {
+ viewer->addPointCloud (voxel_centroid_cloud, "voxel centroids");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2.0, "voxel centroids");
+ }
}
else
{
- viewer->removePointCloud ("colored voxels");
+ viewer->removePointCloud ("voxel centroids");
}
-
-
+
if (show_supervoxel_normals)
{
if (refined_sv_normal_shown != show_refined || !sv_added)
viewer->removeShape ("refined_text");
}
-
+bool
+hasField (const pcl::PCLPointCloud2 &pc2, const std::string field_name)
+{
+ for (size_t cf = 0; cf < pc2.fields.size (); ++cf)
+ if (pc2.fields[cf].name == field_name)
+ return true;
+ return false;
+}
--- /dev/null
+
+if(BUILD_visualization)
+
+ PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS visualization)
+
+ ## Find VTK
+ if(NOT VTK_FOUND)
+ set(DEFAULT FALSE)
+ set(REASON "VTK was not found.")
+ else(NOT VTK_FOUND)
+ set(DEFAULT TRUE)
+ set(REASON)
+ include (${VTK_USE_FILE})
+ endif(NOT VTK_FOUND)
+
+ PCL_ADD_EXAMPLE(pcl_example_stereo_baseline FILES example_stereo_baseline.cpp
+ LINK_WITH pcl_common pcl_visualization pcl_stereo pcl_io )
+
+endif(BUILD_visualization)
--- /dev/null
+
+#include <pcl/stereo/stereo_matching.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/visualization/image_viewer.h>
+
+#include <pcl/io/pcd_io.h>
+
+/** \brief Stereo Matching demo
+ *
+ * This demo loads a stereo image pair and computes the disparity map and related organized point cloud
+ * using the PCL AdaptiveCost-2SO Stereo Matching algorithm or the PCL Block-based stereo matching algorithm (you have to comment it out, see the code)
+ * Input pcds should be "stereo_left.pcd" and "stereo_right.pcd" which can be found in the test subfolder within trunk.
+ * A rescaled version of the disparity map is displayed, as well as the point cloud.
+ *
+ * \author Federico Tombari (federico.tombari@unibo.it)
+ * \ingroup stereo
+ */
+
+int
+main(int argc, char **argv)
+{
+
+ if (argc < 3)
+ {
+ std::cerr << "Needs two pcd input files." << std::endl;
+ return (-1);
+ }
+
+ pcl::PointCloud<pcl::RGB>::Ptr left_cloud (new pcl::PointCloud<pcl::RGB>);
+ pcl::PointCloud<pcl::RGB>::Ptr right_cloud (new pcl::PointCloud<pcl::RGB>);
+
+ //Read pcd files
+ pcl::PCDReader pcd;
+
+ if (pcd.read (argv[1], *left_cloud) == -1)
+ return (-1);
+
+ if (pcd.read (argv[2], *right_cloud) == -1)
+ return (-1);
+
+ if (!left_cloud->isOrganized () || !right_cloud->isOrganized () || left_cloud->width != right_cloud->width || left_cloud->height != right_cloud->height)
+ {
+ std::cout << "Wrong stereo pair; please check input pcds .." << std::endl;
+ return 0;
+ }
+
+ //choice between the two algorithms:
+ //pcl::AdaptiveCostSOStereoMatching stereo;
+ pcl::BlockBasedStereoMatching stereo;
+
+ stereo.setMaxDisparity(60);
+ stereo.setXOffset(0);
+ stereo.setRadius(5);
+
+ //only needed for AdaptiveCostSOStereoMatching:
+ //stereo.setSmoothWeak(20);
+ //stereo.setSmoothStrong(100);
+ //stereo.setGammaC(25);
+ //stereo.setGammaS(10);
+
+ stereo.setRatioFilter(20);
+ stereo.setPeakFilter(0);
+
+ stereo.setLeftRightCheck(true);
+ stereo.setLeftRightCheckThreshold(1);
+
+ stereo.setPreProcessing(true);
+
+ stereo.compute(*left_cloud, *right_cloud);
+
+ stereo.medianFilter(4);
+
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud( new pcl::PointCloud<pcl::PointXYZRGB> );
+
+ stereo.getPointCloud(318.112200, 224.334900, 368.534700, 0.8387445, out_cloud, left_cloud);
+
+ pcl::PointCloud<pcl::RGB>::Ptr vmap( new pcl::PointCloud<pcl::RGB> );
+ stereo.getVisualMap(vmap);
+
+ pcl::visualization::ImageViewer iv ("My viewer");
+ iv.addRGBImage<pcl::RGB> (vmap);
+ //iv.addRGBImage<pcl::RGB> (left_cloud);
+ //iv.spin (); // press 'q' to exit
+
+ boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
+ viewer->setBackgroundColor (0, 0, 0);
+
+ //viewer->addPointCloud<pcl::PointXYZRGB> (out_cloud, "stereo");
+ pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> intensity(out_cloud);
+ viewer->addPointCloud<pcl::PointXYZRGB> (out_cloud, intensity, "stereo");
+
+ //viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "stereo");
+ viewer->initCameraParameters ();
+ //viewer->spin();
+ while (!viewer->wasStopped ())
+ {
+ viewer->spinOnce (100);
+ iv.spinOnce (100); // press 'q' to exit
+ boost::this_thread::sleep (boost::posix_time::microseconds (100000));
+ }
+}
set(SUBSYS_NAME features)
set(SUBSYS_DESC "Point cloud features library")
-set(SUBSYS_DEPS common search kdtree octree filters)
+set(SUBSYS_DEPS common search kdtree octree filters 2d)
set(build TRUE)
PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
"include/pcl/${SUBSYS_NAME}/boost.h"
"include/pcl/${SUBSYS_NAME}/eigen.h"
"include/pcl/${SUBSYS_NAME}/board.h"
+ "include/pcl/${SUBSYS_NAME}/brisk_2d.h"
"include/pcl/${SUBSYS_NAME}/cppf.h"
"include/pcl/${SUBSYS_NAME}/cvfh.h"
"include/pcl/${SUBSYS_NAME}/our_cvfh.h"
"include/pcl/${SUBSYS_NAME}/feature.h"
"include/pcl/${SUBSYS_NAME}/fpfh.h"
"include/pcl/${SUBSYS_NAME}/fpfh_omp.h"
+ "include/pcl/${SUBSYS_NAME}/from_meshes.h"
"include/pcl/${SUBSYS_NAME}/gfpfh.h"
"include/pcl/${SUBSYS_NAME}/integral_image2D.h"
"include/pcl/${SUBSYS_NAME}/integral_image_normal.h"
"include/pcl/${SUBSYS_NAME}/normal_3d.h"
"include/pcl/${SUBSYS_NAME}/normal_3d_omp.h"
"include/pcl/${SUBSYS_NAME}/normal_based_signature.h"
+ "include/pcl/${SUBSYS_NAME}/organized_edge_detection.h"
"include/pcl/${SUBSYS_NAME}/pfh.h"
"include/pcl/${SUBSYS_NAME}/pfh_tools.h"
"include/pcl/${SUBSYS_NAME}/pfhrgb.h"
"include/pcl/${SUBSYS_NAME}/principal_curvatures.h"
"include/pcl/${SUBSYS_NAME}/rift.h"
"include/pcl/${SUBSYS_NAME}/rops_estimation.h"
- #"include/pcl/${SUBSYS_NAME}/rsd.h"
+ "include/pcl/${SUBSYS_NAME}/rsd.h"
+ "include/pcl/${SUBSYS_NAME}/grsd.h"
"include/pcl/${SUBSYS_NAME}/statistical_multiscale_interest_region_extraction.h"
"include/pcl/${SUBSYS_NAME}/vfh.h"
"include/pcl/${SUBSYS_NAME}/esf.h"
set(impl_incs
"include/pcl/${SUBSYS_NAME}/impl/board.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/brisk_2d.hpp"
"include/pcl/${SUBSYS_NAME}/impl/cppf.hpp"
"include/pcl/${SUBSYS_NAME}/impl/cvfh.hpp"
"include/pcl/${SUBSYS_NAME}/impl/our_cvfh.hpp"
"include/pcl/${SUBSYS_NAME}/impl/normal_3d.hpp"
"include/pcl/${SUBSYS_NAME}/impl/normal_3d_omp.hpp"
"include/pcl/${SUBSYS_NAME}/impl/normal_based_signature.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/organized_edge_detection.hpp"
"include/pcl/${SUBSYS_NAME}/impl/pfh.hpp"
"include/pcl/${SUBSYS_NAME}/impl/pfhrgb.hpp"
"include/pcl/${SUBSYS_NAME}/impl/ppf.hpp"
"include/pcl/${SUBSYS_NAME}/impl/principal_curvatures.hpp"
"include/pcl/${SUBSYS_NAME}/impl/rift.hpp"
"include/pcl/${SUBSYS_NAME}/impl/rops_estimation.hpp"
- #"include/pcl/${SUBSYS_NAME}/impl/rsd.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/rsd.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/grsd.hpp"
"include/pcl/${SUBSYS_NAME}/impl/statistical_multiscale_interest_region_extraction.hpp"
"include/pcl/${SUBSYS_NAME}/impl/vfh.hpp"
"include/pcl/${SUBSYS_NAME}/impl/esf.hpp"
set(srcs
src/board.cpp
+ src/brisk_2d.cpp
src/boundary.cpp
src/cppf.cpp
src/cvfh.cpp
src/narf.cpp
src/normal_3d.cpp
src/normal_based_signature.cpp
+ src/organized_edge_detection.cpp
src/pfh.cpp
src/ppf.cpp
src/shot.cpp
src/principal_curvatures.cpp
src/rift.cpp
src/rops_estimation.cpp
- #src/rsd.cpp
+ src/rsd.cpp
+ src/grsd.cpp
src/statistical_multiscale_interest_region_extraction.cpp
src/vfh.cpp
src/esf.cpp
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception , Inc.
+ * Copyright (C) 2011 The Autonomous Systems Lab (ASL), ETH Zurich,
+ * Stefan Leutenegger, Simon Lynen and Margarita Chli.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_FEATURES_BRISK_2D_H_
+#define PCL_FEATURES_BRISK_2D_H_
+
+// PCL includes
+#include <pcl/features/feature.h>
+#include <pcl/common/eigen.h>
+#include <pcl/common/centroid.h>
+#include <pcl/common/intensity.h>
+
+namespace pcl
+{
+ /** \brief Implementation of the BRISK-descriptor, based on the original code and paper reference by
+ *
+ * \par
+ * Stefan Leutenegger,Margarita Chli and Roland Siegwart,
+ * BRISK: Binary Robust Invariant Scalable Keypoints,
+ * in Proceedings of the IEEE International Conference on Computer Vision (ICCV2011).
+ *
+ * \warning The input keypoints cloud is not const, and it will be modified: keypoints for which descriptors can not
+ * be computed will be deleted from the cloud.
+ *
+ * \author Radu B. Rusu, Stefan Holzer
+ * \ingroup features
+ */
+ template <typename PointInT,
+ typename PointOutT = pcl::BRISKSignature512,
+ typename KeypointT = pcl::PointWithScale,
+ typename IntensityT = pcl::common::IntensityFieldAccessor<PointInT> >
+ class BRISK2DEstimation// : public Feature<PointT, KeyPointT>
+ {
+ public:
+ typedef boost::shared_ptr<BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT> > Ptr;
+ typedef boost::shared_ptr<const BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT> > ConstPtr;
+
+ typedef typename pcl::PointCloud<PointInT> PointCloudInT;
+ typedef typename pcl::PointCloud<PointInT>::ConstPtr PointCloudInTConstPtr;
+
+ typedef typename pcl::PointCloud<KeypointT> KeypointPointCloudT;
+ typedef typename pcl::PointCloud<KeypointT>::Ptr KeypointPointCloudTPtr;
+ typedef typename pcl::PointCloud<KeypointT>::ConstPtr KeypointPointCloudTConstPtr;
+
+ typedef typename pcl::PointCloud<PointOutT> PointCloudOutT;
+
+ /** \brief Constructor. */
+ BRISK2DEstimation ();
+
+ /** \brief Destructor. */
+ virtual ~BRISK2DEstimation ();
+
+ /** \brief Determines whether rotation invariance is enabled.
+ * \param[in] enable determines whether rotation invariance is enabled.
+ */
+ inline void
+ setRotationInvariance (const bool enable)
+ {
+ rotation_invariance_enabled_ = enable;
+ }
+
+ /** \brief Determines whether scale invariance is enabled.
+ * \param[in] enable determines whether scale invariance is enabled.
+ */
+ inline void
+ setScaleInvariance (const bool enable)
+ {
+ scale_invariance_enabled_ = enable;
+ }
+
+ /** \brief Sets the input cloud.
+ * \param[in] cloud the input cloud.
+ */
+ inline void
+ setInputCloud (const PointCloudInTConstPtr & cloud)
+ {
+ input_cloud_ = cloud;
+ }
+
+ /** \brief Sets the input keypoints.
+ * \param[in] keypoints the input cloud containing the keypoints.
+ */
+ inline void
+ setKeypoints (const KeypointPointCloudTPtr &keypoints)
+ {
+ // Make a copy as we will have to erase keypoints that we don't use
+ // TO DO: change this later
+ //keypoints_.reset (new KeypointPointCloudT (*keypoints));
+ keypoints_ = keypoints;
+ }
+
+ /** \brief Computes the descriptors for the previously specified
+ * points and input data.
+ * \param[out] output descriptors the destination for the computed descriptors.
+ */
+ void
+ compute (PointCloudOutT &output);
+ //td::vector<pcl::features::brisk::BRISKDescriptor> & descriptors) const;
+
+ protected:
+ /** \brief Call this to generate the kernel:
+ * circle of radius r (pixels), with n points;
+ * short pairings with dMax, long pairings with dMin
+ *
+ * \note This should never be called by a regular user. We use a fixed type in PCL
+ * (BRISKSignature512) and tampering with the parameters might lead to a different
+ * size descriptor which the user needs to accomodate in a new point type.
+ */
+ void
+ generateKernel (std::vector<float> &radius_list,
+ std::vector<int> &number_list,
+ float d_max = 5.85f, float d_min = 8.2f,
+ std::vector<int> index_change = std::vector<int> ());
+
+ /** \brief Compute the smoothed intensity for a given x/y position in the image. */
+ inline int
+ smoothedIntensity (const std::vector<unsigned char>& image,
+ int image_width, int image_height,
+ const std::vector<int>& integral_image,
+ const float key_x, const float key_y, const unsigned int scale,
+ const unsigned int rot, const unsigned int point) const;
+
+ private:
+ /** \brief ROI predicate comparator. */
+ bool
+ RoiPredicate (const float min_x, const float min_y,
+ const float max_x, const float max_y, const KeypointT& key_pt);
+
+ /** \brief Specifies whether rotation invariance is enabled. */
+ bool rotation_invariance_enabled_;
+
+ /** \brief Specifies whether scale invariance is enabled. */
+ bool scale_invariance_enabled_;
+
+ /** \brief Specifies the scale of the pattern. */
+ const float pattern_scale_;
+
+ /** \brief the input cloud. */
+ PointCloudInTConstPtr input_cloud_;
+
+ /** \brief the input keypoints. */
+ KeypointPointCloudTPtr keypoints_;
+
+ // TODO: set
+ float scale_range_;
+
+ // Some helper structures for the Brisk pattern representation
+ struct BriskPatternPoint
+ {
+ /** x coordinate relative to center. */
+ float x;
+ /** x coordinate relative to center. */
+ float y;
+ /** Gaussian smoothing sigma. */
+ float sigma;
+ };
+
+ struct BriskShortPair
+ {
+ /** index of the first pattern point. */
+ unsigned int i;
+ /** index of other pattern point. */
+ unsigned int j;
+ };
+
+ struct BriskLongPair
+ {
+ /** index of the first pattern point. */
+ unsigned int i;
+ /** index of other pattern point. */
+ unsigned int j;
+ /** 1024.0/dx. */
+ int weighted_dx;
+ /** 1024.0/dy. */
+ int weighted_dy;
+ };
+
+ // pattern properties
+ /** [i][rotation][scale]. */
+ BriskPatternPoint* pattern_points_;
+
+ /** Total number of collocation points. */
+ unsigned int points_;
+
+ /** Discretization of the rotation look-up. */
+ const unsigned int n_rot_;
+
+ /** Lists the scaling per scale index [scale]. */
+ float* scale_list_;
+
+ /** Lists the total pattern size per scale index [scale]. */
+ unsigned int* size_list_;
+
+ /** Scales discretization. */
+ const unsigned int scales_;
+
+ /** Span of sizes 40->4 Octaves - else, this needs to be adjusted... */
+ const float scalerange_;
+
+ // general
+ const float basic_size_;
+
+ // pairs
+ /** Number of uchars the descriptor consists of. */
+ int strings_;
+ /** Short pair maximum distance. */
+ float d_max_;
+ /** Long pair maximum distance. */
+ float d_min_;
+ /** d<_d_max. */
+ BriskShortPair* short_pairs_;
+ /** d>_d_min. */
+ BriskLongPair* long_pairs_;
+ /** Number of short pairs. */
+ unsigned int no_short_pairs_;
+ /** Number of long pairs. */
+ unsigned int no_long_pairs_;
+
+ /** \brief Intensity field accessor. */
+ IntensityT intensity_;
+
+ /** \brief The name of the class. */
+ std::string name_;
+ };
+
+}
+
+#include <pcl/features/impl/brisk_2d.hpp>
+
+#endif //#ifndef PCL_FEATURES_BRISK_2D_H_
* \param[out] centroids vector to hold the centroids
*/
inline void
- getCentroidClusters (std::vector<Eigen::Vector3f> & centroids)
+ getCentroidClusters (std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > & centroids)
{
for (size_t i = 0; i < centroids_dominant_orientations_.size (); ++i)
centroids.push_back (centroids_dominant_orientations_[i]);
* \param[out] centroids vector to hold the normal centroids
*/
inline void
- getCentroidNormalClusters (std::vector<Eigen::Vector3f> & centroids)
+ getCentroidNormalClusters (std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > & centroids)
{
for (size_t i = 0; i < dominant_normals_.size (); ++i)
centroids.push_back (dominant_normals_[i]);
protected:
/** \brief Centroids that were used to compute different CVFH descriptors */
- std::vector<Eigen::Vector3f> centroids_dominant_orientations_;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > centroids_dominant_orientations_;
/** \brief Normal centroids that were used to compute different CVFH descriptors */
- std::vector<Eigen::Vector3f> dominant_normals_;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > dominant_normals_;
};
}
--- /dev/null
+#ifndef PCL_FEATURES_FROM_MESHES_H_
+#define PCL_FEATURES_FROM_MESHES_H_
+
+#include <pcl/features/normal_3d.h>
+
+namespace pcl
+{
+ namespace features
+ {
+
+ /** \brief Compute approximate surface normals on a mesh.
+ * \param[in] cloud Point cloud containing the XYZ coordinates.
+ * \param[in] polygons Polygons from the mesh.
+ * \param[out] normals Point cloud with computed surface normals
+ */
+ template <typename PointT, typename PointNT> inline void
+ computeApproximateNormals(const pcl::PointCloud<PointT>& cloud, const std::vector<pcl::Vertices>& polygons, pcl::PointCloud<PointNT>& normals)
+ {
+ int nr_points = static_cast<int>(cloud.points.size());
+ int nr_polygons = static_cast<int>(polygons.size());
+
+ normals.header = cloud.header;
+ normals.width = cloud.width;
+ normals.height = cloud.height;
+ normals.points.resize(nr_points);
+
+ for ( int i = 0; i < nr_points; ++i )
+ normals.points[i].getNormalVector3fMap() = Eigen::Vector3f::Zero();
+
+ // NOTE: for efficiency the weight is computed implicitly by using the
+ // cross product, this causes inaccurate normals for meshes containing
+ // non-triangle polygons (quads or other types)
+ for ( int i = 0; i < nr_polygons; ++i )
+ {
+ const int nr_points_polygon = (int)polygons[i].vertices.size();
+ if (nr_points_polygon < 3) continue;
+
+ // compute normal for triangle
+ Eigen::Vector3f vec_a_b = cloud.points[polygons[i].vertices[0]].getVector3fMap() - cloud.points[polygons[i].vertices[1]].getVector3fMap();
+ Eigen::Vector3f vec_a_c = cloud.points[polygons[i].vertices[0]].getVector3fMap() - cloud.points[polygons[i].vertices[2]].getVector3fMap();
+ Eigen::Vector3f normal = vec_a_b.cross(vec_a_c);
+ pcl::flipNormalTowardsViewpoint(cloud.points[polygons[i].vertices[0]], 0.0f, 0.0f, 0.0f, normal(0), normal(1), normal(2));
+
+ // add normal to all points in polygon
+ for ( int j = 0; j < nr_points_polygon; ++j )
+ normals.points[polygons[i].vertices[j]].getNormalVector3fMap() += normal;
+ }
+
+ for ( int i = 0; i < nr_points; ++i )
+ {
+ normals.points[i].getNormalVector3fMap().normalize();
+ pcl::flipNormalTowardsViewpoint(cloud.points[i], 0.0f, 0.0f, 0.0f, normals.points[i].normal_x, normals.points[i].normal_y, normals.points[i].normal_z);
+ }
+ }
+
+
+ /** \brief Compute GICP-style covariance matrices given a point cloud and
+ * the corresponding surface normals.
+ * \param[in] cloud Point cloud containing the XYZ coordinates,
+ * \param[in] normals Point cloud containing the corresponding surface normals.
+ * \param[out] covariances Vector of computed covariances.
+ * \param[in] Optional: Epsilon for the expected noise along the surface normal (default: 0.001)
+ */
+ template <typename PointT, typename PointNT> inline void
+ computeApproximateCovariances(const pcl::PointCloud<PointT>& cloud,
+ const pcl::PointCloud<PointNT>& normals,
+ std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> >& covariances,
+ double epsilon = 0.001)
+ {
+ assert(cloud.points.size() == normals.points.size());
+
+ int nr_points = static_cast<int>(cloud.points.size());
+ covariances.resize(nr_points);
+ for (int i = 0; i < nr_points; ++i)
+ {
+ Eigen::Vector3d normal(normals.points[i].normal_x,
+ normals.points[i].normal_y,
+ normals.points[i].normal_z);
+
+ // compute rotation matrix
+ Eigen::Matrix3d rot;
+ Eigen::Vector3d y;
+ y << 0, 1, 0;
+ rot.row(2) = normal;
+ y = y - normal(1) * normal;
+ y.normalize();
+ rot.row(1) = y;
+ rot.row(0) = normal.cross(rot.row(1));
+
+ // comnpute approximate covariance
+ Eigen::Matrix3d cov;
+ cov << 1, 0, 0,
+ 0, 1, 0,
+ 0, 0, epsilon;
+ covariances[i] = rot.transpose()*cov*rot;
+ }
+ }
+
+ }
+}
+
+
+#endif // PCL_FEATURES_FROM_MESHES_H_
+
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2009-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_FEATURES_GRSD_H_
+#define PCL_FEATURES_GRSD_H_
+
+#include <pcl/features/feature.h>
+#include <pcl/features/rsd.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/kdtree/kdtree_flann.h>
+
+namespace pcl
+{
+ /** \brief @b GRSDEstimation estimates the Global Radius-based Surface Descriptor (GRSD) for a given point cloud dataset
+ * containing points and normals.
+ *
+ * @note If you use this code in any academic work, please cite (first for the ray-casting and second for the additive version):
+ *
+ * <ul>
+ * <li> Z.C. Marton, D. Pangercic, N. Blodow, Michael Beetz.
+ * Combined 2D-3D Categorization and Classification for Multimodal Perception Systems.
+ * In The International Journal of Robotics Research, Sage Publications
+ * pages 1378--1402, Volume 30, Number 11, September 2011.
+ * </li>
+ * <li> A. Kanezaki, Z.C. Marton, D. Pangercic, T. Harada, Y. Kuniyoshi, M. Beetz.
+ * Voxelized Shape and Color Histograms for RGB-D
+ * In the Workshop on Active Semantic Perception and Object Search in the Real World, in conjunction with the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)
+ * San Francisco, California, September 25-30, 2011.
+ * </li>
+ * </ul>
+ *
+ * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
+ * \ref FPFHEstimationOMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
+ * \author Zoltan Csaba Marton
+ * \ingroup features
+ */
+
+ template <typename PointInT, typename PointNT, typename PointOutT>
+ class GRSDEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT>
+ {
+ public:
+ using Feature<PointInT, PointOutT>::feature_name_;
+ using Feature<PointInT, PointOutT>::getClassName;
+ using Feature<PointInT, PointOutT>::indices_;
+ using Feature<PointInT, PointOutT>::search_radius_;
+ using Feature<PointInT, PointOutT>::search_parameter_;
+ using Feature<PointInT, PointOutT>::input_;
+ using FeatureFromNormals<PointInT, PointNT, PointOutT>::normals_;
+ using Feature<PointInT, PointOutT>::setSearchSurface;
+ //using Feature<PointInT, PointOutT>::computeFeature;
+
+ typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut;
+ typedef typename Feature<PointInT, PointOutT>::PointCloudIn PointCloudIn;
+ typedef typename Feature<PointInT, PointOutT>::PointCloudInPtr PointCloudInPtr;
+
+ /** \brief Constructor. */
+ GRSDEstimation () : additive_ (true)
+ {
+ feature_name_ = "GRSDEstimation";
+ relative_coordinates_all_ = getAllNeighborCellIndices ();
+ };
+
+ /** \brief Set the sphere radius that is to be used for determining the nearest neighbors used for the feature
+ * estimation. Same value will be used for the internal voxel grid leaf size.
+ * \param[in] radius the sphere radius used as the maximum distance to consider a point a neighbor
+ */
+ inline void
+ setRadiusSearch (double radius) { width_ = search_radius_ = radius; }
+
+ /** \brief Get the sphere radius used for determining the neighbors.
+ * \return the sphere radius used as the maximum distance to consider a point a neighbor
+ */
+ inline double
+ getRadiusSearch () const { return (search_radius_); }
+
+ /** \brief Get the type of the local surface based on the min and max radius computed.
+ * \return the integer that represents the type of the local surface with values as
+ * Plane (1), Cylinder (2), Noise or corner (0), Sphere (3) and Edge (4)
+ */
+ static inline int
+ getSimpleType (float min_radius, float max_radius,
+ double min_radius_plane = 0.100,
+ double max_radius_noise = 0.015,
+ double min_radius_cylinder = 0.175,
+ double max_min_radius_diff = 0.050);
+
+ protected:
+
+ /** \brief Estimate the Global Radius-based Surface Descriptor (GRSD) for a set of points given by
+ * <setInputCloud (), setIndices ()> using the surface in setSearchSurface () and the spatial locator in
+ * setSearchMethod ()
+ * \param output the resultant point cloud that contains the GRSD feature
+ */
+ void
+ computeFeature (PointCloudOut &output);
+
+ private:
+
+ /** \brief Defines if an additive feature is computed or ray-casting is used to get a more descriptive feature. */
+ bool additive_;
+
+ /** \brief Defines the voxel size to be used. */
+ double width_;
+
+ /** \brief Pre-computed the relative cell indices of all the 26 neighbors. */
+ Eigen::MatrixXi relative_coordinates_all_;
+
+ };
+
+}
+
+#ifdef PCL_NO_PRECOMPILE
+#include <pcl/features/impl/grsd.hpp>
+#endif
+
+#endif /* PCL_FEATURES_GRSD_H_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception , Inc.
+ * Copyright (C) 2011 The Autonomous Systems Lab (ASL), ETH Zurich,
+ * Stefan Leutenegger, Simon Lynen and Margarita Chli.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_FEATURES_IMPL_BRISK_2D_HPP_
+#define PCL_FEATURES_IMPL_BRISK_2D_HPP_
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT>
+pcl::BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::BRISK2DEstimation ()
+ : rotation_invariance_enabled_ (true)
+ , scale_invariance_enabled_ (true)
+ , pattern_scale_ (1.0f)
+ , input_cloud_ (), keypoints_ (), scale_range_ (), pattern_points_ (), points_ ()
+ , n_rot_ (1024), scale_list_ (NULL), size_list_ (NULL)
+ , scales_ (64)
+ , scalerange_ (30)
+ , basic_size_ (12.0)
+ , strings_ (0), d_max_ (0.0f), d_min_ (0.0f), short_pairs_ (), long_pairs_ ()
+ , no_short_pairs_ (0), no_long_pairs_ (0)
+ , intensity_ ()
+ , name_ ("BRISK2Destimation")
+{
+ // Since we do not assume pattern_scale_ should be changed by the user, we
+ // can initialize the kernel in the constructor
+ std::vector<float> r_list;
+ std::vector<int> n_list;
+
+ // this is the standard pattern found to be suitable also
+ r_list.resize (5);
+ n_list.resize (5);
+ const float f = 0.85f * pattern_scale_;
+
+ r_list[0] = f * 0.0f;
+ r_list[1] = f * 2.9f;
+ r_list[2] = f * 4.9f;
+ r_list[3] = f * 7.4f;
+ r_list[4] = f * 10.8f;
+
+ n_list[0] = 1;
+ n_list[1] = 10;
+ n_list[2] = 14;
+ n_list[3] = 15;
+ n_list[4] = 20;
+
+ generateKernel (r_list, n_list, 5.85f * pattern_scale_, 8.2f * pattern_scale_);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT>
+pcl::BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::~BRISK2DEstimation ()
+{
+ if (pattern_points_) delete [] pattern_points_;
+ if (short_pairs_) delete [] short_pairs_;
+ if (long_pairs_) delete [] long_pairs_;
+ if (scale_list_) delete [] scale_list_;
+ if (size_list_) delete [] size_list_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT> void
+pcl::BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::generateKernel (
+ std::vector<float> &radius_list,
+ std::vector<int> &number_list, float d_max, float d_min,
+ std::vector<int> index_change)
+{
+ d_max_ = d_max;
+ d_min_ = d_min;
+
+ // get the total number of points
+ const int rings = int (radius_list.size ());
+ assert (radius_list.size () != 0 && radius_list.size () == number_list.size ());
+ points_ = 0; // remember the total number of points
+ for (int ring = 0; ring < rings; ring++)
+ points_ += number_list[ring];
+
+ // set up the patterns
+ pattern_points_ = new BriskPatternPoint[points_*scales_*n_rot_];
+ BriskPatternPoint* pattern_iterator = pattern_points_;
+
+ // define the scale discretization:
+ static const float lb_scale = logf (scalerange_) / logf (2.0);
+ static const float lb_scale_step = lb_scale / (float (scales_));
+
+ scale_list_ = new float[scales_];
+ size_list_ = new unsigned int[scales_];
+
+ const float sigma_scale = 1.3f;
+
+ for (unsigned int scale = 0; scale < scales_; ++scale)
+ {
+ scale_list_[scale] = static_cast<float> (pow (double (2.0), static_cast<double> (float (scale) * lb_scale_step)));
+ size_list_[scale] = 0;
+
+ // generate the pattern points look-up
+ double alpha, theta;
+ for (size_t rot = 0; rot < n_rot_; ++rot)
+ {
+ // this is the rotation of the feature
+ theta = double (rot) * 2 * M_PI / double (n_rot_);
+ for (int ring = 0; ring < rings; ++ring)
+ {
+ for (int num = 0; num < number_list[ring]; ++num)
+ {
+ // the actual coordinates on the circle
+ alpha = double (num) * 2 * M_PI / double (number_list[ring]);
+
+ // feature rotation plus angle of the point
+ pattern_iterator->x = scale_list_[scale] * radius_list[ring] * static_cast<float> (cos (alpha + theta));
+ pattern_iterator->y = scale_list_[scale] * radius_list[ring] * static_cast<float> (sin (alpha + theta));
+ // and the gaussian kernel sigma
+ if (ring == 0)
+ pattern_iterator->sigma = sigma_scale * scale_list_[scale] * 0.5f;
+ else
+ pattern_iterator->sigma = static_cast<float> (sigma_scale * scale_list_[scale] * (double (radius_list[ring])) * sin (M_PI / double (number_list[ring])));
+
+ // adapt the sizeList if necessary
+ const unsigned int size = static_cast<const unsigned int> (ceil (((scale_list_[scale] * radius_list[ring]) + pattern_iterator->sigma)) + 1);
+
+ if (size_list_[scale] < size)
+ size_list_[scale] = size;
+
+ // increment the iterator
+ ++pattern_iterator;
+ }
+ }
+ }
+ }
+
+ // now also generate pairings
+ short_pairs_ = new BriskShortPair[points_ * (points_ - 1) / 2];
+ long_pairs_ = new BriskLongPair[points_ * (points_ - 1) / 2];
+ no_short_pairs_ = 0;
+ no_long_pairs_ = 0;
+
+ // fill index_change with 0..n if empty
+ unsigned int ind_size = static_cast<unsigned int> (index_change.size ());
+ if (ind_size == 0)
+ {
+ index_change.resize (points_ * (points_ - 1) / 2);
+ ind_size = static_cast<unsigned int> (index_change.size ());
+ }
+ for (unsigned int i = 0; i < ind_size; i++)
+ index_change[i] = i;
+
+ const float d_min_sq = d_min_ * d_min_;
+ const float d_max_sq = d_max_ * d_max_;
+ for (unsigned int i = 1; i < points_; i++)
+ {
+ for (unsigned int j = 0; j < i; j++)
+ { //(find all the pairs)
+ // point pair distance:
+ const float dx = pattern_points_[j].x - pattern_points_[i].x;
+ const float dy = pattern_points_[j].y - pattern_points_[i].y;
+ const float norm_sq = (dx*dx+dy*dy);
+ if (norm_sq > d_min_sq)
+ {
+ // save to long pairs
+ BriskLongPair& longPair = long_pairs_[no_long_pairs_];
+ longPair.weighted_dx = int ((dx / (norm_sq)) * 2048.0 + 0.5);
+ longPair.weighted_dy = int ((dy / (norm_sq)) * 2048.0 + 0.5);
+ longPair.i = i;
+ longPair.j = j;
+ ++no_long_pairs_;
+ }
+ else if (norm_sq < d_max_sq)
+ {
+ // save to short pairs
+ assert (no_short_pairs_ < ind_size); // make sure the user passes something sensible
+ BriskShortPair& shortPair = short_pairs_[index_change[no_short_pairs_]];
+ shortPair.j = j;
+ shortPair.i = i;
+ ++no_short_pairs_;
+ }
+ }
+ }
+
+ // no bits:
+ strings_ = int (ceil ((float (no_short_pairs_)) / 128.0)) * 4 * 4;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT> inline int
+pcl::BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::smoothedIntensity (
+ const std::vector<unsigned char> &image,
+ int image_width, int,
+ //const Stefan& integral,
+ const std::vector<int> &integral_image,
+ const float key_x, const float key_y, const unsigned int scale,
+ const unsigned int rot, const unsigned int point) const
+{
+ // get the float position
+ const BriskPatternPoint& brisk_point = pattern_points_[scale * n_rot_*points_ + rot * points_ + point];
+ const float xf = brisk_point.x + key_x;
+ const float yf = brisk_point.y + key_y;
+ const int x = int (xf);
+ const int y = int (yf);
+ const int& imagecols = image_width;
+
+ // get the sigma:
+ const float sigma_half = brisk_point.sigma;
+ const float area = 4.0f * sigma_half * sigma_half;
+
+ // Get the point step
+
+ // calculate output:
+ int ret_val;
+ if (sigma_half < 0.5)
+ {
+ // interpolation multipliers:
+ const int r_x = static_cast<int> ((xf - float (x)) * 1024);
+ const int r_y = static_cast<int> ((yf - float (y)) * 1024);
+ const int r_x_1 = (1024 - r_x);
+ const int r_y_1 = (1024 - r_y);
+
+ //+const unsigned char* ptr = static_cast<const unsigned char*> (&image.points[0].r) + x + y * imagecols;
+ const unsigned char* ptr = static_cast<const unsigned char*>(&image[0]) + x + y * imagecols;
+
+ // just interpolate:
+ ret_val = (r_x_1 * r_y_1 * int (*ptr));
+
+ //+ptr += sizeof (PointInT);
+ ptr++;
+
+ ret_val += (r_x * r_y_1 * int (*ptr));
+
+ //+ptr += (imagecols * sizeof (PointInT));
+ ptr += imagecols;
+
+ ret_val += (r_x * r_y * int (*ptr));
+
+ //+ptr -= sizeof (PointInT);
+ ptr--;
+
+ ret_val += (r_x_1 * r_y * int (*ptr));
+ return (ret_val + 512) / 1024;
+ }
+
+ // this is the standard case (simple, not speed optimized yet):
+
+ // scaling:
+ const int scaling = static_cast<int> (4194304.0f / area);
+ const int scaling2 = static_cast<int> (float (scaling) * area / 1024.0f);
+
+ // the integral image is larger:
+ const int integralcols = imagecols + 1;
+
+ // calculate borders
+ const float x_1 = xf - sigma_half;
+ const float x1 = xf + sigma_half;
+ const float y_1 = yf - sigma_half;
+ const float y1 = yf + sigma_half;
+
+ const int x_left = int (x_1 + 0.5);
+ const int y_top = int (y_1 + 0.5);
+ const int x_right = int (x1 + 0.5);
+ const int y_bottom = int (y1 + 0.5);
+
+ // overlap area - multiplication factors:
+ const float r_x_1 = float (x_left) - x_1 + 0.5f;
+ const float r_y_1 = float (y_top) - y_1 + 0.5f;
+ const float r_x1 = x1 - float (x_right) + 0.5f;
+ const float r_y1 = y1 - float (y_bottom) + 0.5f;
+ const int dx = x_right - x_left - 1;
+ const int dy = y_bottom - y_top - 1;
+ const int A = static_cast<int> ((r_x_1 * r_y_1) * float (scaling));
+ const int B = static_cast<int> ((r_x1 * r_y_1) * float (scaling));
+ const int C = static_cast<int> ((r_x1 * r_y1) * float (scaling));
+ const int D = static_cast<int> ((r_x_1 * r_y1) * float (scaling));
+ const int r_x_1_i = static_cast<int> (r_x_1 * float (scaling));
+ const int r_y_1_i = static_cast<int> (r_y_1 * float (scaling));
+ const int r_x1_i = static_cast<int> (r_x1 * float (scaling));
+ const int r_y1_i = static_cast<int> (r_y1 * float (scaling));
+
+ if (dx + dy > 2)
+ {
+ // now the calculation:
+
+ //+const unsigned char* ptr = static_cast<const unsigned char*> (&image.points[0].r) + x_left + imagecols * y_top;
+ const unsigned char* ptr = static_cast<const unsigned char*>(&image[0]) + x_left + imagecols * y_top;
+
+ // first the corners:
+ ret_val = A * int (*ptr);
+
+ //+ptr += (dx + 1) * sizeof (PointInT);
+ ptr += dx + 1;
+
+ ret_val += B * int (*ptr);
+
+ //+ptr += (dy * imagecols + 1) * sizeof (PointInT);
+ ptr += dy * imagecols + 1;
+
+ ret_val += C * int (*ptr);
+
+ //+ptr -= (dx + 1) * sizeof (PointInT);
+ ptr -= dx + 1;
+
+ ret_val += D * int (*ptr);
+
+ // next the edges:
+ //+int* ptr_integral;// = static_cast<int*> (integral.data) + x_left + integralcols * y_top + 1;
+ const int* ptr_integral = static_cast<const int*> (&integral_image[0]) + x_left + integralcols * y_top + 1;
+
+ // find a simple path through the different surface corners
+ const int tmp1 = (*ptr_integral);
+ ptr_integral += dx;
+ const int tmp2 = (*ptr_integral);
+ ptr_integral += integralcols;
+ const int tmp3 = (*ptr_integral);
+ ptr_integral++;
+ const int tmp4 = (*ptr_integral);
+ ptr_integral += dy * integralcols;
+ const int tmp5 = (*ptr_integral);
+ ptr_integral--;
+ const int tmp6 = (*ptr_integral);
+ ptr_integral += integralcols;
+ const int tmp7 = (*ptr_integral);
+ ptr_integral -= dx;
+ const int tmp8 = (*ptr_integral);
+ ptr_integral -= integralcols;
+ const int tmp9 = (*ptr_integral);
+ ptr_integral--;
+ const int tmp10 = (*ptr_integral);
+ ptr_integral -= dy * integralcols;
+ const int tmp11 = (*ptr_integral);
+ ptr_integral++;
+ const int tmp12 = (*ptr_integral);
+
+ // assign the weighted surface integrals:
+ const int upper = (tmp3 -tmp2 +tmp1 -tmp12) * r_y_1_i;
+ const int middle = (tmp6 -tmp3 +tmp12 -tmp9) * scaling;
+ const int left = (tmp9 -tmp12 +tmp11 -tmp10) * r_x_1_i;
+ const int right = (tmp5 -tmp4 +tmp3 -tmp6) * r_x1_i;
+ const int bottom = (tmp7 -tmp6 +tmp9 -tmp8) * r_y1_i;
+
+ return (ret_val + upper + middle + left + right + bottom + scaling2 / 2) / scaling2;
+ }
+
+ // now the calculation:
+
+ //const unsigned char* ptr = static_cast<const unsigned char*> (&image.points[0].r) + x_left + imagecols * y_top;
+ const unsigned char* ptr = static_cast<const unsigned char*>(&image[0]) + x_left + imagecols * y_top;
+
+ // first row:
+ ret_val = A * int (*ptr);
+
+ //+ptr += sizeof (PointInT);
+ ptr++;
+
+ //+const unsigned char* end1 = ptr + (dx * sizeof (PointInT));
+ const unsigned char* end1 = ptr + dx;
+
+ //+for (; ptr < end1; ptr += sizeof (PointInT))
+ for (; ptr < end1; ptr++)
+ ret_val += r_y_1_i * int (*ptr);
+ ret_val += B * int (*ptr);
+
+ // middle ones:
+ //+ptr += (imagecols - dx - 1) * sizeof (PointInT);
+ ptr += imagecols - dx - 1;
+
+ //+const unsigned char* end_j = ptr + (dy * imagecols) * sizeof (PointInT);
+ const unsigned char* end_j = ptr + dy * imagecols;
+
+ //+for (; ptr < end_j; ptr += (imagecols - dx - 1) * sizeof (PointInT))
+ for (; ptr < end_j; ptr += imagecols - dx - 1)
+ {
+ ret_val += r_x_1_i * int (*ptr);
+
+ //+ptr += sizeof (PointInT);
+ ptr++;
+
+ //+const unsigned char* end2 = ptr + (dx * sizeof (PointInT));
+ const unsigned char* end2 = ptr + dx;
+
+ //+for (; ptr < end2; ptr += sizeof (PointInT))
+ for (; ptr < end2; ptr++)
+ ret_val += int (*ptr) * scaling;
+
+ ret_val += r_x1_i * int (*ptr);
+ }
+ // last row:
+ ret_val += D * int (*ptr);
+
+ //+ptr += sizeof (PointInT);
+ ptr++;
+
+ //+const unsigned char* end3 = ptr + (dx * sizeof (PointInT));
+ const unsigned char* end3 = ptr + dx;
+
+ //+for (; ptr<end3; ptr += sizeof (PointInT))
+ for (; ptr<end3; ptr++)
+ ret_val += r_y1_i * int (*ptr);
+
+ ret_val += C * int (*ptr);
+
+ return (ret_val + scaling2 / 2) / scaling2;
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT> bool
+pcl::BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::RoiPredicate (
+ const float min_x, const float min_y,
+ const float max_x, const float max_y, const KeypointT& pt)
+{
+ return ((pt.x < min_x) || (pt.x >= max_x) || (pt.y < min_y) || (pt.y >= max_y));
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT> void
+pcl::BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::compute (
+ PointCloudOutT &output)
+{
+ if (!input_cloud_->isOrganized ())
+ {
+ PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
+ return;
+ }
+
+ // image size
+ const int width = int (input_cloud_->width);
+ const int height = int (input_cloud_->height);
+
+ // destination for intensity data; will be forwarded to BRISK
+ std::vector<unsigned char> image_data (width*height);
+
+ for (size_t i = 0; i < image_data.size (); ++i)
+ image_data[i] = static_cast<unsigned char> (intensity_ ((*input_cloud_)[i]));
+
+ // Remove keypoints very close to the border
+ size_t ksize = keypoints_->points.size ();
+ std::vector<int> kscales; // remember the scale per keypoint
+ kscales.resize (ksize);
+
+ // initialize constants
+ static const float log2 = 0.693147180559945f;
+ static const float lb_scalerange = std::log (scalerange_) / (log2);
+
+ typename std::vector<KeypointT, Eigen::aligned_allocator<KeypointT> >::iterator beginning = keypoints_->points.begin ();
+ std::vector<int>::iterator beginningkscales = kscales.begin ();
+
+ static const float basic_size_06 = basic_size_ * 0.6f;
+ unsigned int basicscale = 0;
+
+ if (!scale_invariance_enabled_)
+ basicscale = std::max (static_cast<int> (float (scales_) / lb_scalerange * (log (1.45f * basic_size_ / (basic_size_06)) / log2) + 0.5f), 0);
+
+ for (size_t k = 0; k < ksize; k++)
+ {
+ unsigned int scale;
+ if (scale_invariance_enabled_)
+ {
+ scale = std::max (static_cast<int> (float (scales_) / lb_scalerange * (log (keypoints_->points[k].size / (basic_size_06)) / log2) + 0.5f), 0);
+ // saturate
+ if (scale >= scales_) scale = scales_ - 1;
+ kscales[k] = scale;
+ }
+ else
+ {
+ scale = basicscale;
+ kscales[k] = scale;
+ }
+
+ const int border = size_list_[scale];
+ const int border_x = width - border;
+ const int border_y = height - border;
+
+ if (RoiPredicate (float (border), float (border), float (border_x), float (border_y), keypoints_->points[k]))
+ {
+ //std::cerr << "remove keypoint" << std::endl;
+ keypoints_->points.erase (beginning + k);
+ kscales.erase (beginningkscales + k);
+ if (k == 0)
+ {
+ beginning = keypoints_->points.begin ();
+ beginningkscales = kscales.begin ();
+ }
+ ksize--;
+ k--;
+ }
+ }
+
+ keypoints_->width = uint32_t (keypoints_->size ());
+ keypoints_->height = 1;
+
+ // first, calculate the integral image over the whole image:
+ // current integral image
+ std::vector<int> integral ((width+1)*(height+1), 0); // the integral image
+
+ for (size_t row_index = 1; row_index < height; ++row_index)
+ {
+ for (size_t col_index = 1; col_index < width; ++col_index)
+ {
+ const size_t index = row_index*width+col_index;
+ const size_t index2 = (row_index)*(width+1)+(col_index);
+
+ integral[index2] = static_cast<int> (image_data[index])
+ - integral[index2-1-(width+1)]
+ + integral[index2-(width+1)]
+ + integral[index2-1];
+ }
+ }
+
+ int* values = new int[points_]; // for temporary use
+
+ // resize the descriptors:
+ //output = zeros (ksize, strings_);
+
+ // now do the extraction for all keypoints:
+
+ // temporary variables containing gray values at sample points:
+ int t1;
+ int t2;
+
+ // the feature orientation
+ int direction0;
+ int direction1;
+
+ output.resize (ksize);
+ //output.width = ksize;
+ //output.height = 1;
+ for (size_t k = 0; k < ksize; k++)
+ {
+ unsigned char* ptr = &output.points[k].descriptor[0];
+
+ int theta;
+ KeypointT &kp = keypoints_->points[k];
+ const int& scale = kscales[k];
+ int shifter = 0;
+ int* pvalues = values;
+ const float& x = float (kp.x);
+ const float& y = float (kp.y);
+ if (true) // kp.angle==-1
+ {
+ if (!rotation_invariance_enabled_)
+ // don't compute the gradient direction, just assign a rotation of 0°
+ theta = 0;
+ else
+ {
+ // get the gray values in the unrotated pattern
+ for (unsigned int i = 0; i < points_; i++)
+ *(pvalues++) = smoothedIntensity (image_data, width, height, integral, x, y, scale, 0, i);
+
+ direction0 = 0;
+ direction1 = 0;
+ // now iterate through the long pairings
+ const BriskLongPair* max = long_pairs_ + no_long_pairs_;
+
+ for (BriskLongPair* iter = long_pairs_; iter < max; ++iter)
+ {
+ t1 = *(values + iter->i);
+ t2 = *(values + iter->j);
+ const int delta_t = (t1 - t2);
+
+ // update the direction:
+ const int tmp0 = delta_t * (iter->weighted_dx) / 1024;
+ const int tmp1 = delta_t * (iter->weighted_dy) / 1024;
+ direction0 += tmp0;
+ direction1 += tmp1;
+ }
+ kp.angle = atan2f (float (direction1), float (direction0)) / float (M_PI) * 180.0f;
+ theta = static_cast<int> ((float (n_rot_) * kp.angle) / (360.0f) + 0.5f);
+ if (theta < 0)
+ theta += n_rot_;
+ if (theta >= int (n_rot_))
+ theta -= n_rot_;
+ }
+ }
+ else
+ {
+ // figure out the direction:
+ //int theta=rotationInvariance*round((_n_rot*atan2(direction.at<int>(0,0),direction.at<int>(1,0)))/(2*M_PI));
+ if (!rotation_invariance_enabled_)
+ theta = 0;
+ else
+ {
+ theta = static_cast<int> (n_rot_ * (kp.angle / (360.0)) + 0.5);
+ if (theta < 0)
+ theta += n_rot_;
+ if (theta >= int (n_rot_))
+ theta -= n_rot_;
+ }
+ }
+
+ // now also extract the stuff for the actual direction:
+ // let us compute the smoothed values
+ shifter = 0;
+
+ //unsigned int mean=0;
+ pvalues = values;
+ // get the gray values in the rotated pattern
+ for (unsigned int i = 0; i < points_; i++)
+ *(pvalues++) = smoothedIntensity (image_data, width, height, integral, x, y, scale, theta, i);
+
+#ifdef __GNUC__
+ typedef uint32_t __attribute__ ((__may_alias__)) UINT32_ALIAS;
+#endif
+#ifdef _MSC_VER
+ // Todo: find the equivalent to may_alias
+ #define UCHAR_ALIAS uint32_t //__declspec(noalias)
+ #define UINT32_ALIAS uint32_t //__declspec(noalias)
+#endif
+
+ // now iterate through all the pairings
+ UINT32_ALIAS* ptr2 = reinterpret_cast<UINT32_ALIAS*> (ptr);
+ const BriskShortPair* max = short_pairs_ + no_short_pairs_;
+
+ for (BriskShortPair* iter = short_pairs_; iter < max; ++iter)
+ {
+ t1 = *(values + iter->i);
+ t2 = *(values + iter->j);
+
+ if (t1 > t2)
+ *ptr2 |= ((1) << shifter);
+
+ // else already initialized with zero
+ // take care of the iterators:
+ ++shifter;
+
+ if (shifter == 32)
+ {
+ shifter = 0;
+ ++ptr2;
+ }
+ }
+
+ //ptr += strings_;
+
+ //// Account for the scale + orientation;
+ //ptr += sizeof (output.points[0].scale);
+ //ptr += sizeof (output.points[0].orientation);
+ }
+
+ // we do not change the denseness
+ output.width = int (output.points.size ());
+ output.height = 1;
+ output.is_dense = true;
+
+ // clean-up
+ delete [] values;
+}
+
+
+#endif //#ifndef PCL_FEATURES_IMPL_BRISK_2D_HPP_
+
pcl::transformPointCloudWithNormals (grid, grid, transformPC);
- //fill spatial data vector
- kiss_fft_scalar * spatial_data = new kiss_fft_scalar[nbins];
+ //fill spatial data vector and the zero-initialize or "value-initialize" an array on c++,
+ // the initialization is made with () after the [nbins]
+ kiss_fft_scalar * spatial_data = new kiss_fft_scalar[nbins]();
+
+
float sum_w = 0, w = 0;
int bin = 0;
for (size_t i = 0; i < grid.points.size (); ++i)
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2009-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_FEATURES_IMPL_GRSD_H_
+#define PCL_FEATURES_IMPL_GRSD_H_
+
+#include <pcl/features/grsd.h>
+///////// STATIC /////////
+template <typename PointInT, typename PointNT, typename PointOutT> int
+pcl::GRSDEstimation<PointInT, PointNT, PointOutT>::getSimpleType (float min_radius, float max_radius,
+ double min_radius_plane,
+ double max_radius_noise,
+ double min_radius_cylinder,
+ double max_min_radius_diff)
+{
+ if (min_radius > min_radius_plane)
+ return (1); // plane
+ else if (max_radius > min_radius_cylinder)
+ return (2); // cylinder (rim)
+ else if (min_radius < max_radius_noise)
+ return (0); // noise/corner
+ else if (max_radius - min_radius < max_min_radius_diff)
+ return (3); // sphere/corner
+ else
+ return (4); // edge
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointNT, typename PointOutT> void
+pcl::GRSDEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
+{
+ // Check if search_radius_ was set
+ if (width_ < 0)
+ {
+ PCL_ERROR ("[pcl::%s::computeFeature] A voxel cell width needs to be set!\n", getClassName ().c_str ());
+ output.width = output.height = 0;
+ output.points.clear ();
+ return;
+ }
+
+ // Create the voxel grid
+ PointCloudInPtr cloud_downsampled (new PointCloudIn());
+ pcl::VoxelGrid<PointInT> grid;
+ grid.setLeafSize (width_, width_, width_);
+ grid.setInputCloud (input_);
+ grid.setSaveLeafLayout (true); // TODO maybe avoid this using nearest neighbor search
+ grid.filter (*cloud_downsampled);
+
+ // Compute RSD
+ pcl::PointCloud<pcl::PrincipalRadiiRSD>::Ptr radii (new pcl::PointCloud<pcl::PrincipalRadiiRSD>());
+ pcl::RSDEstimation<PointInT, PointNT, pcl::PrincipalRadiiRSD> rsd;
+ rsd.setInputCloud (cloud_downsampled);
+ rsd.setSearchSurface (input_);
+ rsd.setInputNormals (normals_);
+ rsd.setRadiusSearch (std::max (search_radius_, std::sqrt (3.0) * width_ / 2));
+// pcl::KdTree<PointInT>::Ptr tree = boost::make_shared<pcl::KdTreeFLANN<PointInT> >();
+// tree->setInputCloud(input_);
+// rsd.setSearchMethod(tree);
+ rsd.compute (*radii);
+
+ // Save the type of each point
+ int NR_CLASS = 5; // TODO make this nicer
+ std::vector<int> types (radii->points.size ());
+ for (size_t idx = 0; idx < radii->points.size (); ++idx)
+ types[idx] = getSimpleType (radii->points[idx].r_min, radii->points[idx].r_max);
+
+ // Get the transitions between surface types between neighbors of occupied cells
+ Eigen::MatrixXi transition_matrix = Eigen::MatrixXi::Zero (NR_CLASS + 1, NR_CLASS + 1);
+ for (size_t idx = 0; idx < cloud_downsampled->points.size (); ++idx)
+ {
+ int source_type = types[idx];
+ std::vector<int> neighbors = grid.getNeighborCentroidIndices (cloud_downsampled->points[idx], relative_coordinates_all_);
+ for (unsigned id_n = 0; id_n < neighbors.size (); id_n++)
+ {
+ int neighbor_type;
+ if (neighbors[id_n] == -1) // empty
+ neighbor_type = NR_CLASS;
+ else
+ neighbor_type = types[neighbors[id_n]];
+ transition_matrix (source_type, neighbor_type)++;
+ }
+ }
+
+ // Save feature values
+ output.points.resize (1);
+ output.height = output.width = 1;
+ int nrf = 0;
+ for (int i = 0; i < NR_CLASS + 1; i++)
+ for (int j = i; j < NR_CLASS + 1; j++)
+ output.points[0].histogram[nrf++] = transition_matrix (i, j) + transition_matrix (j, i);
+}
+
+#define PCL_INSTANTIATE_GRSDEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::GRSDEstimation<T,NT,OutT>;
+
+#endif /* PCL_FEATURES_IMPL_GRSD_H_ */
float minor_value = 0.0f;
computeEigenVectors (covariance_matrix, major_axis, middle_axis, minor_axis, major_value, middle_value, minor_value);
- float major = abs (major_axis.dot (normal_vector));
- float middle = abs (middle_axis.dot (normal_vector));
- float minor = abs (minor_axis.dot (normal_vector));
+ float major = std::abs (major_axis.dot (normal_vector));
+ float middle = std::abs (middle_axis.dot (normal_vector));
+ float minor = std::abs (minor_axis.dot (normal_vector));
float eccentricity = 0.0f;
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
- if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
+ if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 ||
+ !computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature))
{
output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();
continue;
}
- computePointNormal (*surface_, nn_indices,
- output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature);
-
flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
- this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
+ this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 ||
+ !computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature))
{
output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();
continue;
}
- computePointNormal (*surface_, nn_indices,
- output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature);
-
flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
+#define PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
+
+#include <pcl/2d/edge.h>
+#include <pcl/features/organized_edge_detection.h>
+#include <pcl/console/print.h>
+#include <pcl/console/time.h>
+
+/**
+ * Directions: 1 2 3
+ * 0 x 4
+ * 7 6 5
+ * e.g. direction y means we came from pixel with label y to the center pixel x
+ */
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointT, typename PointLT> void
+pcl::OrganizedEdgeBase<PointT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
+{
+ pcl::Label invalid_pt;
+ invalid_pt.label = unsigned (0);
+ labels.points.resize (input_->points.size (), invalid_pt);
+ labels.width = input_->width;
+ labels.height = input_->height;
+
+ extractEdges (labels);
+
+ assignLabelIndices (labels, label_indices);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointT, typename PointLT> void
+pcl::OrganizedEdgeBase<PointT, PointLT>::assignLabelIndices (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
+{
+ const unsigned invalid_label = unsigned (0);
+ label_indices.resize (num_of_edgetype_);
+ for (unsigned idx = 0; idx < input_->points.size (); idx++)
+ {
+ if (labels[idx].label != invalid_label)
+ {
+ for (int edge_type = 0; edge_type < num_of_edgetype_; edge_type++)
+ {
+ if ((labels[idx].label >> edge_type) & 1)
+ label_indices[edge_type].indices.push_back (idx);
+ }
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointT, typename PointLT> void
+pcl::OrganizedEdgeBase<PointT, PointLT>::extractEdges (pcl::PointCloud<PointLT>& labels) const
+{
+ if ((detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY) || (detecting_edge_types_ & EDGELABEL_OCCLUDING) || (detecting_edge_types_ & EDGELABEL_OCCLUDED))
+ {
+ // Fill lookup table for next points to visit
+ const int num_of_ngbr = 8;
+ Neighbor directions [num_of_ngbr] = {Neighbor(-1, 0, -1),
+ Neighbor(-1, -1, -labels.width - 1),
+ Neighbor( 0, -1, -labels.width ),
+ Neighbor( 1, -1, -labels.width + 1),
+ Neighbor( 1, 0, 1),
+ Neighbor( 1, 1, labels.width + 1),
+ Neighbor( 0, 1, labels.width ),
+ Neighbor(-1, 1, labels.width - 1)};
+
+ for (int row = 1; row < int(input_->height) - 1; row++)
+ {
+ for (int col = 1; col < int(input_->width) - 1; col++)
+ {
+ int curr_idx = row*int(input_->width) + col;
+ if (!pcl_isfinite (input_->points[curr_idx].z))
+ continue;
+
+ float curr_depth = fabsf (input_->points[curr_idx].z);
+
+ // Calculate depth distances between current point and neighboring points
+ std::vector<float> nghr_dist;
+ nghr_dist.resize (8);
+ bool found_invalid_neighbor = false;
+ for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
+ {
+ int nghr_idx = curr_idx + directions[d_idx].d_index;
+ assert (nghr_idx >= 0 && nghr_idx < input_->points.size ());
+ if (!pcl_isfinite (input_->points[nghr_idx].z))
+ {
+ found_invalid_neighbor = true;
+ break;
+ }
+ nghr_dist[d_idx] = curr_depth - fabsf (input_->points[nghr_idx].z);
+ }
+
+ if (!found_invalid_neighbor)
+ {
+ // Every neighboring points are valid
+ std::vector<float>::iterator min_itr = std::min_element (nghr_dist.begin (), nghr_dist.end ());
+ std::vector<float>::iterator max_itr = std::max_element (nghr_dist.begin (), nghr_dist.end ());
+ float nghr_dist_min = *min_itr;
+ float nghr_dist_max = *max_itr;
+ float dist_dominant = fabsf (nghr_dist_min) > fabsf (nghr_dist_max) ? nghr_dist_min : nghr_dist_max;
+ if (fabsf (dist_dominant) > th_depth_discon_*fabsf (curr_depth))
+ {
+ // Found a depth discontinuity
+ if (dist_dominant > 0.f)
+ {
+ if (detecting_edge_types_ & EDGELABEL_OCCLUDED)
+ labels[curr_idx].label |= EDGELABEL_OCCLUDED;
+ }
+ else
+ {
+ if (detecting_edge_types_ & EDGELABEL_OCCLUDING)
+ labels[curr_idx].label |= EDGELABEL_OCCLUDING;
+ }
+ }
+ }
+ else
+ {
+ // Some neighboring points are not valid (nan points)
+ // Search for corresponding point across invalid points
+ // Search direction is determined by nan point locations with respect to current point
+ int dx = 0;
+ int dy = 0;
+ int num_of_invalid_pt = 0;
+ for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
+ {
+ int nghr_idx = curr_idx + directions[d_idx].d_index;
+ assert (nghr_idx >= 0 && nghr_idx < input_->points.size ());
+ if (!pcl_isfinite (input_->points[nghr_idx].z))
+ {
+ dx += directions[d_idx].d_x;
+ dy += directions[d_idx].d_y;
+ num_of_invalid_pt++;
+ }
+ }
+
+ // Search directions
+ assert (num_of_invalid_pt > 0);
+ float f_dx = static_cast<float> (dx) / static_cast<float> (num_of_invalid_pt);
+ float f_dy = static_cast<float> (dy) / static_cast<float> (num_of_invalid_pt);
+
+ // Search for corresponding point across invalid points
+ float corr_depth = std::numeric_limits<float>::quiet_NaN ();
+ for (int s_idx = 1; s_idx < max_search_neighbors_; s_idx++)
+ {
+ int s_row = row + static_cast<int> (std::floor (f_dy*static_cast<float> (s_idx)));
+ int s_col = col + static_cast<int> (std::floor (f_dx*static_cast<float> (s_idx)));
+
+ if (s_row < 0 || s_row >= int(input_->height) || s_col < 0 || s_col >= int(input_->width))
+ break;
+
+ if (pcl_isfinite (input_->points[s_row*int(input_->width)+s_col].z))
+ {
+ corr_depth = fabsf (input_->points[s_row*int(input_->width)+s_col].z);
+ break;
+ }
+ }
+
+ if (!pcl_isnan (corr_depth))
+ {
+ // Found a corresponding point
+ float dist = curr_depth - corr_depth;
+ if (fabsf (dist) > th_depth_discon_*fabsf (curr_depth))
+ {
+ // Found a depth discontinuity
+ if (dist > 0.f)
+ {
+ if (detecting_edge_types_ & EDGELABEL_OCCLUDED)
+ labels[curr_idx].label |= EDGELABEL_OCCLUDED;
+ }
+ else
+ {
+ if (detecting_edge_types_ & EDGELABEL_OCCLUDING)
+ labels[curr_idx].label |= EDGELABEL_OCCLUDING;
+ }
+ }
+ }
+ else
+ {
+ // Not found a corresponding point, just nan boundary edge
+ if (detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY)
+ labels[curr_idx].label |= EDGELABEL_NAN_BOUNDARY;
+ }
+ }
+ }
+ }
+ }
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointT, typename PointLT> void
+pcl::OrganizedEdgeFromRGB<PointT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
+{
+ pcl::Label invalid_pt;
+ invalid_pt.label = unsigned (0);
+ labels.points.resize (input_->points.size (), invalid_pt);
+ labels.width = input_->width;
+ labels.height = input_->height;
+
+ OrganizedEdgeBase<PointT, PointLT>::extractEdges (labels);
+ extractEdges (labels);
+
+ this->assignLabelIndices (labels, label_indices);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointT, typename PointLT> void
+pcl::OrganizedEdgeFromRGB<PointT, PointLT>::extractEdges (pcl::PointCloud<PointLT>& labels) const
+{
+ if ((detecting_edge_types_ & EDGELABEL_RGB_CANNY))
+ {
+ pcl::PointCloud<PointXYZI>::Ptr gray (new pcl::PointCloud<PointXYZI>);
+ gray->width = input_->width;
+ gray->height = input_->height;
+ gray->resize (input_->height*input_->width);
+
+ for (size_t i = 0; i < input_->size (); ++i)
+ (*gray)[i].intensity = float (((*input_)[i].r + (*input_)[i].g + (*input_)[i].b) / 3);
+
+ pcl::PointCloud<pcl::PointXYZIEdge> img_edge_rgb;
+ pcl::Edge<PointXYZI, pcl::PointXYZIEdge> edge;
+ edge.setInputCloud (gray);
+ edge.setHysteresisThresholdLow (th_rgb_canny_low_);
+ edge.setHysteresisThresholdHigh (th_rgb_canny_high_);
+ edge.detectEdgeCanny (img_edge_rgb);
+
+ for (uint32_t row=0; row<labels.height; row++)
+ {
+ for (uint32_t col=0; col<labels.width; col++)
+ {
+ if (img_edge_rgb (col, row).magnitude == 255.f)
+ labels[row * labels.width + col].label |= EDGELABEL_RGB_CANNY;
+ }
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointT, typename PointNT, typename PointLT> void
+pcl::OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
+{
+ pcl::Label invalid_pt;
+ invalid_pt.label = unsigned (0);
+ labels.points.resize (input_->points.size (), invalid_pt);
+ labels.width = input_->width;
+ labels.height = input_->height;
+
+ OrganizedEdgeBase<PointT, PointLT>::extractEdges (labels);
+ extractEdges (labels);
+
+ this->assignLabelIndices (labels, label_indices);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointT, typename PointNT, typename PointLT> void
+pcl::OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::extractEdges (pcl::PointCloud<PointLT>& labels) const
+{
+ if ((detecting_edge_types_ & EDGELABEL_HIGH_CURVATURE))
+ {
+
+ pcl::PointCloud<PointXYZI> nx, ny;
+ nx.width = normals_->width;
+ nx.height = normals_->height;
+ nx.resize (normals_->height*normals_->width);
+
+ ny.width = normals_->width;
+ ny.height = normals_->height;
+ ny.resize (normals_->height*normals_->width);
+
+ for (uint32_t row=0; row<normals_->height; row++)
+ {
+ for (uint32_t col=0; col<normals_->width; col++)
+ {
+ nx (col, row).intensity = normals_->points[row*normals_->width + col].normal_x;
+ ny (col, row).intensity = normals_->points[row*normals_->width + col].normal_y;
+ }
+ }
+
+ pcl::PointCloud<pcl::PointXYZIEdge> img_edge;
+ pcl::Edge<PointXYZI, pcl::PointXYZIEdge> edge;
+ edge.setHysteresisThresholdLow (th_hc_canny_low_);
+ edge.setHysteresisThresholdHigh (th_hc_canny_high_);
+ edge.canny (nx, ny, img_edge);
+
+ for (uint32_t row=0; row<labels.height; row++)
+ {
+ for (uint32_t col=0; col<labels.width; col++)
+ {
+ if (img_edge (col, row).magnitude == 255.f)
+ labels[row * labels.width + col].label |= EDGELABEL_HIGH_CURVATURE;
+ }
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+template<typename PointT, typename PointNT, typename PointLT> void
+pcl::OrganizedEdgeFromRGBNormals<PointT, PointNT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
+{
+ pcl::Label invalid_pt;
+ invalid_pt.label = unsigned (0);
+ labels.points.resize (input_->points.size (), invalid_pt);
+ labels.width = input_->width;
+ labels.height = input_->height;
+
+ OrganizedEdgeBase<PointT, PointLT>::extractEdges (labels);
+ OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::extractEdges (labels);
+ OrganizedEdgeFromRGB<PointT, PointLT>::extractEdges (labels);
+
+ this->assignLabelIndices (labels, label_indices);
+}
+
+#define PCL_INSTANTIATE_OrganizedEdgeBase(T,LT) template class PCL_EXPORTS pcl::OrganizedEdgeBase<T,LT>;
+#define PCL_INSTANTIATE_OrganizedEdgeFromRGB(T,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromRGB<T,LT>;
+#define PCL_INSTANTIATE_OrganizedEdgeFromNormals(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromNormals<T,NT,LT>;
+#define PCL_INSTANTIATE_OrganizedEdgeFromRGBNormals(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromRGBNormals<T,NT,LT>;
+
+#endif //#ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
key = std::pair<int, int> (p1, p2);
// Check to see if we already estimated this pair in the global hashmap
- std::map<std::pair<int, int>, Eigen::Vector4f, std::less<std::pair<int, int> >, Eigen::aligned_allocator<Eigen::Vector4f> >::iterator fm_it = feature_map_.find (key);
+ std::map<std::pair<int, int>, Eigen::Vector4f, std::less<std::pair<int, int> >, Eigen::aligned_allocator<std::pair<const std::pair<int, int>, Eigen::Vector4f> > >::iterator fm_it = feature_map_.find (key);
if (fm_it != feature_map_.end ())
pfh_tuple_ = fm_it->second;
else
// Check to see if we need to remove an element due to exceeding max_size
if (key_list_.size () > max_cache_size_)
{
- // Remove the last element.
- feature_map_.erase (key_list_.back ());
+ // Remove the oldest element.
+ feature_map_.erase (key_list_.front ());
key_list_.pop ();
}
}
pfhrgb_histogram.setZero ();
// Factorization constant
- float hist_incr = 100.0f / static_cast<float> (indices.size () * indices.size () - 1);
+ float hist_incr = 100.0f / static_cast<float> (indices.size () * (indices.size () - 1) / 2);
// Iterate over all the points in the neighborhood
for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
number_of_rotations_ (3),
support_radius_ (1.0f),
sqr_support_radius_ (1.0f),
- step_ (30.0f),
+ step_ (22.5f),
triangles_ (0),
triangles_of_the_point_ (0)
{
float norm = 0.0f;
for (unsigned int i_dim = 0; i_dim < feature_size; i_dim++)
- norm += feature[i_dim];
- if (abs (norm) < std::numeric_limits <float>::epsilon ())
- norm = 1.0f / norm;
- else
+ norm += std::abs (feature[i_dim]);
+ if (norm < std::numeric_limits <float>::epsilon ())
norm = 1.0f;
+ else
+ norm = 1.0f / norm;
for (unsigned int i_dim = 0; i_dim < feature_size; i_dim++)
output.points[i_point].histogram[i_dim] = feature[i_dim] * norm;
{
const unsigned int number_of_triangles = static_cast <unsigned int> (local_triangles.size ());
- std::vector <Eigen::Matrix3f> scatter_matrices (number_of_triangles);
+ std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > scatter_matrices (number_of_triangles);
std::vector <float> triangle_area (number_of_triangles);
std::vector <float> distance_weight (number_of_triangles);
scatter_matrices[i_triangle] = coeff * curr_scatter_matrix;
}
- if (abs (total_area) < std::numeric_limits <float>::epsilon ())
+ if (std::abs (total_area) < std::numeric_limits <float>::epsilon ())
total_area = 1.0f / total_area;
else
total_area = 1.0f;
#define PCL_INSTANTIATE_RSDEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::RSDEstimation<T,NT,OutT>;
-#endif // PCL_FEATURES_IMPL_VFH_H_
+#endif // PCL_FEATURES_IMPL_RSD_H_
template <typename PointInT, typename PointOutT, typename PointRFT> void
pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computeFeature (PointCloudOut &output)
{
- assert (descriptor_length_ == 1980);
+ assert (descriptor_length_ == 1960);
output.is_dense = true;
* \f]
* \ingroup features
*/
- template <typename PointT> inline void
+ template <typename PointT> inline bool
computePointNormal (const pcl::PointCloud<PointT> &cloud,
Eigen::Vector4f &plane_parameters, float &curvature)
{
{
plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
curvature = std::numeric_limits<float>::quiet_NaN ();
- return;
+ return false;
}
// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature);
+ return true;
}
/** \brief Compute the Least-Squares plane fit for a given set of points, using their indices,
* \f]
* \ingroup features
*/
- template <typename PointT> inline void
+ template <typename PointT> inline bool
computePointNormal (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
Eigen::Vector4f &plane_parameters, float &curvature)
{
{
plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
curvature = std::numeric_limits<float>::quiet_NaN ();
- return;
+ return false;
}
// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature);
+ return true;
}
/** \brief Flip (in place) the estimated normal of a point towards a given viewpoint
* \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2)
* \f]
*/
- inline void
+ inline bool
computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices,
Eigen::Vector4f &plane_parameters, float &curvature)
{
{
plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
curvature = std::numeric_limits<float>::quiet_NaN ();
- return;
+ return false;
}
// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix_, xyz_centroid_, plane_parameters, curvature);
+ return true;
}
/** \brief Compute the Least-Squares plane fit for a given set of points, using their indices,
* \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2)
* \f]
*/
- inline void
+ inline bool
computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices,
float &nx, float &ny, float &nz, float &curvature)
{
computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix_, xyz_centroid_) == 0)
{
nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
- return;
+ return false;
}
// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix_, nx, ny, nz, curvature);
+ return true;
}
/** \brief Provide a pointer to the input dataset
protected:
/** \brief Estimate normals for all points given in <setInputCloud (), setIndices ()> using the surface in
* setSearchSurface () and the spatial locator in setSearchMethod ()
- * \note In situations where not enough neighbors are found, the normal and curvature values are set to -1.
+ * \note In situations where not enough neighbors are found, the normal and curvature values are set to NaN.
* \param output the resultant point cloud model dataset that contains surface normals and curvatures
*/
void
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_FEATURES_ORGANIZED_EDGE_DETECTION_H_
+#define PCL_FEATURES_ORGANIZED_EDGE_DETECTION_H_
+
+#include <pcl/pcl_base.h>
+#include <pcl/PointIndices.h>
+
+namespace pcl
+{
+ /** \brief OrganizedEdgeBase, OrganizedEdgeFromRGB, OrganizedEdgeFromNormals,
+ * and OrganizedEdgeFromRGBNormals find 3D edges from an organized point
+ * cloud data. Given an organized point cloud, they will output a PointCloud
+ * of edge labels and a vector of PointIndices.
+ * OrganizedEdgeBase accepts PCL_XYZ_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, and EDGELABEL_OCCLUDED.
+ * OrganizedEdgeFromRGB accepts PCL_RGB_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, EDGELABEL_OCCLUDED, and EDGELABEL_RGB_CANNY.
+ * OrganizedEdgeFromNormals accepts PCL_XYZ_POINT_TYPES with PCL_NORMAL_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, EDGELABEL_OCCLUDED, and EDGELABEL_HIGH_CURVATURE.
+ * OrganizedEdgeFromRGBNormals accepts PCL_RGB_POINT_TYPES with PCL_NORMAL_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, EDGELABEL_OCCLUDED, EDGELABEL_HIGH_CURVATURE, and EDGELABEL_RGB_CANNY.
+ *
+ * \author Changhyun Choi
+ */
+ template <typename PointT, typename PointLT>
+ class OrganizedEdgeBase : public PCLBase<PointT>
+ {
+ typedef typename pcl::PointCloud<PointT> PointCloud;
+ typedef typename PointCloud::Ptr PointCloudPtr;
+ typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+ typedef typename pcl::PointCloud<PointLT> PointCloudL;
+ typedef typename PointCloudL::Ptr PointCloudLPtr;
+ typedef typename PointCloudL::ConstPtr PointCloudLConstPtr;
+
+ public:
+ typedef boost::shared_ptr<OrganizedEdgeBase<PointT, PointLT> > Ptr;
+ typedef boost::shared_ptr<const OrganizedEdgeBase<PointT, PointLT> > ConstPtr;
+ using PCLBase<PointT>::input_;
+ using PCLBase<PointT>::indices_;
+ using PCLBase<PointT>::initCompute;
+ using PCLBase<PointT>::deinitCompute;
+
+ /** \brief Constructor for OrganizedEdgeBase */
+ OrganizedEdgeBase ()
+ : th_depth_discon_ (0.02f)
+ , max_search_neighbors_ (50)
+ , detecting_edge_types_ (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED)
+ {
+ }
+
+ /** \brief Destructor for OrganizedEdgeBase */
+ virtual
+ ~OrganizedEdgeBase ()
+ {
+ }
+
+ /** \brief Perform the 3D edge detection (edges from depth discontinuities)
+ * \param[out] labels a PointCloud of edge labels
+ * \param[out] label_indices a vector of PointIndices corresponding to each edge label
+ */
+ void
+ compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const;
+
+ /** \brief Set the tolerance in meters for difference in depth values between neighboring points. */
+ inline void
+ setDepthDisconThreshold (const float th)
+ {
+ th_depth_discon_ = th;
+ }
+
+ /** \brief Get the tolerance in meters for difference in depth values between neighboring points. */
+ inline float
+ getDepthDisconThreshold () const
+ {
+ return (th_depth_discon_);
+ }
+
+ /** \brief Set the max search distance for deciding occluding and occluded edges. */
+ inline void
+ setMaxSearchNeighbors (const int max_dist)
+ {
+ max_search_neighbors_ = max_dist;
+ }
+
+ /** \brief Get the max search distance for deciding occluding and occluded edges. */
+ inline int
+ getMaxSearchNeighbors () const
+ {
+ return (max_search_neighbors_);
+ }
+
+ /** \brief Set the detecting edge types. */
+ inline void
+ setEdgeType (int edge_types)
+ {
+ detecting_edge_types_ = edge_types;
+ }
+
+ /** \brief Get the detecting edge types. */
+ inline int
+ getEdgeType () const
+ {
+ return detecting_edge_types_;
+ }
+
+ enum {EDGELABEL_NAN_BOUNDARY=1, EDGELABEL_OCCLUDING=2, EDGELABEL_OCCLUDED=4, EDGELABEL_HIGH_CURVATURE=8, EDGELABEL_RGB_CANNY=16};
+ static const int num_of_edgetype_ = 5;
+
+ protected:
+ /** \brief Perform the 3D edge detection (edges from depth discontinuities) and assign point indices for each edge label
+ * \param[out] labels a PointCloud of edge labels
+ */
+ void
+ extractEdges (pcl::PointCloud<PointLT>& labels) const;
+
+ /** \brief Assign point indices for each edge label
+ * \param[out] labels a PointCloud of edge labels
+ * \param[out] label_indices a vector of PointIndices corresponding to each edge label
+ */
+ void
+ assignLabelIndices (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const;
+
+ struct Neighbor
+ {
+ Neighbor (int dx, int dy, int didx)
+ : d_x (dx)
+ , d_y (dy)
+ , d_index (didx)
+ {}
+
+ int d_x;
+ int d_y;
+ int d_index; // = dy * width + dx: pre-calculated
+ };
+
+ /** \brief The tolerance in meters for difference in depth values between neighboring points
+ * (The value is set for 1 meter and is adapted with respect to depth value linearly.
+ * (e.g. 2.0*th_depth_discon_ in 2 meter depth))
+ */
+ float th_depth_discon_;
+
+ /** \brief The max search distance for deciding occluding and occluded edges */
+ int max_search_neighbors_;
+
+ /** \brief The bit encoded value that represents edge types to detect */
+ int detecting_edge_types_;
+ };
+
+ template <typename PointT, typename PointLT>
+ class OrganizedEdgeFromRGB : virtual public OrganizedEdgeBase<PointT, PointLT>
+ {
+ typedef typename pcl::PointCloud<PointT> PointCloud;
+ typedef typename PointCloud::Ptr PointCloudPtr;
+ typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+ typedef typename pcl::PointCloud<PointLT> PointCloudL;
+ typedef typename PointCloudL::Ptr PointCloudLPtr;
+ typedef typename PointCloudL::ConstPtr PointCloudLConstPtr;
+
+ public:
+ using OrganizedEdgeBase<PointT, PointLT>::input_;
+ using OrganizedEdgeBase<PointT, PointLT>::indices_;
+ using OrganizedEdgeBase<PointT, PointLT>::initCompute;
+ using OrganizedEdgeBase<PointT, PointLT>::deinitCompute;
+ using OrganizedEdgeBase<PointT, PointLT>::detecting_edge_types_;
+ using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_NAN_BOUNDARY;
+ using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_OCCLUDING;
+ using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_OCCLUDED;
+ using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_RGB_CANNY;
+
+ /** \brief Constructor for OrganizedEdgeFromRGB */
+ OrganizedEdgeFromRGB ()
+ : OrganizedEdgeBase<PointT, PointLT> ()
+ , th_rgb_canny_low_ (40.0)
+ , th_rgb_canny_high_ (100.0)
+ {
+ this->setEdgeType (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED | EDGELABEL_RGB_CANNY);
+ }
+
+ /** \brief Destructor for OrganizedEdgeFromRGB */
+ virtual
+ ~OrganizedEdgeFromRGB ()
+ {
+ }
+
+ /** \brief Perform the 3D edge detection (edges from depth discontinuities and RGB Canny edge) and assign point indices for each edge label
+ * \param[out] labels a PointCloud of edge labels
+ * \param[out] label_indices a vector of PointIndices corresponding to each edge label
+ */
+ void
+ compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const;
+
+ /** \brief Set the low threshold value for RGB Canny edge detection */
+ inline void
+ setRGBCannyLowThreshold (const float th)
+ {
+ th_rgb_canny_low_ = th;
+ }
+
+ /** \brief Get the low threshold value for RGB Canny edge detection */
+ inline float
+ getRGBCannyLowThreshold () const
+ {
+ return (th_rgb_canny_low_);
+ }
+
+ /** \brief Set the high threshold value for RGB Canny edge detection */
+ inline void
+ setRGBCannyHighThreshold (const float th)
+ {
+ th_rgb_canny_high_ = th;
+ }
+
+ /** \brief Get the high threshold value for RGB Canny edge detection */
+ inline float
+ getRGBCannyHighThreshold () const
+ {
+ return (th_rgb_canny_high_);
+ }
+
+ protected:
+ /** \brief Perform the 3D edge detection (edges from depth discontinuities and RGB Canny edge)
+ * \param[out] labels a PointCloud of edge labels
+ */
+ void
+ extractEdges (pcl::PointCloud<PointLT>& labels) const;
+
+ /** \brief The low threshold value for RGB Canny edge detection (default: 40.0) */
+ float th_rgb_canny_low_;
+
+ /** \brief The high threshold value for RGB Canny edge detection (default: 100.0) */
+ float th_rgb_canny_high_;
+ };
+
+ template <typename PointT, typename PointNT, typename PointLT>
+ class OrganizedEdgeFromNormals : virtual public OrganizedEdgeBase<PointT, PointLT>
+ {
+ typedef typename pcl::PointCloud<PointT> PointCloud;
+ typedef typename PointCloud::Ptr PointCloudPtr;
+ typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+ typedef typename pcl::PointCloud<PointNT> PointCloudN;
+ typedef typename PointCloudN::Ptr PointCloudNPtr;
+ typedef typename PointCloudN::ConstPtr PointCloudNConstPtr;
+
+ typedef typename pcl::PointCloud<PointLT> PointCloudL;
+ typedef typename PointCloudL::Ptr PointCloudLPtr;
+ typedef typename PointCloudL::ConstPtr PointCloudLConstPtr;
+
+ public:
+ using OrganizedEdgeBase<PointT, PointLT>::input_;
+ using OrganizedEdgeBase<PointT, PointLT>::indices_;
+ using OrganizedEdgeBase<PointT, PointLT>::initCompute;
+ using OrganizedEdgeBase<PointT, PointLT>::deinitCompute;
+ using OrganizedEdgeBase<PointT, PointLT>::detecting_edge_types_;
+ using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_NAN_BOUNDARY;
+ using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_OCCLUDING;
+ using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_OCCLUDED;
+ using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_HIGH_CURVATURE;
+
+ /** \brief Constructor for OrganizedEdgeFromNormals */
+ OrganizedEdgeFromNormals ()
+ : OrganizedEdgeBase<PointT, PointLT> ()
+ , normals_ ()
+ , th_hc_canny_low_ (0.4f)
+ , th_hc_canny_high_ (1.1f)
+ {
+ this->setEdgeType (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED | EDGELABEL_HIGH_CURVATURE);
+ }
+
+ /** \brief Destructor for OrganizedEdgeFromNormals */
+ virtual
+ ~OrganizedEdgeFromNormals ()
+ {
+ }
+
+ /** \brief Perform the 3D edge detection (edges from depth discontinuities and high curvature regions) and assign point indices for each edge label
+ * \param[out] labels a PointCloud of edge labels
+ * \param[out] label_indices a vector of PointIndices corresponding to each edge label
+ */
+ void
+ compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const;
+
+ /** \brief Provide a pointer to the input normals.
+ * \param[in] normals the input normal cloud
+ */
+ inline void
+ setInputNormals (const PointCloudNConstPtr &normals)
+ {
+ normals_ = normals;
+ }
+
+ /** \brief Get the input normals. */
+ inline PointCloudNConstPtr
+ getInputNormals () const
+ {
+ return (normals_);
+ }
+
+ /** \brief Set the low threshold value for high curvature Canny edge detection */
+ inline void
+ setHCCannyLowThreshold (const float th)
+ {
+ th_hc_canny_low_ = th;
+ }
+
+ /** \brief Get the low threshold value for high curvature Canny edge detection */
+ inline float
+ getHCCannyLowThreshold () const
+ {
+ return (th_hc_canny_low_);
+ }
+
+ /** \brief Set the high threshold value for high curvature Canny edge detection */
+ inline void
+ setHCCannyHighThreshold (const float th)
+ {
+ th_hc_canny_high_ = th;
+ }
+
+ /** \brief Get the high threshold value for high curvature Canny edge detection */
+ inline float
+ getHCCannyHighThreshold () const
+ {
+ return (th_hc_canny_high_);
+ }
+
+ protected:
+ /** \brief Perform the 3D edge detection (edges from depth discontinuities and high curvature regions)
+ * \param[out] labels a PointCloud of edge labels
+ */
+ void
+ extractEdges (pcl::PointCloud<PointLT>& labels) const;
+
+ /** \brief A pointer to the input normals */
+ PointCloudNConstPtr normals_;
+
+ /** \brief The low threshold value for high curvature Canny edge detection (default: 0.4) */
+ float th_hc_canny_low_;
+
+ /** \brief The high threshold value for high curvature Canny edge detection (default: 1.1) */
+ float th_hc_canny_high_;
+ };
+
+ template <typename PointT, typename PointNT, typename PointLT>
+ class OrganizedEdgeFromRGBNormals : public OrganizedEdgeFromRGB<PointT, PointLT>, public OrganizedEdgeFromNormals<PointT, PointNT, PointLT>
+ {
+ typedef typename pcl::PointCloud<PointT> PointCloud;
+ typedef typename PointCloud::Ptr PointCloudPtr;
+ typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+ typedef typename pcl::PointCloud<PointNT> PointCloudN;
+ typedef typename PointCloudN::Ptr PointCloudNPtr;
+ typedef typename PointCloudN::ConstPtr PointCloudNConstPtr;
+
+ typedef typename pcl::PointCloud<PointLT> PointCloudL;
+ typedef typename PointCloudL::Ptr PointCloudLPtr;
+ typedef typename PointCloudL::ConstPtr PointCloudLConstPtr;
+
+ public:
+ using OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::input_;
+ using OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::indices_;
+ using OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::initCompute;
+ using OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::deinitCompute;
+ using OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::detecting_edge_types_;
+ using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_NAN_BOUNDARY;
+ using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_OCCLUDING;
+ using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_OCCLUDED;
+ using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_HIGH_CURVATURE;
+ using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_RGB_CANNY;
+
+ /** \brief Constructor for OrganizedEdgeFromRGBNormals */
+ OrganizedEdgeFromRGBNormals ()
+ : OrganizedEdgeFromRGB<PointT, PointLT> ()
+ , OrganizedEdgeFromNormals<PointT, PointNT, PointLT> ()
+ {
+ this->setEdgeType (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED | EDGELABEL_RGB_CANNY | EDGELABEL_HIGH_CURVATURE);
+ }
+
+ /** \brief Destructor for OrganizedEdgeFromRGBNormals */
+ virtual
+ ~OrganizedEdgeFromRGBNormals ()
+ {
+ }
+
+ /** \brief Perform the 3D edge detection (edges from depth discontinuities, RGB Canny edge, and high curvature regions) and assign point indices for each edge label
+ * \param[out] labels a PointCloud of edge labels
+ * \param[out] label_indices a vector of PointIndices corresponding to each edge label
+ */
+ void
+ compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const;
+ };
+}
+
+#ifdef PCL_NO_PRECOMPILE
+#include <pcl/features/impl/organized_edge_detection.hpp>
+#endif
+
+#endif //#ifndef PCL_FEATURES_ORGANIZED_EDGE_DETECTION_H_
* \param[out] centroids vector to hold the centroids
*/
inline void
- getCentroidClusters (std::vector<Eigen::Vector3f> & centroids)
+ getCentroidClusters (std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > & centroids)
{
for (size_t i = 0; i < centroids_dominant_orientations_.size (); ++i)
centroids.push_back (centroids_dominant_orientations_[i]);
* \param[out] centroids vector to hold the normal centroids
*/
inline void
- getCentroidNormalClusters (std::vector<Eigen::Vector3f> & centroids)
+ getCentroidNormalClusters (std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > & centroids)
{
for (size_t i = 0; i < dominant_normals_.size (); ++i)
centroids.push_back (dominant_normals_[i]);
protected:
/** \brief Centroids that were used to compute different OUR-CVFH descriptors */
- std::vector<Eigen::Vector3f> centroids_dominant_orientations_;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > centroids_dominant_orientations_;
/** \brief Normal centroids that were used to compute different OUR-CVFH descriptors */
- std::vector<Eigen::Vector3f> dominant_normals_;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > dominant_normals_;
/** \brief Indices to the points representing the stable clusters */
std::vector<pcl::PointIndices> clusters_;
/** \brief Mapping from clusters to OUR-CVFH descriptors */
float d_pi_;
/** \brief Internal hashmap, used to optimize efficiency of redundant computations. */
- std::map<std::pair<int, int>, Eigen::Vector4f, std::less<std::pair<int, int> >, Eigen::aligned_allocator<Eigen::Vector4f> > feature_map_;
+ std::map<std::pair<int, int>, Eigen::Vector4f, std::less<std::pair<int, int> >, Eigen::aligned_allocator<std::pair<const std::pair<int, int>, Eigen::Vector4f> > > feature_map_;
/** \brief Queue of pairs saved, used to constrain memory usage. */
std::queue<std::pair<int, int> > key_list_;
private:
/** \brief A pointer to the input dataset that contains the point normals of the XYZ dataset. */
- std::vector<Eigen::Vector3f> projected_normals_;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > projected_normals_;
/** \brief SSE aligned placeholder for the XYZ centroid of a surface patch. */
Eigen::Vector3f xyz_centroid_;
inline void
setNrSubdivisions (int nr_subdiv) { nr_subdiv_ = nr_subdiv; }
- /** \brief Get the number of subdivisions for the considered distance interval. */
+ /** \brief Get the number of subdivisions for the considered distance interval.
+ * \return the number of subdivisions
+ */
inline int
getNrSubdivisions () const { return (nr_subdiv_); }
inline void
setPlaneRadius (double plane_radius) { plane_radius_ = plane_radius; }
- /** \brief Get the maximum radius, above which everything can be considered planar. */
+ /** \brief Get the maximum radius, above which everything can be considered planar.
+ * \return the plane_radius used
+ */
inline double
getPlaneRadius () const { return (plane_radius_); }
inline void
setSaveHistograms (bool save) { save_histograms_ = save; }
- /** \brief Returns whether the full distance-angle histograms are being saved. */
+ /** \brief Returns whether the full distance-angle histograms are being saved.
+ * \return true if the histograms are being be saved
+ */
inline bool
getSaveHistograms () const { return (save_histograms_); }
- /** \brief Returns a pointer to the list of full distance-angle histograms for all points. */
+ /** \brief Returns a pointer to the list of full distance-angle histograms for all points.
+ * \return the histogram being saved when computing RSD
+ */
inline boost::shared_ptr<std::vector<Eigen::MatrixXf, Eigen::aligned_allocator<Eigen::MatrixXf> > >
getHistograms () const { return (histograms_); }
* International Workshop on 3D Object Retrieval (3DOR 10) -
* in conjuction with ACM Multimedia 2010
*
- * The suggested PointOutT is pcl::ShapeContext1980
+ * The suggested PointOutT is pcl::UniqueShapeContext1960
*
* \author Alessandro Franchi, Federico Tombari, Samuele Salti (original code)
* \author Nizar Sallem (port to PCL)
* \ingroup features
*/
- template <typename PointInT, typename PointOutT = pcl::ShapeContext1980, typename PointRFT = pcl::ReferenceFrame>
+ template <typename PointInT, typename PointOutT = pcl::UniqueShapeContext1960, typename PointRFT = pcl::ReferenceFrame>
class UniqueShapeContext : public Feature<PointInT, PointOutT>,
public FeatureWithLocalReferenceFrames<PointInT, PointRFT>
{
/** \brief Constructor. */
UniqueShapeContext () :
radii_interval_(0), theta_divisions_(0), phi_divisions_(0), volume_lut_(0),
- azimuth_bins_(12), elevation_bins_(11), radius_bins_(15),
- min_radius_(0.1), point_density_radius_(0.2), descriptor_length_ (), local_radius_ (2.5)
+ azimuth_bins_(14), elevation_bins_(14), radius_bins_(10),
+ min_radius_(0.1), point_density_radius_(0.1), descriptor_length_ (), local_radius_ (2.0)
{
feature_name_ = "UniqueShapeContext";
- search_radius_ = 2.5;
+ search_radius_ = 2.0;
}
virtual ~UniqueShapeContext() { }
- //inline void
- //setAzimuthBins (size_t bins) { azimuth_bins_ = bins; }
-
/** \return The number of bins along the azimuth. */
inline size_t
getAzimuthBins () const { return (azimuth_bins_); }
- //inline void
- //setElevationBins (size_t bins) { elevation_bins_ = bins; }
-
/** \return The number of bins along the elevation */
inline size_t
getElevationBins () const { return (elevation_bins_); }
- //inline void
- //setRadiusBins (size_t bins) { radius_bins_ = bins; }
-
/** \return The number of bins along the radii direction. */
inline size_t
getRadiusBins () const { return (radius_bins_); }
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception , Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ */
+//#include <pcl/point_types.h>
+//#include <pcl/impl/instantiate.hpp>
+//#include <pcl/features/brisk_2d.h>
+//#include <pcl/features/impl/brisk_2d.hpp>
+
+// Instantiations of specific point types
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2009-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "pcl/features/impl/grsd.hpp"
+
+#ifndef PCL_NO_PRECOMPILE
+#include "pcl/point_types.h"
+#include "pcl/impl/instantiate.hpp"
+// Instantiations of specific point types
+#ifdef PCL_ONLY_CORE_POINT_TYPES
+ PCL_INSTANTIATE_PRODUCT(GRSDEstimation, ((pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA))((pcl::Normal))((pcl::GRSDSignature21)))
+#else
+ PCL_INSTANTIATE_PRODUCT(GRSDEstimation, (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES)((pcl::GRSDSignature21)))
+#endif
+#endif // PCL_NO_PRECOMPILE
+
//!!! nizar 20110408 : for OpenMP sake on MSVC this must be kept signed
for (int interest_point_idx = 0; interest_point_idx < int (interest_points.points.size ()); ++interest_point_idx)
{
- Vector3fMapConst point = interest_points.points[interest_point_idx].getVector3fMap ();
+ const InterestPoint& interest_point = interest_points.points[interest_point_idx];
+ Vector3fMapConst point = interest_point.getVector3fMap ();
Narf* feature = new Narf;
if (!feature->extractFromRangeImage(range_image, point, descriptor_size, support_size))
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/features/impl/organized_edge_detection.hpp>
+
+#ifndef PCL_NO_PRECOMPILE
+#include <pcl/point_types.h>
+#include <pcl/impl/instantiate.hpp>
+PCL_INSTANTIATE_PRODUCT(OrganizedEdgeBase, (PCL_XYZ_POINT_TYPES)((pcl::Label)))
+PCL_INSTANTIATE_PRODUCT(OrganizedEdgeFromRGB, (PCL_RGB_POINT_TYPES)((pcl::Label)))
+PCL_INSTANTIATE_PRODUCT(OrganizedEdgeFromNormals, (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES)((pcl::Label)))
+PCL_INSTANTIATE_PRODUCT(OrganizedEdgeFromRGBNormals, (PCL_RGB_POINT_TYPES)(PCL_NORMAL_POINT_TYPES)((pcl::Label)))
+#endif // PCL_NO_PRECOMPILE
+
#include <pcl/impl/instantiate.hpp>
// Instantiations of specific point types
#ifdef PCL_ONLY_CORE_POINT_TYPES
- PCL_INSTANTIATE_PRODUCT(UniqueShapeContext, ((pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA))((pcl::ShapeContext1980))((pcl::ReferenceFrame)))
+ PCL_INSTANTIATE_PRODUCT(UniqueShapeContext, ((pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA))((pcl::UniqueShapeContext1960))((pcl::ReferenceFrame)))
#else
- PCL_INSTANTIATE_PRODUCT(UniqueShapeContext, (PCL_XYZ_POINT_TYPES)((pcl::ShapeContext1980))((pcl::ReferenceFrame)))
+ PCL_INSTANTIATE_PRODUCT(UniqueShapeContext, (PCL_XYZ_POINT_TYPES)((pcl::UniqueShapeContext1960))((pcl::ReferenceFrame)))
#endif
#endif // PCL_NO_PRECOMPILE
src/frustum_culling.cpp
src/covariance_sampling.cpp
src/median_filter.cpp
+ src/uniform_sampling.cpp
src/voxel_grid_occlusion_estimation.cpp
src/normal_refinement.cpp
src/grid_minimum.cpp
"include/pcl/${SUBSYS_NAME}/frustum_culling.h"
"include/pcl/${SUBSYS_NAME}/covariance_sampling.h"
"include/pcl/${SUBSYS_NAME}/median_filter.h"
+ "include/pcl/${SUBSYS_NAME}/uniform_sampling.h"
"include/pcl/${SUBSYS_NAME}/normal_refinement.h"
"include/pcl/${SUBSYS_NAME}/grid_minimum.h"
"include/pcl/${SUBSYS_NAME}/morphological_filter.h"
"include/pcl/${SUBSYS_NAME}/impl/frustum_culling.hpp"
"include/pcl/${SUBSYS_NAME}/impl/covariance_sampling.hpp"
"include/pcl/${SUBSYS_NAME}/impl/median_filter.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/uniform_sampling.hpp"
"include/pcl/${SUBSYS_NAME}/impl/normal_refinement.hpp"
"include/pcl/${SUBSYS_NAME}/impl/grid_minimum.hpp"
"include/pcl/${SUBSYS_NAME}/impl/morphological_filter.hpp"
clipPlanarPolygon3D (std::vector<PointT, Eigen::aligned_allocator<PointT> >& polygon) const;
virtual void
- clipPlanarPolygon3D (const std::vector<PointT, Eigen::aligned_allocator<PointT> >& polygon, std::vector<PointT>& clipped_polygon) const;
+ clipPlanarPolygon3D (const std::vector<PointT, Eigen::aligned_allocator<PointT> >& polygon, std::vector<PointT, Eigen::aligned_allocator<PointT> >& clipped_polygon) const;
virtual void
clipPointCloud3D (const pcl::PointCloud<PointT> &cloud_in, std::vector<int>& clipped, const std::vector<int>& indices = std::vector<int> ()) const;
/** \brief The normals computed at each point in the input cloud */
NormalsConstPtr input_normals_;
- std::vector<Eigen::Vector3f> scaled_points_;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > scaled_points_;
bool
initCompute ();
x_dim_ = width;
y_dim_ = height;
z_dim_ = depth;
- v_ = std::vector<Eigen::Vector2f> (width*height*depth, Eigen::Vector2f (0.0f, 0.0f));
+ v_ = std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > (width*height*depth, Eigen::Vector2f (0.0f, 0.0f));
}
inline Eigen::Vector2f&
z_size () const
{ return z_dim_; }
- inline std::vector<Eigen::Vector2f >::iterator
+ inline std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >::iterator
begin ()
{ return v_.begin (); }
- inline std::vector<Eigen::Vector2f >::iterator
+ inline std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >::iterator
end ()
{ return v_.end (); }
- inline std::vector<Eigen::Vector2f >::const_iterator
+ inline std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >::const_iterator
begin () const
{ return v_.begin (); }
- inline std::vector<Eigen::Vector2f >::const_iterator
+ inline std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >::const_iterator
end () const
{ return v_.end (); }
private:
- std::vector<Eigen::Vector2f > v_;
+ std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > v_;
size_t x_dim_, y_dim_, z_dim_;
};
#define PCL_FILTER_H_
#include <pcl/pcl_base.h>
+#include <pcl/common/io.h>
#include <pcl/conversions.h>
#include <pcl/filters/boost.h>
#include <cfloat>
if (!initCompute ())
return;
- // Resize the output dataset
- //if (output.points.size () != indices_->size ())
- // output.points.resize (indices_->size ());
-
- // Copy header at a minimum
- output.header = input_->header;
- output.sensor_origin_ = input_->sensor_origin_;
- output.sensor_orientation_ = input_->sensor_orientation_;
-
- // Apply the actual filter
- applyFilter (output);
+ if (input_.get () == &output) // cloud_in = cloud_out
+ {
+ PointCloud output_temp;
+ applyFilter (output_temp);
+ output_temp.header = input_->header;
+ output_temp.sensor_origin_ = input_->sensor_origin_;
+ output_temp.sensor_orientation_ = input_->sensor_orientation_;
+ pcl::copyPointCloud (output_temp, output);
+ }
+ else
+ {
+ output.header = input_->header;
+ output.sensor_origin_ = input_->sensor_origin_;
+ output.sensor_orientation_ = input_->sensor_orientation_;
+ applyFilter (output);
+ }
deinitCompute ();
}
// In case a search method has not been given, initialize it using some defaults
if (!tree_)
{
- // For organized datasets, use an OrganizedDataIndex
+ // For organized datasets, use an OrganizedNeighbor
if (input_->isOrganized ())
tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
// For unorganized data, use a FLANN kdtree
#ifdef _OPENMP
#pragma omp parallel for shared (output) private (nn_indices, nn_distances) num_threads (threads_)
#endif
- for (std::size_t point_idx = 0; point_idx < surface_->size (); ++point_idx)
+ for (int64_t point_idx = 0; point_idx < static_cast<int64_t> (surface_->size ()); ++point_idx)
{
const PointInT& point_in = surface_->points [point_idx];
PointOutT& point_out = output [point_idx];
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2009-2011, Willow Garage, Inc.
+ * Copyright (c) 2015, Google, Inc.
*
* All rights reserved.
*
inverse_transform = transform.inverse ();
}
+ bool transform_matrix_is_identity = transform_.matrix ().isIdentity ();
+ bool translation_is_zero = (translation_ != Eigen::Vector3f::Zero ());
+ bool inverse_transform_matrix_is_identity = inverse_transform.matrix ().isIdentity ();
+
for (size_t index = 0; index < indices_->size (); ++index)
{
if (!input_->is_dense)
PointT local_pt = input_->points[(*indices_)[index]];
// Transform point to world space
- if (!(transform_.matrix ().isIdentity ()))
+ if (!transform_matrix_is_identity)
local_pt = pcl::transformPoint<PointT> (local_pt, transform_);
- if (translation_ != Eigen::Vector3f::Zero ())
+ if (translation_is_zero)
{
local_pt.x -= translation_ (0);
local_pt.y -= translation_ (1);
}
// Transform point to local space of crop box
- if (!(inverse_transform.matrix ().isIdentity ()))
+ if (!inverse_transform_matrix_is_identity)
local_pt = pcl::transformPoint<PointT> (local_pt, inverse_transform);
// If outside the cropbox
if (early_division_)
{
- for (std::vector<Eigen::Vector2f >::iterator d = data.begin (); d != data.end (); ++d)
+ for (std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >::iterator d = data.begin (); d != data.end (); ++d)
*d /= ((*d)[0] != 0) ? (*d)[1] : 1;
for (size_t x = 0; x < input_->width; x++)
if (early_division_)
{
- for (std::vector<Eigen::Vector2f >::iterator d = data.begin (); d != data.end (); ++d)
+ for (std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >::iterator d = data.begin (); d != data.end (); ++d)
*d /= ((*d)[0] != 0) ? (*d)[1] : 1;
#ifdef _OPENMP
removed_indices_->resize (indices_->size ());
int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator
- for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
+ // If the data is dense => use nearest-k search
+ if (input_->is_dense)
{
- // Perform the radius search
// Note: k includes the query point, so is always at least 1
- int k = searcher_->radiusSearch (*it, search_radius_, nn_indices, nn_dists);
+ int mean_k = min_pts_radius_ + 1;
+ double nn_dists_max = search_radius_ * search_radius_;
- // Points having too few neighbors are outliers and are passed to removed indices
- // Unless negative was set, then it's the opposite condition
- if ((!negative_ && k <= min_pts_radius_) || (negative_ && k > min_pts_radius_))
+ for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
{
- if (extract_removed_indices_)
- (*removed_indices_)[rii++] = *it;
- continue;
+ // Perform the nearest-k search
+ int k = searcher_->nearestKSearch (*it, mean_k, nn_indices, nn_dists);
+
+ // Check the number of neighbors
+ // Note: nn_dists is sorted, so check the last item
+ bool chk_neighbors = true;
+ if (k == mean_k)
+ {
+ if (negative_)
+ {
+ chk_neighbors = false;
+ if (nn_dists_max < nn_dists[k-1])
+ {
+ chk_neighbors = true;
+ }
+ }
+ else
+ {
+ chk_neighbors = true;
+ if (nn_dists_max < nn_dists[k-1])
+ {
+ chk_neighbors = false;
+ }
+ }
+ }
+ else
+ {
+ if (negative_)
+ chk_neighbors = true;
+ else
+ chk_neighbors = false;
+ }
+
+ // Points having too few neighbors are outliers and are passed to removed indices
+ // Unless negative was set, then it's the opposite condition
+ if (!chk_neighbors)
+ {
+ if (extract_removed_indices_)
+ (*removed_indices_)[rii++] = *it;
+ continue;
+ }
+
+ // Otherwise it was a normal point for output (inlier)
+ indices[oii++] = *it;
}
+ }
+ // NaN or Inf values could exist => use radius search
+ else
+ {
+ for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
+ {
+ // Perform the radius search
+ // Note: k includes the query point, so is always at least 1
+ int k = searcher_->radiusSearch (*it, search_radius_, nn_indices, nn_dists);
- // Otherwise it was a normal point for output (inlier)
- indices[oii++] = *it;
+ // Points having too few neighbors are outliers and are passed to removed indices
+ // Unless negative was set, then it's the opposite condition
+ if ((!negative_ && k <= min_pts_radius_) || (negative_ && k > min_pts_radius_))
+ {
+ if (extract_removed_indices_)
+ (*removed_indices_)[rii++] = *it;
+ continue;
+ }
+
+ // Otherwise it was a normal point for output (inlier)
+ indices[oii++] = *it;
+ }
}
// Resize the output arrays
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_FILTERS_UNIFORM_SAMPLING_IMPL_H_
+#define PCL_FILTERS_UNIFORM_SAMPLING_IMPL_H_
+
+#include <pcl/common/common.h>
+#include <pcl/filters/uniform_sampling.h>
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::UniformSampling<PointT>::applyFilter (PointCloud &output)
+{
+ // Has the input dataset been set already?
+ if (!input_)
+ {
+ PCL_WARN ("[pcl::%s::detectKeypoints] No input dataset given!\n", getClassName ().c_str ());
+ output.width = output.height = 0;
+ output.points.clear ();
+ return;
+ }
+
+ output.height = 1; // downsampling breaks the organized structure
+ output.is_dense = true; // we filter out invalid points
+
+ Eigen::Vector4f min_p, max_p;
+ // Get the minimum and maximum dimensions
+ pcl::getMinMax3D<PointT>(*input_, min_p, max_p);
+
+ // Compute the minimum and maximum bounding box values
+ min_b_[0] = static_cast<int> (floor (min_p[0] * inverse_leaf_size_[0]));
+ max_b_[0] = static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0]));
+ min_b_[1] = static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1]));
+ max_b_[1] = static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1]));
+ min_b_[2] = static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2]));
+ max_b_[2] = static_cast<int> (floor (max_p[2] * inverse_leaf_size_[2]));
+
+ // Compute the number of divisions needed along all axis
+ div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
+ div_b_[3] = 0;
+
+ // Clear the leaves
+ leaves_.clear ();
+
+ // Set up the division multiplier
+ divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
+
+ // First pass: build a set of leaves with the point index closest to the leaf center
+ for (size_t cp = 0; cp < indices_->size (); ++cp)
+ {
+ if (!input_->is_dense)
+ // Check if the point is invalid
+ if (!pcl_isfinite (input_->points[(*indices_)[cp]].x) ||
+ !pcl_isfinite (input_->points[(*indices_)[cp]].y) ||
+ !pcl_isfinite (input_->points[(*indices_)[cp]].z))
+ continue;
+
+ Eigen::Vector4i ijk = Eigen::Vector4i::Zero ();
+ ijk[0] = static_cast<int> (floor (input_->points[(*indices_)[cp]].x * inverse_leaf_size_[0]));
+ ijk[1] = static_cast<int> (floor (input_->points[(*indices_)[cp]].y * inverse_leaf_size_[1]));
+ ijk[2] = static_cast<int> (floor (input_->points[(*indices_)[cp]].z * inverse_leaf_size_[2]));
+
+ // Compute the leaf index
+ int idx = (ijk - min_b_).dot (divb_mul_);
+ Leaf& leaf = leaves_[idx];
+ // First time we initialize the index
+ if (leaf.idx == -1)
+ {
+ leaf.idx = (*indices_)[cp];
+ continue;
+ }
+
+ // Check to see if this point is closer to the leaf center than the previous one we saved
+ float diff_cur = (input_->points[(*indices_)[cp]].getVector4fMap () - ijk.cast<float> ()).squaredNorm ();
+ float diff_prev = (input_->points[leaf.idx].getVector4fMap () - ijk.cast<float> ()).squaredNorm ();
+
+ // If current point is closer, copy its index instead
+ if (diff_cur < diff_prev)
+ leaf.idx = (*indices_)[cp];
+ }
+
+ // Second pass: go over all leaves and copy data
+ output.points.resize (leaves_.size ());
+ int cp = 0;
+
+ for (typename boost::unordered_map<size_t, Leaf>::const_iterator it = leaves_.begin (); it != leaves_.end (); ++it)
+ output.points[cp++] = input_->points[it->second.idx];
+ output.width = static_cast<uint32_t> (output.points.size ());
+}
+
+#define PCL_INSTANTIATE_UniformSampling(T) template class PCL_EXPORTS pcl::UniformSampling<T>;
+
+#endif // PCL_FILTERS_UNIFORM_SAMPLING_IMPL_H_
+
#ifndef PCL_FILTERS_IMPL_VOXEL_GRID_H_
#define PCL_FILTERS_IMPL_VOXEL_GRID_H_
+#include <pcl/common/centroid.h>
#include <pcl/common/common.h>
#include <pcl/common/io.h>
#include <pcl/filters/voxel_grid.h>
// Set up the division multiplier
divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
- int centroid_size = 4;
- if (downsample_all_data_)
- centroid_size = boost::mpl::size<FieldList>::value;
-
- // ---[ RGB special case
- std::vector<pcl::PCLPointField> fields;
- int rgba_index = -1;
- rgba_index = pcl::getFieldIndex (*input_, "rgb", fields);
- if (rgba_index == -1)
- rgba_index = pcl::getFieldIndex (*input_, "rgba", fields);
- if (rgba_index >= 0)
- {
- rgba_index = fields[rgba_index].offset;
- centroid_size += 3;
- }
-
+ // Storage for mapping leaf and pointcloud indexes
std::vector<cloud_point_index_idx> index_vector;
index_vector.reserve (indices_->size ());
}
index = 0;
- Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
- Eigen::VectorXf temporary = Eigen::VectorXf::Zero (centroid_size);
-
for (unsigned int cp = 0; cp < first_and_last_indices_vector.size (); ++cp)
{
// calculate centroid - sum values from all input points, that have the same idx value in index_vector array
- unsigned int first_index = first_and_last_indices_vector[cp].first;
- unsigned int last_index = first_and_last_indices_vector[cp].second;
- if (!downsample_all_data_)
- {
- centroid[0] = input_->points[index_vector[first_index].cloud_point_index].x;
- centroid[1] = input_->points[index_vector[first_index].cloud_point_index].y;
- centroid[2] = input_->points[index_vector[first_index].cloud_point_index].z;
- }
- else
- {
- // ---[ RGB special case
- if (rgba_index >= 0)
- {
- // Fill r/g/b data, assuming that the order is BGRA
- pcl::RGB rgb;
- memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[index_vector[first_index].cloud_point_index]) + rgba_index, sizeof (RGB));
- centroid[centroid_size-3] = rgb.r;
- centroid[centroid_size-2] = rgb.g;
- centroid[centroid_size-1] = rgb.b;
- }
- pcl::for_each_type <FieldList> (NdCopyPointEigenFunctor <PointT> (input_->points[index_vector[first_index].cloud_point_index], centroid));
- }
-
- for (unsigned int i = first_index + 1; i < last_index; ++i)
- {
- if (!downsample_all_data_)
- {
- centroid[0] += input_->points[index_vector[i].cloud_point_index].x;
- centroid[1] += input_->points[index_vector[i].cloud_point_index].y;
- centroid[2] += input_->points[index_vector[i].cloud_point_index].z;
- }
- else
- {
- // ---[ RGB special case
- if (rgba_index >= 0)
- {
- // Fill r/g/b data, assuming that the order is BGRA
- pcl::RGB rgb;
- memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[index_vector[i].cloud_point_index]) + rgba_index, sizeof (RGB));
- temporary[centroid_size-3] = rgb.r;
- temporary[centroid_size-2] = rgb.g;
- temporary[centroid_size-1] = rgb.b;
- }
- pcl::for_each_type <FieldList> (NdCopyPointEigenFunctor <PointT> (input_->points[index_vector[i].cloud_point_index], temporary));
- centroid += temporary;
- }
- }
+ unsigned int first_index = first_and_last_indices_vector[cp].first;
+ unsigned int last_index = first_and_last_indices_vector[cp].second;
// index is centroid final position in resulting PointCloud
if (save_leaf_layout_)
leaf_layout_[index_vector[first_index].idx] = index;
- centroid /= static_cast<float> (last_index - first_index);
-
- // store centroid
- // Do we need to process all the fields?
- if (!downsample_all_data_)
+ //Limit downsampling to coords
+ if (!downsample_all_data_)
{
- output.points[index].x = centroid[0];
- output.points[index].y = centroid[1];
- output.points[index].z = centroid[2];
+ Eigen::Vector4f centroid (Eigen::Vector4f::Zero ());
+
+ for (unsigned int li = first_index; li < last_index; ++li)
+ centroid += input_->points[index_vector[li].cloud_point_index].getVector4fMap ();
+
+ centroid /= static_cast<float> (last_index - first_index);
+ output.points[index].getVector4fMap () = centroid;
}
- else
+ else
{
- pcl::for_each_type<FieldList> (pcl::NdCopyEigenPointFunctor <PointT> (centroid, output.points[index]));
- // ---[ RGB special case
- if (rgba_index >= 0)
- {
- // pack r/g/b into rgb
- float r = centroid[centroid_size-3], g = centroid[centroid_size-2], b = centroid[centroid_size-1];
- int rgb = (static_cast<int> (r) << 16) | (static_cast<int> (g) << 8) | static_cast<int> (b);
- memcpy (reinterpret_cast<char*> (&output.points[index]) + rgba_index, &rgb, sizeof (float));
- }
+ CentroidPoint<PointT> centroid;
+
+ // fill in the accumulator with leaf points
+ for (unsigned int li = first_index; li < last_index; ++li)
+ centroid.add (input_->points[index_vector[li].cloud_point_index]);
+
+ centroid.get (output.points[index]);
}
+
++index;
}
output.width = static_cast<uint32_t> (output.points.size ());
if (rgba_index >= 0)
{
rgba_index = fields[rgba_index].offset;
- centroid_size += 3;
+ centroid_size += 4;
}
// If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first...
{
// Copy all the fields
Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
+ pcl::for_each_type<FieldList> (NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid));
// ---[ RGB special case
if (rgba_index >= 0)
{
- // fill r/g/b data
- int rgb;
- memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index, sizeof (int));
- centroid[centroid_size - 3] = static_cast<float> ((rgb >> 16) & 0x0000ff);
- centroid[centroid_size - 2] = static_cast<float> ((rgb >> 8) & 0x0000ff);
- centroid[centroid_size - 1] = static_cast<float> ((rgb) & 0x0000ff);
+ // Fill r/g/b data, assuming that the order is BGRA
+ const pcl::RGB& rgb = *reinterpret_cast<const RGB*> (reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index);
+ centroid[centroid_size - 4] = rgb.a;
+ centroid[centroid_size - 3] = rgb.r;
+ centroid[centroid_size - 2] = rgb.g;
+ centroid[centroid_size - 1] = rgb.b;
}
- pcl::for_each_type<FieldList> (NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid));
leaf.centroid += centroid;
}
++leaf.nr_points;
{
// Copy all the fields
Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
+ pcl::for_each_type<FieldList> (NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid));
// ---[ RGB special case
if (rgba_index >= 0)
{
// Fill r/g/b data, assuming that the order is BGRA
- int rgb;
- memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index, sizeof (int));
- centroid[centroid_size - 3] = static_cast<float> ((rgb >> 16) & 0x0000ff);
- centroid[centroid_size - 2] = static_cast<float> ((rgb >> 8) & 0x0000ff);
- centroid[centroid_size - 1] = static_cast<float> ((rgb) & 0x0000ff);
+ const pcl::RGB& rgb = *reinterpret_cast<const RGB*> (reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index);
+ centroid[centroid_size - 4] = rgb.a;
+ centroid[centroid_size - 3] = rgb.r;
+ centroid[centroid_size - 2] = rgb.g;
+ centroid[centroid_size - 1] = rgb.b;
}
- pcl::for_each_type<FieldList> (NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid));
leaf.centroid += centroid;
}
++leaf.nr_points;
// ---[ RGB special case
if (rgba_index >= 0)
{
- // pack r/g/b into rgb
- float r = leaf.centroid[centroid_size - 3], g = leaf.centroid[centroid_size - 2], b = leaf.centroid[centroid_size - 1];
- int rgb = (static_cast<int> (r)) << 16 | (static_cast<int> (g)) << 8 | (static_cast<int> (b));
- memcpy (reinterpret_cast<char*> (&output.points.back ()) + rgba_index, &rgb, sizeof (float));
+ pcl::RGB& rgb = *reinterpret_cast<RGB*> (reinterpret_cast<char*> (&output.points.back ()) + rgba_index);
+ rgb.a = leaf.centroid[centroid_size - 4];
+ rgb.r = leaf.centroid[centroid_size - 3];
+ rgb.g = leaf.centroid[centroid_size - 2];
+ rgb.b = leaf.centroid[centroid_size - 1];
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::VoxelGridOcclusionEstimation<PointT>::occlusionEstimation (int& out_state,
- std::vector<Eigen::Vector3i>& out_ray,
+ std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
const Eigen::Vector3i& in_target_voxel)
{
if (!initialized_)
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
-pcl::VoxelGridOcclusionEstimation<PointT>::occlusionEstimationAll (std::vector<Eigen::Vector3i>& occluded_voxels)
+pcl::VoxelGridOcclusionEstimation<PointT>::occlusionEstimationAll (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& occluded_voxels)
{
if (!initialized_)
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
-pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (std::vector <Eigen::Vector3i>& out_ray,
+pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
const Eigen::Vector3i& target_voxel,
const Eigen::Vector4f& origin,
const Eigen::Vector4f& direction,
}
/** \brief Register a different threshold function
- * \param[in] pointer to a threshold function
+ * \param[in] thresh pointer to a threshold function
*/
void
setThresholdFunction (boost::function<bool (double)> thresh)
}
/** \brief Register a different threshold function
- * \param[in] pointer to a threshold function
+ * \param[in] thresh_function pointer to a threshold function
* \param[in] instance
*/
template <typename T> void
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_FILTERS_UNIFORM_SAMPLING_H_
+#define PCL_FILTERS_UNIFORM_SAMPLING_H_
+
+#include <pcl/filters/filter.h>
+#include <boost/unordered_map.hpp>
+
+namespace pcl
+{
+ /** \brief @b UniformSampling assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
+ *
+ * The @b UniformSampling class creates a *3D voxel grid* (think about a voxel
+ * grid as a set of tiny 3D boxes in space) over the input point cloud data.
+ * Then, in each *voxel* (i.e., 3D box), all the points present will be
+ * approximated (i.e., *downsampled*) with their centroid. This approach is
+ * a bit slower than approximating them with the center of the voxel, but it
+ * represents the underlying surface more accurately.
+ *
+ * \author Radu Bogdan Rusu
+ * \ingroup keypoints
+ */
+ template <typename PointT>
+ class UniformSampling: public Filter<PointT>
+ {
+ typedef typename Filter<PointT>::PointCloud PointCloud;
+
+ using Filter<PointT>::filter_name_;
+ using Filter<PointT>::input_;
+ using Filter<PointT>::indices_;
+ using Filter<PointT>::getClassName;
+
+ public:
+ typedef boost::shared_ptr<UniformSampling<PointT> > Ptr;
+ typedef boost::shared_ptr<const UniformSampling<PointT> > ConstPtr;
+
+ /** \brief Empty constructor. */
+ UniformSampling () :
+ leaves_ (),
+ leaf_size_ (Eigen::Vector4f::Zero ()),
+ inverse_leaf_size_ (Eigen::Vector4f::Zero ()),
+ min_b_ (Eigen::Vector4i::Zero ()),
+ max_b_ (Eigen::Vector4i::Zero ()),
+ div_b_ (Eigen::Vector4i::Zero ()),
+ divb_mul_ (Eigen::Vector4i::Zero ()),
+ search_radius_ (0)
+ {
+ filter_name_ = "UniformSampling";
+ }
+
+ /** \brief Destructor. */
+ virtual ~UniformSampling ()
+ {
+ leaves_.clear();
+ }
+
+ /** \brief Set the 3D grid leaf size.
+ * \param radius the 3D grid leaf size
+ */
+ virtual inline void
+ setRadiusSearch (double radius)
+ {
+ leaf_size_[0] = leaf_size_[1] = leaf_size_[2] = static_cast<float> (radius);
+ // Avoid division errors
+ if (leaf_size_[3] == 0)
+ leaf_size_[3] = 1;
+ // Use multiplications instead of divisions
+ inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
+ search_radius_ = radius;
+ }
+
+ protected:
+ /** \brief Simple structure to hold an nD centroid and the number of points in a leaf. */
+ struct Leaf
+ {
+ Leaf () : idx (-1) { }
+ int idx;
+ };
+
+ /** \brief The 3D grid leaves. */
+ boost::unordered_map<size_t, Leaf> leaves_;
+
+ /** \brief The size of a leaf. */
+ Eigen::Vector4f leaf_size_;
+
+ /** \brief Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons. */
+ Eigen::Array4f inverse_leaf_size_;
+
+ /** \brief The minimum and maximum bin coordinates, the number of divisions, and the division multiplier. */
+ Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_;
+
+ /** \brief The nearest neighbors search radius for each point. */
+ double search_radius_;
+
+ /** \brief Downsample a Point Cloud using a voxelized grid approach
+ * \param[out] output the resultant point cloud message
+ */
+ void
+ applyFilter (PointCloud &output);
+ };
+}
+
+#ifdef PCL_NO_PRECOMPILE
+#include <pcl/filters/impl/uniform_sampling.hpp>
+#endif
+
+#endif //#ifndef PCL_FILTERS_UNIFORM_SAMPLING_H_
+
* \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed
*/
inline std::vector<int>
- getNeighborCentroidIndices (float x, float y, float z, const std::vector<Eigen::Vector3i> &relative_coordinates)
+ getNeighborCentroidIndices (float x, float y, float z, const std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > &relative_coordinates)
{
Eigen::Vector4i ijk (static_cast<int> (floorf (x * inverse_leaf_size_[0])), static_cast<int> (floorf (y * inverse_leaf_size_[1])), static_cast<int> (floorf (z * inverse_leaf_size_[2])), 0);
std::vector<int> neighbors;
neighbors.reserve (relative_coordinates.size ());
- for (std::vector<Eigen::Vector3i>::const_iterator it = relative_coordinates.begin (); it != relative_coordinates.end (); it++)
+ for (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >::const_iterator it = relative_coordinates.begin (); it != relative_coordinates.end (); it++)
neighbors.push_back (leaf_layout_[(ijk + (Eigen::Vector4i() << *it, 0).finished() - min_b_).dot (divb_mul_)]);
return (neighbors);
}
/** \brief Constructor.
- * Sets \ref leaf_size_ to 0 and \ref searchable_ to false.
+ * Sets \ref leaf_size_ to 0.
*/
VoxelGridLabel () {};
void
initializeVoxelGrid ();
- /** \brief Returns the state (free = 0, occluded = 1) of the voxel
+ /** \brief Computes the state (free = 0, occluded = 1) of the voxel
* after utilizing a ray traversal algorithm to a target voxel
* in (i, j, k) coordinates.
* \param[out] out_state The state of the voxel.
* \param[in] in_target_voxel The target voxel coordinate (i, j, k) of the voxel.
- * \return the state (free = 0, occluded = 1) of the voxel
+ * \return 0 upon success and -1 if an error occurs
*/
int
occlusionEstimation (int& out_state,
const Eigen::Vector3i& in_target_voxel);
- /** \brief Returns the state (free = 0, occluded = 1) of the voxel
+ /** \brief Computes the state (free = 0, occluded = 1) of the voxel
* after utilizing a ray traversal algorithm to a target voxel
* in (i, j, k) coordinates. Additionally, this function returns
* the voxels penetrated of the ray-traversal algorithm till reaching
* \param[out] out_state The state of the voxel.
* \param[out] out_ray The voxels penetrated of the ray-traversal algorithm.
* \param[in] in_target_voxel The target voxel coordinate (i, j, k) of the voxel.
- * \return the state (free = 0, occluded = 1) of the voxel
+ * \return 0 upon success and -1 if an error occurs
*/
int
occlusionEstimation (int& out_state,
- std::vector<Eigen::Vector3i>& out_ray,
+ std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
const Eigen::Vector3i& in_target_voxel);
- /** \brief Returns the voxel coordinates (i, j, k) of all occluded
+ /** \brief Computes the voxel coordinates (i, j, k) of all occluded
* voxels in the voxel gird.
* \param[out] occluded_voxels the coordinates (i, j, k) of all occluded voxels
- * \return the voxel coordinates (i, j, k)
+ * \return 0 upon success and -1 if an error occurs
*/
int
- occlusionEstimationAll (std::vector<Eigen::Vector3i>& occluded_voxels);
+ occlusionEstimationAll (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& occluded_voxels);
/** \brief Returns the voxel grid filtered point cloud
* \return The voxel grid filtered point cloud
* \return The estimated voxel state.
*/
int
- rayTraversal (std::vector <Eigen::Vector3i>& out_ray,
+ rayTraversal (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
const Eigen::Vector3i& target_voxel,
const Eigen::Vector4f& origin,
const Eigen::Vector4f& direction,
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2009, Willow Garage, Inc.
* Copyright (c) 2012-, Open Perception, Inc.
+ * Copyright (c) 2015, Google, Inc.
*
* All rights reserved.
*
//PointXYZ local_pt;
Eigen::Vector3f local_pt (Eigen::Vector3f::Zero ());
+ bool transform_matrix_is_identity = transform_.matrix ().isIdentity ();
+ bool translation_is_zero = (translation_ != Eigen::Vector3f::Zero ());
+ bool inverse_transform_matrix_is_identity = inverse_transform.matrix ().isIdentity ();
+
for (size_t index = 0; index < indices_->size (); ++index)
{
// Get local point
continue;
// Transform point to world space
- if (!(transform_.matrix().isIdentity()))
+ if (!transform_matrix_is_identity)
local_pt = transform_ * local_pt;
- if (translation_ != Eigen::Vector3f::Zero ())
+ if (translation_is_zero)
{
local_pt.x () = local_pt.x () - translation_ (0);
local_pt.y () = local_pt.y () - translation_ (1);
}
// Transform point to local space of crop box
- if (!(inverse_transform.matrix ().isIdentity ()))
+ if (!inverse_transform_matrix_is_identity)
local_pt = inverse_transform * local_pt;
// If outside the cropbox
//PointXYZ local_pt;
Eigen::Vector3f local_pt (Eigen::Vector3f::Zero ());
+ bool transform_matrix_is_identity = transform_.matrix ().isIdentity ();
+ bool translation_is_zero = (translation_ != Eigen::Vector3f::Zero ());
+ bool inverse_transform_matrix_is_identity = inverse_transform.matrix ().isIdentity ();
+
for (size_t index = 0; index < indices_->size (); index++)
{
// Get local point
memcpy (&local_pt, &input_->data[offset], sizeof (float)*3);
// Transform point to world space
- if (!(transform_.matrix().isIdentity()))
+ if (!transform_matrix_is_identity)
local_pt = transform_ * local_pt;
- if (translation_ != Eigen::Vector3f::Zero ())
+ if (translation_is_zero)
{
local_pt.x () -= translation_ (0);
local_pt.y () -= translation_ (1);
}
// Transform point to local space of crop box
- if (!(inverse_transform.matrix().isIdentity()))
+ if (!(inverse_transform_matrix_is_identity))
local_pt = inverse_transform * local_pt;
// If outside the cropbox
void
pcl::ExtractIndices<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
{
- // TODO: the PCLPointCloud2 implementation is not yet using the keep_organized_ system -FF
+ if (keep_organized_)
+ {
+ output = *input_;
+ if (negative_)
+ {
+ // Prepare the output and copy the data
+ for (size_t i = 0; i < indices_->size (); ++i)
+ for (size_t j = 0; j < output.fields.size(); ++j)
+ memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[j].offset],
+ &user_filter_value_, sizeof(float));
+ }
+ else
+ {
+ // Prepare a vector holding all indices
+ std::vector<int> all_indices (input_->width * input_->height);
+ for (int i = 0; i < static_cast<int>(all_indices.size ()); ++i)
+ all_indices[i] = i;
+
+ std::vector<int> indices = *indices_;
+ std::sort (indices.begin (), indices.end ());
+
+ // Get the diference
+ std::vector<int> remaining_indices;
+ set_difference (all_indices.begin (), all_indices.end (), indices.begin (), indices.end (),
+ inserter (remaining_indices, remaining_indices.begin ()));
+
+ // Prepare the output and copy the data
+ for (size_t i = 0; i < remaining_indices.size (); ++i)
+ for (size_t j = 0; j < output.fields.size(); ++j)
+ memcpy (&output.data[remaining_indices[i] * output.point_step + output.fields[j].offset],
+ &user_filter_value_, sizeof(float));
+ }
+ if (!pcl_isfinite (user_filter_value_))
+ output.is_dense = false;
+ return;
+ }
if (indices_->empty () || (input_->width * input_->height == 0))
{
output.width = output.height = 0;
if (!initCompute ())
return;
- // Copy fields and header at a minimum
- output.header = input_->header;
- output.fields = input_->fields;
-
- // Apply the actual filter
- applyFilter (output);
+ if (input_.get () == &output) // cloud_in = cloud_out
+ {
+ pcl::PCLPointCloud2 output_temp;
+ applyFilter (output_temp);
+ output_temp.fields = input_->fields;
+ output_temp.header = input_->header;
+ pcl::copyPointCloud (output_temp, output);
+ }
+ else
+ {
+ output.fields = input_->fields;
+ output.header = input_->header;
+ applyFilter (output);
+ }
deinitCompute ();
}
return;
}
- float badpt = std::numeric_limits<float>::quiet_NaN ();
+ float badpt = user_filter_value_;
// Check whether we need to store filtered valued in place
if (keep_organized_)
{
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ */
+
+#include <pcl/filters/uniform_sampling.h>
+#include <pcl/filters/impl/uniform_sampling.hpp>
+#include <pcl/point_types.h>
+#include <pcl/impl/instantiate.hpp>
+
+
+// Instantiations of specific point types
+PCL_INSTANTIATE_PRODUCT(UniformSampling, (PCL_XYZ_POINT_TYPES))
Eigen::Vector4f pt = Eigen::Vector4f::Zero ();
int centroid_size = 4;
- if (downsample_all_data_)
- centroid_size = static_cast<int> (input_->fields.size ());
-
int rgba_index = -1;
- // ---[ RGB special case
- // if the data contains "rgba" or "rgb", add an extra field for r/g/b in centroid
- for (int d = 0; d < centroid_size; ++d)
+ if (downsample_all_data_)
{
- if (input_->fields[d].name == std::string ("rgba") || input_->fields[d].name == std::string ("rgb"))
+ centroid_size = static_cast<int> (input_->fields.size ());
+
+ // ---[ RGB special case
+ // if the data contains "rgba" or "rgb", add an extra field for r/g/b in centroid
+ for (int d = 0; d < centroid_size; ++d)
{
- rgba_index = d;
- centroid_size += 3;
- break;
+ if (input_->fields[d].name == std::string ("rgba") || input_->fields[d].name == std::string ("rgb"))
+ {
+ rgba_index = d;
+ centroid_size += 4;
+ break;
+ }
}
}
-
+
// If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first...
if (!filter_field_name_.empty ())
{
{
pcl::RGB rgb;
memcpy (&rgb, &input_->data[point_offset + input_->fields[rgba_index].offset], sizeof (RGB));
- centroid[centroid_size-3] = rgb.r;
- centroid[centroid_size-2] = rgb.g;
- centroid[centroid_size-1] = rgb.b;
+ centroid[centroid_size-4] = rgb.r;
+ centroid[centroid_size-3] = rgb.g;
+ centroid[centroid_size-2] = rgb.b;
+ centroid[centroid_size-1] = rgb.a;
}
// Copy all the fields
for (size_t d = 0; d < input_->fields.size (); ++d)
{
pcl::RGB rgb;
memcpy (&rgb, &input_->data[point_offset + input_->fields[rgba_index].offset], sizeof (RGB));
- temporary[centroid_size-3] = rgb.r;
- temporary[centroid_size-2] = rgb.g;
- temporary[centroid_size-1] = rgb.b;
+ temporary[centroid_size-4] = rgb.r;
+ temporary[centroid_size-3] = rgb.g;
+ temporary[centroid_size-2] = rgb.b;
+ temporary[centroid_size-1] = rgb.a;
}
// Copy all the fields
for (size_t d = 0; d < input_->fields.size (); ++d)
// full extra r/g/b centroid field
if (rgba_index >= 0)
{
- float r = centroid[centroid_size-3], g = centroid[centroid_size-2], b = centroid[centroid_size-1];
- int rgb = (static_cast<int> (r) << 16) | (static_cast<int> (g) << 8) | static_cast<int> (b);
+ float r = centroid[centroid_size-4], g = centroid[centroid_size-3], b = centroid[centroid_size-2], a = centroid[centroid_size-1];
+ int rgb = (static_cast<int> (a) << 24) | (static_cast<int> (r) << 16) | (static_cast<int> (g) << 8) | static_cast<int> (b);
memcpy (&output.data[point_offset + output.fields[rgba_index].offset], &rgb, sizeof (float));
}
}
int label = it->first;
if (it != labels.end ())
{
- for (it = labels.begin (); it != labels.end (); it++)
+ for (it = labels.begin (); it != labels.end (); ++it)
{
if (n_occurrence < it->second)
{
approx_polygon.reserve (result.size ());
if (refine)
{
- std::vector<Eigen::Vector3f> lines (result.size ());
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > lines (result.size ());
std::reverse (result.begin (), result.end ());
for (unsigned rIdx = 0; rIdx < result.size (); ++rIdx)
{
////////////////////////////////////////////////////////////////////////
/** \brief Removes mesh elements and data that are marked as deleted from the container.
- * \param IndexContainerT e.g. std::vector \<VertexIndex\>
- * \param ElementContainerT e.g. std::vector \<Vertex\>
- * \param DataContainerT e.g. std::vector \<VertexData\>
- * \param HasDataT Integral constant specifying if the mesh has data associated with the elements.
+ * \tparam ElementContainerT e.g. std::vector \<Vertex\>
+ * \tparam DataContainerT e.g. std::vector \<VertexData\>
+ * \tparam IndexContainerT e.g. std::vector \<VertexIndex\>
+ * \tparam HasDataT Integral constant specifying if the mesh has data associated with the elements.
+ *
* \param[in, out] elements Container for the mesh elements. Resized to the new size.
* \param[in, out] data_cloud Container for the mesh data. Resized to the new size.
* \return Container with the same size as the old input data. Holds the indices to the new elements for each non-deleted element and an invalid index if it is deleted.
--- /dev/null
+set(SUBSYS_NAME gpu)
+set(SUBSYS_DESC "Point cloud GPU libraries")
+set(SUBSYS_DEPS )
+
+OPTION(BUILD_GPU "Build the GPU-related subsystems" ${DEFAULT})
+
+if(BUILD_GPU AND CUDA_FOUND)
+
+ if(CMAKE_COMPILER_IS_GNUCXX)
+ string(REPLACE "-Wold-style-cast" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+ string(REPLACE "-Wno-invalid-offsetof" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-conversion -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function")
+ if (GCC_VERSION VERSION_GREATER 4.3)
+ SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-unused-but-set-variable")
+ endif()
+ endif()
+
+ collect_subproject_directory_names("${CMAKE_CURRENT_SOURCE_DIR}" "CMakeLists.txt" PCL_GPU_MODULES_NAMES PCL_GPU_MODULES_DIRS)
+ set(PCL_GPU_MODULES_NAMES_UNSORTED ${PCL_GPU_MODULES_NAMES})
+ topological_sort(PCL_GPU_MODULES_NAMES PCL_ _DEPENDS)
+ sort_relative(PCL_GPU_MODULES_NAMES_UNSORTED PCL_GPU_MODULES_NAMES PCL_GPU_MODULES_DIRS)
+ foreach(subdir ${PCL_GPU_MODULES_DIRS})
+ add_subdirectory("${CMAKE_CURRENT_SOURCE_DIR}/${subdir}")
+ endforeach(subdir)
+endif()
+
--- /dev/null
+set(SUBSYS_NAME gpu_containers)
+set(SUBSYS_PATH gpu/containers)
+set(SUBSYS_DESC "Containers with reference counting for GPU memory. This module can be compiled without Cuda Toolkit.")
+set(SUBSYS_DEPS common)
+
+set(build TRUE)
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS})
+PCL_SET_SUBSYS_INCLUDE_DIR("${SUBSYS_NAME}" "${SUBSYS_PATH}")
+mark_as_advanced("BUILD_${SUBSYS_NAME}")
+
+if (build)
+ FILE(GLOB srcs src/*.cpp src/*.hpp)
+ FILE(GLOB incs include/pcl/gpu/containers/*.h)
+ FILE(GLOB incs_impl include/pcl/gpu/containers/impl/*.hpp)
+
+ source_group("Header Files\\impl" FILES ${incs_impl})
+ LIST(APPEND incs ${incs_impl})
+
+ get_filename_component(UTILS_INC "${CMAKE_CURRENT_SOURCE_DIR}/../utils/include" REALPATH)
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include" ${UTILS_INC})
+ PCL_CUDA_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs})
+
+ set(EXT_DEPS "")
+ #set(EXT_DEPS CUDA)
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "")
+
+ # Install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}" ${incs})
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}/impl" ${incs_impl})
+endif()
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#ifndef PCL_GPU_CONTAINER_DEVICE_ARRAY_HPP_
+#define PCL_GPU_CONTAINER_DEVICE_ARRAY_HPP_
+
+#include <pcl/pcl_exports.h>
+#include <pcl/gpu/containers/device_memory.h>
+
+#include <vector>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b DeviceArray class
+ *
+ * \note Typed container for GPU memory with reference counting.
+ *
+ * \author Anatoly Baksheev
+ */
+ template<class T>
+ class PCL_EXPORTS DeviceArray : public DeviceMemory
+ {
+ public:
+ /** \brief Element type. */
+ typedef T type;
+
+ /** \brief Element size. */
+ enum { elem_size = sizeof(T) };
+
+ /** \brief Empty constructor. */
+ DeviceArray();
+
+ /** \brief Allocates internal buffer in GPU memory
+ * \param size number of elements to allocate
+ * */
+ DeviceArray(size_t size);
+
+ /** \brief Initializes with user allocated buffer. Reference counting is disabled in this case.
+ * \param ptr pointer to buffer
+ * \param size elemens number
+ * */
+ DeviceArray(T *ptr, size_t size);
+
+ /** \brief Copy constructor. Just increments reference counter. */
+ DeviceArray(const DeviceArray& other);
+
+ /** \brief Assigment operator. Just increments reference counter. */
+ DeviceArray& operator = (const DeviceArray& other);
+
+ /** \brief Allocates internal buffer in GPU memory. If internal buffer was created before the function recreates it with new size. If new and old sizes are equal it does nothing.
+ * \param size elemens number
+ * */
+ void create(size_t size);
+
+ /** \brief Decrements reference counter and releases internal buffer if needed. */
+ void release();
+
+ /** \brief Performs data copying. If destination size differs it will be reallocated.
+ * \param other destination container
+ * */
+ void copyTo(DeviceArray& other) const;
+
+ /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is enough.
+ * \param host_ptr pointer to buffer to upload
+ * \param size elemens number
+ * */
+ void upload(const T *host_ptr, size_t size);
+
+ /** \brief Downloads data from internal buffer to CPU memory
+ * \param host_ptr pointer to buffer to download
+ * */
+ void download(T *host_ptr) const;
+
+ /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is enough.
+ * \param data host vector to upload from
+ * */
+ template<class A>
+ void upload(const std::vector<T, A>& data);
+
+ /** \brief Downloads data from internal buffer to CPU memory
+ * \param data host vector to download to
+ * */
+ template<typename A>
+ void download(std::vector<T, A>& data) const;
+
+ /** \brief Performs swap of data pointed with another device array.
+ * \param other_arg device array to swap with
+ * */
+ void swap(DeviceArray& other_arg);
+
+ /** \brief Returns pointer for internal buffer in GPU memory. */
+ T* ptr();
+
+ /** \brief Returns const pointer for internal buffer in GPU memory. */
+ const T* ptr() const;
+
+ //using DeviceMemory::ptr;
+
+ /** \brief Returns pointer for internal buffer in GPU memory. */
+ operator T*();
+
+ /** \brief Returns const pointer for internal buffer in GPU memory. */
+ operator const T*() const;
+
+ /** \brief Returns size in elements. */
+ size_t size() const;
+ };
+
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b DeviceArray2D class
+ *
+ * \note Typed container for pitched GPU memory with reference counting.
+ *
+ * \author Anatoly Baksheev
+ */
+ template<class T>
+ class PCL_EXPORTS DeviceArray2D : public DeviceMemory2D
+ {
+ public:
+ /** \brief Element type. */
+ typedef T type;
+
+ /** \brief Element size. */
+ enum { elem_size = sizeof(T) };
+
+ /** \brief Empty constructor. */
+ DeviceArray2D();
+
+ /** \brief Allocates internal buffer in GPU memory
+ * \param rows number of rows to allocate
+ * \param cols number of elements in each row
+ * */
+ DeviceArray2D(int rows, int cols);
+
+ /** \brief Initializes with user allocated buffer. Reference counting is disabled in this case.
+ * \param rows number of rows
+ * \param cols number of elements in each row
+ * \param data pointer to buffer
+ * \param stepBytes stride between two consecutive rows in bytes
+ * */
+ DeviceArray2D(int rows, int cols, void *data, size_t stepBytes);
+
+ /** \brief Copy constructor. Just increments reference counter. */
+ DeviceArray2D(const DeviceArray2D& other);
+
+ /** \brief Assigment operator. Just increments reference counter. */
+ DeviceArray2D& operator = (const DeviceArray2D& other);
+
+ /** \brief Allocates internal buffer in GPU memory. If internal buffer was created before the function recreates it with new size. If new and old sizes are equal it does nothing.
+ * \param rows number of rows to allocate
+ * \param cols number of elements in each row
+ * */
+ void create(int rows, int cols);
+
+ /** \brief Decrements reference counter and releases internal buffer if needed. */
+ void release();
+
+ /** \brief Performs data copying. If destination size differs it will be reallocated.
+ * \param other destination container
+ * */
+ void copyTo(DeviceArray2D& other) const;
+
+ /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is enough.
+ * \param host_ptr pointer to host buffer to upload
+ * \param host_step stride between two consecutive rows in bytes for host buffer
+ * \param rows number of rows to upload
+ * \param cols number of elements in each row
+ * */
+ void upload(const void *host_ptr, size_t host_step, int rows, int cols);
+
+ /** \brief Downloads data from internal buffer to CPU memory. User is resposible for correct host buffer size.
+ * \param host_ptr pointer to host buffer to download
+ * \param host_step stride between two consecutive rows in bytes for host buffer
+ * */
+ void download(void *host_ptr, size_t host_step) const;
+
+ /** \brief Performs swap of data pointed with another device array.
+ * \param other_arg device array to swap with
+ * */
+ void swap(DeviceArray2D& other_arg);
+
+ /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is enough.
+ * \param data host vector to upload from
+ * \param cols stride in elements between two consecutive rows for host buffer
+ * */
+ template<class A>
+ void upload(const std::vector<T, A>& data, int cols);
+
+ /** \brief Downloads data from internal buffer to CPU memory
+ * \param data host vector to download to
+ * \param cols Output stride in elements between two consecutive rows for host vector.
+ * */
+ template<class A>
+ void download(std::vector<T, A>& data, int& cols) const;
+
+ /** \brief Returns pointer to given row in internal buffer.
+ * \param y row index
+ * */
+ T* ptr(int y = 0);
+
+ /** \brief Returns const pointer to given row in internal buffer.
+ * \param y row index
+ * */
+ const T* ptr(int y = 0) const;
+
+ //using DeviceMemory2D::ptr;
+
+ /** \brief Returns pointer for internal buffer in GPU memory. */
+ operator T*();
+
+ /** \brief Returns const pointer for internal buffer in GPU memory. */
+ operator const T*() const;
+
+ /** \brief Returns number of elements in each row. */
+ int cols() const;
+
+ /** \brief Returns number of rows. */
+ int rows() const;
+
+ /** \brief Returns step in elements. */
+ size_t elem_step() const;
+ };
+ }
+
+ namespace device
+ {
+ using pcl::gpu::DeviceArray;
+ using pcl::gpu::DeviceArray2D;
+ }
+}
+
+#include <pcl/gpu/containers/impl/device_array.hpp>
+
+#endif /* PCL_GPU_CONTAINER_DEVICE_ARRAY_HPP_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#ifndef PCL_GPU_CONTAINERS_DEVICE_MEMORY_HPP_
+#define PCL_GPU_CONTAINERS_DEVICE_MEMORY_HPP_
+
+#include <pcl/pcl_exports.h>
+#include <pcl/gpu/containers/kernel_containers.h>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b DeviceMemory class
+ *
+ * \note This is a BLOB container class with reference counting for GPU memory.
+ *
+ * \author Anatoly Baksheev
+ */
+
+ class PCL_EXPORTS DeviceMemory
+ {
+ public:
+ /** \brief Empty constructor. */
+ DeviceMemory();
+
+ /** \brief Destructor. */
+ ~DeviceMemory();
+
+ /** \brief Allocates internal buffer in GPU memory
+ * \param sizeBytes_arg amount of memory to allocate
+ * */
+ DeviceMemory(size_t sizeBytes_arg);
+
+ /** \brief Initializes with user allocated buffer. Reference counting is disabled in this case.
+ * \param ptr_arg pointer to buffer
+ * \param sizeBytes_arg buffer size
+ * */
+ DeviceMemory(void *ptr_arg, size_t sizeBytes_arg);
+
+ /** \brief Copy constructor. Just increments reference counter. */
+ DeviceMemory(const DeviceMemory& other_arg);
+
+ /** \brief Assigment operator. Just increments reference counter. */
+ DeviceMemory& operator=(const DeviceMemory& other_arg);
+
+ /** \brief Allocates internal buffer in GPU memory. If internal buffer was created before the function recreates it with new size. If new and old sizes are equal it does nothing.
+ * \param sizeBytes_arg buffer size
+ * */
+ void create(size_t sizeBytes_arg);
+
+ /** \brief Decrements reference counter and releases internal buffer if needed. */
+ void release();
+
+ /** \brief Performs data copying. If destination size differs it will be reallocated.
+ * \param other destination container
+ * */
+ void copyTo(DeviceMemory& other) const;
+
+ /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is enough.
+ * \param host_ptr_arg pointer to buffer to upload
+ * \param sizeBytes_arg buffer size
+ * */
+ void upload(const void *host_ptr_arg, size_t sizeBytes_arg);
+
+ /** \brief Downloads data from internal buffer to CPU memory
+ * \param host_ptr_arg pointer to buffer to download
+ * */
+ void download(void *host_ptr_arg) const;
+
+ /** \brief Performs swap of data pointed with another device memory.
+ * \param other_arg device memory to swap with
+ * */
+ void swap(DeviceMemory& other_arg);
+
+ /** \brief Returns pointer for internal buffer in GPU memory. */
+ template<class T> T* ptr();
+
+ /** \brief Returns constant pointer for internal buffer in GPU memory. */
+ template<class T> const T* ptr() const;
+
+ /** \brief Conversion to PtrSz for passing to kernel functions. */
+ template <class U> operator PtrSz<U>() const;
+
+ /** \brief Returns true if unallocated otherwise false. */
+ bool empty() const;
+
+ size_t sizeBytes() const;
+
+ private:
+ /** \brief Device pointer. */
+ void *data_;
+
+ /** \brief Allocated size in bytes. */
+ size_t sizeBytes_;
+
+ /** \brief Pointer to reference counter in CPU memory. */
+ int* refcount_;
+ };
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b DeviceMemory2D class
+ *
+ * \note This is a BLOB container class with reference counting for pitched GPU memory.
+ *
+ * \author Anatoly Baksheev
+ */
+
+ class PCL_EXPORTS DeviceMemory2D
+ {
+ public:
+ /** \brief Empty constructor. */
+ DeviceMemory2D();
+
+ /** \brief Destructor. */
+ ~DeviceMemory2D();
+
+ /** \brief Allocates internal buffer in GPU memory
+ * \param rows_arg number of rows to allocate
+ * \param colsBytes_arg width of the buffer in bytes
+ * */
+ DeviceMemory2D(int rows_arg, int colsBytes_arg);
+
+
+ /** \brief Initializes with user allocated buffer. Reference counting is disabled in this case.
+ * \param rows_arg number of rows
+ * \param colsBytes_arg width of the buffer in bytes
+ * \param data_arg pointer to buffer
+ * \param step_arg stride between two consecutive rows in bytes
+ * */
+ DeviceMemory2D(int rows_arg, int colsBytes_arg, void *data_arg, size_t step_arg);
+
+ /** \brief Copy constructor. Just increments reference counter. */
+ DeviceMemory2D(const DeviceMemory2D& other_arg);
+
+ /** \brief Assigment operator. Just increments reference counter. */
+ DeviceMemory2D& operator=(const DeviceMemory2D& other_arg);
+
+ /** \brief Allocates internal buffer in GPU memory. If internal buffer was created before the function recreates it with new size. If new and old sizes are equal it does nothing.
+ * \param rows_arg number of rows to allocate
+ * \param colsBytes_arg width of the buffer in bytes
+ * */
+ void create(int rows_arg, int colsBytes_arg);
+
+ /** \brief Decrements reference counter and releases internal buffer if needed. */
+ void release();
+
+ /** \brief Performs data copying. If destination size differs it will be reallocated.
+ * \param other destination container
+ * */
+ void copyTo(DeviceMemory2D& other) const;
+
+ /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is enough.
+ * \param host_ptr_arg pointer to host buffer to upload
+ * \param host_step_arg stride between two consecutive rows in bytes for host buffer
+ * \param rows_arg number of rows to upload
+ * \param colsBytes_arg width of host buffer in bytes
+ * */
+ void upload(const void *host_ptr_arg, size_t host_step_arg, int rows_arg, int colsBytes_arg);
+
+ /** \brief Downloads data from internal buffer to CPU memory. User is resposible for correct host buffer size.
+ * \param host_ptr_arg pointer to host buffer to download
+ * \param host_step_arg stride between two consecutive rows in bytes for host buffer
+ * */
+ void download(void *host_ptr_arg, size_t host_step_arg) const;
+
+ /** \brief Performs swap of data pointed with another device memory.
+ * \param other_arg device memory to swap with
+ * */
+ void swap(DeviceMemory2D& other_arg);
+
+ /** \brief Returns pointer to given row in internal buffer.
+ * \param y_arg row index
+ * */
+ template<class T> T* ptr(int y_arg = 0);
+
+ /** \brief Returns constant pointer to given row in internal buffer.
+ * \param y_arg row index
+ * */
+ template<class T> const T* ptr(int y_arg = 0) const;
+
+ /** \brief Conversion to PtrStep for passing to kernel functions. */
+ template <class U> operator PtrStep<U>() const;
+
+ /** \brief Conversion to PtrStepSz for passing to kernel functions. */
+ template <class U> operator PtrStepSz<U>() const;
+
+ /** \brief Returns true if unallocated otherwise false. */
+ bool empty() const;
+
+ /** \brief Returns number of bytes in each row. */
+ int colsBytes() const;
+
+ /** \brief Returns number of rows. */
+ int rows() const;
+
+ /** \brief Returns stride between two consecutive rows in bytes for internal buffer. Step is stored always and everywhere in bytes!!! */
+ size_t step() const;
+ private:
+ /** \brief Device pointer. */
+ void *data_;
+
+ /** \brief Stride between two consecutive rows in bytes for internal buffer. Step is stored always and everywhere in bytes!!! */
+ size_t step_;
+
+ /** \brief Width of the buffer in bytes. */
+ int colsBytes_;
+
+ /** \brief Number of rows. */
+ int rows_;
+
+ /** \brief Pointer to reference counter in CPU memory. */
+ int* refcount_;
+ };
+ }
+
+ namespace device
+ {
+ using pcl::gpu::DeviceMemory;
+ using pcl::gpu::DeviceMemory2D;
+ }
+}
+
+#include <pcl/gpu/containers/impl/device_memory.hpp>
+
+#endif /* PCL_GPU_CONTAINERS_DEVICE_MEMORY_HPP_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#ifndef PCL_GPU_CONTAINER_DEVICE_ARRAY_IMPL_HPP_
+#define PCL_GPU_CONTAINER_DEVICE_ARRAY_IMPL_HPP_
+
+
+///////////////////// Inline implementations of DeviceArray ////////////////////////////////////////////
+
+template<class T> inline pcl::gpu::DeviceArray<T>::DeviceArray() {}
+template<class T> inline pcl::gpu::DeviceArray<T>::DeviceArray(size_t size) : DeviceMemory(size * elem_size) {}
+template<class T> inline pcl::gpu::DeviceArray<T>::DeviceArray(T *ptr, size_t size) : DeviceMemory(ptr, size * elem_size) {}
+template<class T> inline pcl::gpu::DeviceArray<T>::DeviceArray(const DeviceArray& other) : DeviceMemory(other) {}
+template<class T> inline pcl::gpu::DeviceArray<T>& pcl::gpu::DeviceArray<T>::operator=(const DeviceArray& other)
+{ DeviceMemory::operator=(other); return *this; }
+
+template<class T> inline void pcl::gpu::DeviceArray<T>::create(size_t size)
+{ DeviceMemory::create(size * elem_size); }
+template<class T> inline void pcl::gpu::DeviceArray<T>::release()
+{ DeviceMemory::release(); }
+
+template<class T> inline void pcl::gpu::DeviceArray<T>::copyTo(DeviceArray& other) const
+{ DeviceMemory::copyTo(other); }
+template<class T> inline void pcl::gpu::DeviceArray<T>::upload(const T *host_ptr, size_t size)
+{ DeviceMemory::upload(host_ptr, size * elem_size); }
+template<class T> inline void pcl::gpu::DeviceArray<T>::download(T *host_ptr) const
+{ DeviceMemory::download( host_ptr ); }
+
+template<class T> void pcl::gpu::DeviceArray<T>::swap(DeviceArray& other_arg) { DeviceMemory::swap(other_arg); }
+
+template<class T> inline pcl::gpu::DeviceArray<T>::operator T*() { return ptr(); }
+template<class T> inline pcl::gpu::DeviceArray<T>::operator const T*() const { return ptr(); }
+template<class T> inline size_t pcl::gpu::DeviceArray<T>::size() const { return sizeBytes() / elem_size; }
+
+template<class T> inline T* pcl::gpu::DeviceArray<T>::ptr() { return DeviceMemory::ptr<T>(); }
+template<class T> inline const T* pcl::gpu::DeviceArray<T>::ptr() const { return DeviceMemory::ptr<T>(); }
+
+template<class T> template<class A> inline void pcl::gpu::DeviceArray<T>::upload(const std::vector<T, A>& data) { upload(&data[0], data.size()); }
+template<class T> template<class A> inline void pcl::gpu::DeviceArray<T>::download(std::vector<T, A>& data) const { data.resize(size()); if (!data.empty()) download(&data[0]); }
+
+///////////////////// Inline implementations of DeviceArray2D ////////////////////////////////////////////
+
+template<class T> inline pcl::gpu::DeviceArray2D<T>::DeviceArray2D() {}
+template<class T> inline pcl::gpu::DeviceArray2D<T>::DeviceArray2D(int rows, int cols) : DeviceMemory2D(rows, cols * elem_size) {}
+template<class T> inline pcl::gpu::DeviceArray2D<T>::DeviceArray2D(int rows, int cols, void *data, size_t stepBytes) : DeviceMemory2D(rows, cols * elem_size, data, stepBytes) {}
+template<class T> inline pcl::gpu::DeviceArray2D<T>::DeviceArray2D(const DeviceArray2D& other) : DeviceMemory2D(other) {}
+template<class T> inline pcl::gpu::DeviceArray2D<T>& pcl::gpu::DeviceArray2D<T>::operator=(const DeviceArray2D& other)
+{ DeviceMemory2D::operator=(other); return *this; }
+
+template<class T> inline void pcl::gpu::DeviceArray2D<T>::create(int rows, int cols)
+{ DeviceMemory2D::create(rows, cols * elem_size); }
+template<class T> inline void pcl::gpu::DeviceArray2D<T>::release()
+{ DeviceMemory2D::release(); }
+
+template<class T> inline void pcl::gpu::DeviceArray2D<T>::copyTo(DeviceArray2D& other) const
+{ DeviceMemory2D::copyTo(other); }
+template<class T> inline void pcl::gpu::DeviceArray2D<T>::upload(const void *host_ptr, size_t host_step, int rows, int cols)
+{ DeviceMemory2D::upload(host_ptr, host_step, rows, cols * elem_size); }
+template<class T> inline void pcl::gpu::DeviceArray2D<T>::download(void *host_ptr, size_t host_step) const
+{ DeviceMemory2D::download( host_ptr, host_step ); }
+
+template<class T> template<class A> inline void pcl::gpu::DeviceArray2D<T>::upload(const std::vector<T, A>& data, int cols)
+{ upload(&data[0], cols * elem_size, data.size()/cols, cols); }
+
+template<class T> template<class A> inline void pcl::gpu::DeviceArray2D<T>::download(std::vector<T, A>& data, int& elem_step) const
+{ elem_step = cols(); data.resize(cols() * rows()); if (!data.empty()) download(&data[0], colsBytes()); }
+
+template<class T> void pcl::gpu::DeviceArray2D<T>::swap(DeviceArray2D& other_arg) { DeviceMemory2D::swap(other_arg); }
+
+template<class T> inline T* pcl::gpu::DeviceArray2D<T>::ptr(int y) { return DeviceMemory2D::ptr<T>(y); }
+template<class T> inline const T* pcl::gpu::DeviceArray2D<T>::ptr(int y) const { return DeviceMemory2D::ptr<T>(y); }
+
+template<class T> inline pcl::gpu::DeviceArray2D<T>::operator T*() { return ptr(); }
+template<class T> inline pcl::gpu::DeviceArray2D<T>::operator const T*() const { return ptr(); }
+
+template<class T> inline int pcl::gpu::DeviceArray2D<T>::cols() const { return DeviceMemory2D::colsBytes()/elem_size; }
+template<class T> inline int pcl::gpu::DeviceArray2D<T>::rows() const { return DeviceMemory2D::rows(); }
+
+template<class T> inline size_t pcl::gpu::DeviceArray2D<T>::elem_step() const { return DeviceMemory2D::step()/elem_size; }
+
+
+#endif /* PCL_GPU_CONTAINER_DEVICE_ARRAY_IMPL_HPP_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#ifndef PCL_GPU_CONTAINER_DEVICE_MEMORY_IMPL_HPP_
+#define PCL_GPU_CONTAINER_DEVICE_MEMORY_IMPL_HPP_
+
+///////////////////// Inline implementations of DeviceMemory ////////////////////////////////////////////
+
+template<class T> inline T* pcl::gpu::DeviceMemory::ptr() { return ( T*)data_; }
+template<class T> inline const T* pcl::gpu::DeviceMemory::ptr() const { return (const T*)data_; }
+
+template <class U> inline pcl::gpu::DeviceMemory::operator pcl::gpu::PtrSz<U>() const
+{
+ PtrSz<U> result;
+ result.data = (U*)ptr<U>();
+ result.size = sizeBytes_/sizeof(U);
+ return result;
+}
+
+///////////////////// Inline implementations of DeviceMemory2D ////////////////////////////////////////////
+
+template<class T> T* pcl::gpu::DeviceMemory2D::ptr(int y_arg) { return ( T*)(( char*)data_ + y_arg * step_); }
+template<class T> const T* pcl::gpu::DeviceMemory2D::ptr(int y_arg) const { return (const T*)((const char*)data_ + y_arg * step_); }
+
+template <class U> pcl::gpu::DeviceMemory2D::operator pcl::gpu::PtrStep<U>() const
+{
+ PtrStep<U> result;
+ result.data = (U*)ptr<U>();
+ result.step = step_;
+ return result;
+}
+
+template <class U> pcl::gpu::DeviceMemory2D::operator pcl::gpu::PtrStepSz<U>() const
+{
+ PtrStepSz<U> result;
+ result.data = (U*)ptr<U>();
+ result.step = step_;
+ result.cols = colsBytes_/sizeof(U);
+ result.rows = rows_;
+ return result;
+}
+
+#endif /* PCL_GPU_CONTAINER_DEVICE_MEMORY_IMPL_HPP_ */
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#ifndef PCL_GPU_CONTAINERS_INITIALISATION_H_
+#define PCL_GPU_CONTAINERS_INITIALISATION_H_
+
+#include <pcl/pcl_exports.h>
+#include <string>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ /** \brief Returns number of Cuda device. */
+ PCL_EXPORTS int getCudaEnabledDeviceCount();
+
+ /** \brief Sets active device to work with. */
+ PCL_EXPORTS void setDevice(int device);
+
+ /** \brief Return devuce name for gived device. */
+ PCL_EXPORTS std::string getDeviceName(int device);
+
+ /** \brief Prints infromatoin about given cuda deivce or about all deivces
+ * \param device: if < 0 prints info for all devices, otherwise the function interpets is as device id.
+ */
+ void PCL_EXPORTS printCudaDeviceInfo(int device = -1);
+
+ /** \brief Prints infromatoin about given cuda deivce or about all deivces
+ * \param device: if < 0 prints info for all devices, otherwise the function interpets is as device id.
+ */
+ void PCL_EXPORTS printShortCudaDeviceInfo(int device = -1);
+
+ /** \brief Returns true if pre-Fermi generaton GPU.
+ * \param device: device id to check, if < 0 checks current device.
+ */
+ bool PCL_EXPORTS checkIfPreFermiGPU(int device = -1);
+
+ /** \brief Error handler. All GPU functions call this to report an error. For internal use only */
+ void PCL_EXPORTS error(const char *error_string, const char *file, const int line, const char *func = "");
+ }
+}
+
+#endif /* PCL_GPU_CONTAINERS_INITIALISATION_H_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+
+#ifndef PCL_GPU_CONTAINERS_KERNEL_CONTAINERS_HPP_
+#define PCL_GPU_CONTAINERS_KERNEL_CONTAINERS_HPP_
+
+
+#if defined(__CUDACC__)
+ #define __PCL_GPU_HOST_DEVICE__ __host__ __device__ __forceinline__
+#else
+ #define __PCL_GPU_HOST_DEVICE__
+#endif
+
+#include <cstddef>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ template<typename T> struct DevPtr
+ {
+ typedef T elem_type;
+ const static size_t elem_size = sizeof(elem_type);
+
+ T* data;
+
+ __PCL_GPU_HOST_DEVICE__ DevPtr() : data(0) {}
+ __PCL_GPU_HOST_DEVICE__ DevPtr(T* data_arg) : data(data_arg) {}
+
+ __PCL_GPU_HOST_DEVICE__ size_t elemSize() const { return elem_size; }
+ __PCL_GPU_HOST_DEVICE__ operator T*() { return data; }
+ __PCL_GPU_HOST_DEVICE__ operator const T*() const { return data; }
+ };
+
+ template<typename T> struct PtrSz : public DevPtr<T>
+ {
+ __PCL_GPU_HOST_DEVICE__ PtrSz() : size(0) {}
+ __PCL_GPU_HOST_DEVICE__ PtrSz(T* data_arg, size_t size_arg) : DevPtr<T>(data_arg), size(size_arg) {}
+
+ size_t size;
+ };
+
+ template<typename T> struct PtrStep : public DevPtr<T>
+ {
+ __PCL_GPU_HOST_DEVICE__ PtrStep() : step(0) {}
+ __PCL_GPU_HOST_DEVICE__ PtrStep(T* data_arg, size_t step_arg) : DevPtr<T>(data_arg), step(step_arg) {}
+
+ /** \brief stride between two consecutive rows in bytes. Step is stored always and everywhere in bytes!!! */
+ size_t step;
+
+ __PCL_GPU_HOST_DEVICE__ T* ptr(int y = 0) { return ( T*)( ( char*)DevPtr<T>::data + y * step); }
+ __PCL_GPU_HOST_DEVICE__ const T* ptr(int y = 0) const { return (const T*)( (const char*)DevPtr<T>::data + y * step); }
+
+ __PCL_GPU_HOST_DEVICE__ T& operator()(int y, int x) { return ptr(y)[x]; }
+ __PCL_GPU_HOST_DEVICE__ const T& operator()(int y, int x) const { return ptr(y)[x]; }
+ };
+
+ template <typename T> struct PtrStepSz : public PtrStep<T>
+ {
+ __PCL_GPU_HOST_DEVICE__ PtrStepSz() : cols(0), rows(0) {}
+ __PCL_GPU_HOST_DEVICE__ PtrStepSz(int rows_arg, int cols_arg, T* data_arg, size_t step_arg)
+ : PtrStep<T>(data_arg, step_arg), cols(cols_arg), rows(rows_arg) {}
+
+ int cols;
+ int rows;
+ };
+ }
+
+ namespace device
+ {
+ using pcl::gpu::PtrSz;
+ using pcl::gpu::PtrStep;
+ using pcl::gpu::PtrStepSz;
+ }
+}
+
+#undef __PCL_GPU_HOST_DEVICE__
+
+#endif /* PCL_GPU_CONTAINERS_KERNEL_CONTAINERS_HPP_ */
+
--- /dev/null
+ /*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include <pcl/gpu/containers/device_memory.h>
+#include <pcl/gpu/utils/safe_call.hpp>
+
+#include "cuda_runtime_api.h"
+#include "assert.h"
+
+#define HAVE_CUDA
+//#include <pcl_config.h>
+
+#if !defined(HAVE_CUDA)
+
+void throw_nogpu() { throw "PCL 2.0 exception"; }
+
+pcl::gpu::DeviceMemory::DeviceMemory() { throw_nogpu(); }
+pcl::gpu::DeviceMemory::DeviceMemory(void *, size_t) { throw_nogpu(); }
+pcl::gpu::DeviceMemory::DeviceMemory(size_t) { throw_nogpu(); }
+pcl::gpu::DeviceMemory::~DeviceMemory() { throw_nogpu(); }
+pcl::gpu::DeviceMemory::DeviceMemory(const DeviceMemory& ) { throw_nogpu(); }
+pcl::gpu::DeviceMemory& pcl::gpu::DeviceMemory::operator=(const pcl::gpu::DeviceMemory&) { throw_nogpu(); return *this;}
+void pcl::gpu::DeviceMemory::create(size_t) { throw_nogpu(); }
+void pcl::gpu::DeviceMemory::release() { throw_nogpu(); }
+void pcl::gpu::DeviceMemory::copyTo(DeviceMemory&) const { throw_nogpu(); }
+void pcl::gpu::DeviceMemory::upload(const void*, size_t) { throw_nogpu(); }
+void pcl::gpu::DeviceMemory::download(void*) const { throw_nogpu(); }
+bool pcl::gpu::DeviceMemory::empty() const { throw_nogpu(); }
+pcl::gpu::DeviceMemory2D::DeviceMemory2D() { throw_nogpu(); }
+pcl::gpu::DeviceMemory2D::DeviceMemory2D(int, int) { throw_nogpu(); }
+pcl::gpu::DeviceMemory2D::DeviceMemory2D(int, int, void*, size_t) { throw_nogpu(); }
+pcl::gpu::DeviceMemory2D::~DeviceMemory2D() { throw_nogpu(); }
+pcl::gpu::DeviceMemory2D::DeviceMemory2D(const DeviceMemory2D&) { throw_nogpu(); }
+pcl::gpu::DeviceMemory2D& pcl::gpu::DeviceMemory2D::operator=(const pcl::gpu::DeviceMemory2D&) { throw_nogpu(); return *this;}
+void pcl::gpu::DeviceMemory2D::create(int, int ) { throw_nogpu(); }
+void pcl::gpu::DeviceMemory2D::release() { throw_nogpu(); }
+void pcl::gpu::DeviceMemory2D::copyTo(DeviceMemory2D&) const { throw_nogpu(); }
+void pcl::gpu::DeviceMemory2D::upload(const void *, size_t, int, int ) { throw_nogpu(); }
+void pcl::gpu::DeviceMemory2D::download(void *, size_t ) const { throw_nogpu(); }
+bool pcl::gpu::DeviceMemory2D::empty() const { throw_nogpu(); }
+
+#else
+
+////////////////////////// XADD ///////////////////////////////
+
+#ifdef __GNUC__
+
+ #if __GNUC__*10 + __GNUC_MINOR__ >= 42
+
+ #if !defined WIN32 && (defined __i486__ || defined __i586__ || defined __i686__ || defined __MMX__ || defined __SSE__ || defined __ppc__)
+ #define CV_XADD __sync_fetch_and_add
+ #else
+ #include <ext/atomicity.h>
+ #define CV_XADD __gnu_cxx::__exchange_and_add
+ #endif
+ #else
+ #include <bits/atomicity.h>
+ #if __GNUC__*10 + __GNUC_MINOR__ >= 34
+ #define CV_XADD __gnu_cxx::__exchange_and_add
+ #else
+ #define CV_XADD __exchange_and_add
+ #endif
+ #endif
+
+#elif defined WIN32 || defined _WIN32
+ #include <intrin.h>
+ #define CV_XADD(addr,delta) _InterlockedExchangeAdd((long volatile*)(addr), (delta))
+#else
+
+ template<typename _Tp> static inline _Tp CV_XADD(_Tp* addr, _Tp delta)
+ { int tmp = *addr; *addr += delta; return tmp; }
+
+#endif
+
+//////////////////////// DeviceArray /////////////////////////////
+
+pcl::gpu::DeviceMemory::DeviceMemory() : data_(0), sizeBytes_(0), refcount_(0) {}
+pcl::gpu::DeviceMemory::DeviceMemory(void *ptr_arg, size_t sizeBytes_arg) : data_(ptr_arg), sizeBytes_(sizeBytes_arg), refcount_(0){}
+pcl::gpu::DeviceMemory::DeviceMemory(size_t sizeBtes_arg) : data_(0), sizeBytes_(0), refcount_(0) { create(sizeBtes_arg); }
+pcl::gpu::DeviceMemory::~DeviceMemory() { release(); }
+
+pcl::gpu::DeviceMemory::DeviceMemory(const DeviceMemory& other_arg)
+ : data_(other_arg.data_), sizeBytes_(other_arg.sizeBytes_), refcount_(other_arg.refcount_)
+{
+ if( refcount_ )
+ CV_XADD(refcount_, 1);
+}
+
+pcl::gpu::DeviceMemory& pcl::gpu::DeviceMemory::operator = (const pcl::gpu::DeviceMemory& other_arg)
+{
+ if( this != &other_arg )
+ {
+ if( other_arg.refcount_ )
+ CV_XADD(other_arg.refcount_, 1);
+ release();
+
+ data_ = other_arg.data_;
+ sizeBytes_ = other_arg.sizeBytes_;
+ refcount_ = other_arg.refcount_;
+ }
+ return *this;
+}
+
+void pcl::gpu::DeviceMemory::create(size_t sizeBytes_arg)
+{
+ if (sizeBytes_arg == sizeBytes_)
+ return;
+
+ if( sizeBytes_arg > 0)
+ {
+ if( data_ )
+ release();
+
+ sizeBytes_ = sizeBytes_arg;
+
+ cudaSafeCall( cudaMalloc(&data_, sizeBytes_) );
+
+ //refcount_ = (int*)cv::fastMalloc(sizeof(*refcount_));
+ refcount_ = new int;
+ *refcount_ = 1;
+ }
+}
+
+void pcl::gpu::DeviceMemory::copyTo(DeviceMemory& other) const
+{
+ if (empty())
+ other.release();
+ else
+ {
+ other.create(sizeBytes_);
+ cudaSafeCall( cudaMemcpy(other.data_, data_, sizeBytes_, cudaMemcpyDeviceToDevice) );
+ cudaSafeCall( cudaDeviceSynchronize() );
+ }
+}
+
+void pcl::gpu::DeviceMemory::release()
+{
+ if( refcount_ && CV_XADD(refcount_, -1) == 1 )
+ {
+ //cv::fastFree(refcount);
+ delete refcount_;
+ cudaSafeCall( cudaFree(data_) );
+ }
+ data_ = 0;
+ sizeBytes_ = 0;
+ refcount_ = 0;
+}
+
+void pcl::gpu::DeviceMemory::upload(const void *host_ptr_arg, size_t sizeBytes_arg)
+{
+ create(sizeBytes_arg);
+ cudaSafeCall( cudaMemcpy(data_, host_ptr_arg, sizeBytes_, cudaMemcpyHostToDevice) );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
+
+void pcl::gpu::DeviceMemory::download(void *host_ptr_arg) const
+{
+ cudaSafeCall( cudaMemcpy(host_ptr_arg, data_, sizeBytes_, cudaMemcpyDeviceToHost) );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
+
+void pcl::gpu::DeviceMemory::swap(DeviceMemory& other_arg)
+{
+ std::swap(data_, other_arg.data_);
+ std::swap(sizeBytes_, other_arg.sizeBytes_);
+ std::swap(refcount_, other_arg.refcount_);
+}
+
+bool pcl::gpu::DeviceMemory::empty() const { return !data_; }
+size_t pcl::gpu::DeviceMemory::sizeBytes() const { return sizeBytes_; }
+
+
+//////////////////////// DeviceArray2D /////////////////////////////
+
+pcl::gpu::DeviceMemory2D::DeviceMemory2D() : data_(0), step_(0), colsBytes_(0), rows_(0), refcount_(0) {}
+
+pcl::gpu::DeviceMemory2D::DeviceMemory2D(int rows_arg, int colsBytes_arg)
+ : data_(0), step_(0), colsBytes_(0), rows_(0), refcount_(0)
+{
+ create(rows_arg, colsBytes_arg);
+}
+
+pcl::gpu::DeviceMemory2D::DeviceMemory2D(int rows_arg, int colsBytes_arg, void *data_arg, size_t step_arg)
+ : data_(data_arg), step_(step_arg), colsBytes_(colsBytes_arg), rows_(rows_arg), refcount_(0) {}
+
+pcl::gpu::DeviceMemory2D::~DeviceMemory2D() { release(); }
+
+
+pcl::gpu::DeviceMemory2D::DeviceMemory2D(const DeviceMemory2D& other_arg) :
+ data_(other_arg.data_), step_(other_arg.step_), colsBytes_(other_arg.colsBytes_), rows_(other_arg.rows_), refcount_(other_arg.refcount_)
+{
+ if( refcount_ )
+ CV_XADD(refcount_, 1);
+}
+
+pcl::gpu::DeviceMemory2D& pcl::gpu::DeviceMemory2D::operator = (const pcl::gpu::DeviceMemory2D& other_arg)
+{
+ if( this != &other_arg )
+ {
+ if( other_arg.refcount_ )
+ CV_XADD(other_arg.refcount_, 1);
+ release();
+
+ colsBytes_ = other_arg.colsBytes_;
+ rows_ = other_arg.rows_;
+ data_ = other_arg.data_;
+ step_ = other_arg.step_;
+
+ refcount_ = other_arg.refcount_;
+ }
+ return *this;
+}
+
+void pcl::gpu::DeviceMemory2D::create(int rows_arg, int colsBytes_arg)
+{
+ if (colsBytes_ == colsBytes_arg && rows_ == rows_arg)
+ return;
+
+ if( rows_arg > 0 && colsBytes_arg > 0)
+ {
+ if( data_ )
+ release();
+
+ colsBytes_ = colsBytes_arg;
+ rows_ = rows_arg;
+
+ cudaSafeCall( cudaMallocPitch( (void**)&data_, &step_, colsBytes_, rows_) );
+
+ //refcount = (int*)cv::fastMalloc(sizeof(*refcount));
+ refcount_ = new int;
+ *refcount_ = 1;
+ }
+}
+
+void pcl::gpu::DeviceMemory2D::release()
+{
+ if( refcount_ && CV_XADD(refcount_, -1) == 1 )
+ {
+ //cv::fastFree(refcount);
+ delete refcount_;
+ cudaSafeCall( cudaFree(data_) );
+ }
+
+ colsBytes_ = 0;
+ rows_ = 0;
+ data_ = 0;
+ step_ = 0;
+ refcount_ = 0;
+}
+
+void pcl::gpu::DeviceMemory2D::copyTo(DeviceMemory2D& other) const
+{
+ if (empty())
+ other.release();
+ else
+ {
+ other.create(rows_, colsBytes_);
+ cudaSafeCall( cudaMemcpy2D(other.data_, other.step_, data_, step_, colsBytes_, rows_, cudaMemcpyDeviceToDevice) );
+ cudaSafeCall( cudaDeviceSynchronize() );
+ }
+}
+
+void pcl::gpu::DeviceMemory2D::upload(const void *host_ptr_arg, size_t host_step_arg, int rows_arg, int colsBytes_arg)
+{
+ create(rows_arg, colsBytes_arg);
+ cudaSafeCall( cudaMemcpy2D(data_, step_, host_ptr_arg, host_step_arg, colsBytes_, rows_, cudaMemcpyHostToDevice) );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
+
+void pcl::gpu::DeviceMemory2D::download(void *host_ptr_arg, size_t host_step_arg) const
+{
+ cudaSafeCall( cudaMemcpy2D(host_ptr_arg, host_step_arg, data_, step_, colsBytes_, rows_, cudaMemcpyDeviceToHost) );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
+
+void pcl::gpu::DeviceMemory2D::swap(DeviceMemory2D& other_arg)
+{
+ std::swap(data_, other_arg.data_);
+ std::swap(step_, other_arg.step_);
+
+ std::swap(colsBytes_, other_arg.colsBytes_);
+ std::swap(rows_, other_arg.rows_);
+ std::swap(refcount_, other_arg.refcount_);
+}
+
+bool pcl::gpu::DeviceMemory2D::empty() const { return !data_; }
+int pcl::gpu::DeviceMemory2D::colsBytes() const { return colsBytes_; }
+int pcl::gpu::DeviceMemory2D::rows() const { return rows_; }
+size_t pcl::gpu::DeviceMemory2D::step() const { return step_; }
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include <pcl/gpu/containers/initialization.h>
+
+#include <iostream>
+#include <stdlib.h>
+
+void pcl::gpu::error(const char *error_string, const char *file, const int line, const char *func)
+{
+ std::cout << "Error: " << error_string << "\t" << file << ":" << line << std::endl;
+ exit(0);
+}
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#include <pcl/gpu/containers/initialization.h>
+#include <pcl/gpu/utils/safe_call.hpp>
+
+#include "cuda.h"
+#include <stdio.h>
+
+#define HAVE_CUDA
+//#include <pcl_config.h>
+
+
+#if !defined(HAVE_CUDA)
+
+void throw_nogpu() { throw "PCL 2.0 exception"; }
+int pcl::gpu::getCudaEnabledDeviceCount() { return 0; }
+void pcl::gpu::setDevice(int /*device*/) { throw_nogpu(); }
+std::string pcl::gpu::getDeviceName(int /*device*/) { throw_nogpu(); }
+void pcl::gpu::printCudaDeviceInfo(int /*deivce*/){ throw_nogpu(); }
+void pcl::gpu::printShortCudaDeviceInfo(int /*device*/) { throw_nogpu(); }
+
+#else
+
+
+int pcl::gpu::getCudaEnabledDeviceCount()
+{
+ int count;
+ cudaError_t error = cudaGetDeviceCount( &count );
+
+ if (error == cudaErrorInsufficientDriver)
+ return -1;
+
+ if (error == cudaErrorNoDevice)
+ return 0;
+
+ cudaSafeCall(error);
+ return count;
+}
+
+void pcl::gpu::setDevice(int device)
+{
+ cudaSafeCall( cudaSetDevice( device ) );
+}
+
+std::string pcl::gpu::getDeviceName(int device)
+{
+ cudaDeviceProp prop;
+ cudaSafeCall( cudaGetDeviceProperties(&prop, device) );
+
+ return prop.name;
+}
+
+bool pcl::gpu::checkIfPreFermiGPU(int device)
+{
+ if (device < 0)
+ cudaSafeCall( cudaGetDevice(&device) );
+
+ cudaDeviceProp prop;
+ cudaSafeCall( cudaGetDeviceProperties(&prop, device) );
+ return prop.major < 2; // CC == 1.x
+}
+
+namespace
+{
+ template <class T> inline void getCudaAttribute(T *attribute, CUdevice_attribute device_attribute, int device)
+ {
+ *attribute = T();
+ CUresult error = CUDA_SUCCESS;// = cuDeviceGetAttribute( attribute, device_attribute, device );
+ if( CUDA_SUCCESS == error )
+ return;
+
+ printf("Driver API error = %04d\n", error);
+ pcl::gpu::error("driver API error", __FILE__, __LINE__);
+ }
+
+ inline int convertSMVer2Cores(int major, int minor)
+ {
+ // Defines for GPU Architecture types (using the SM version to determine the # of cores per SM
+ typedef struct {
+ int SM; // 0xMm (hexidecimal notation), M = SM Major version, and m = SM minor version
+ int Cores;
+ } SMtoCores;
+
+ SMtoCores gpuArchCoresPerSM[] = { { 0x10, 8}, { 0x11, 8}, { 0x12, 8}, { 0x13, 8}, { 0x20, 32}, { 0x21, 48}, {0x30, 192}, {0x35, 192}, {0x50, 128}, {0x52, 128}, {-1, -1} };
+
+ int index = 0;
+ while (gpuArchCoresPerSM[index].SM != -1)
+ {
+ if (gpuArchCoresPerSM[index].SM == ((major << 4) + minor) )
+ return gpuArchCoresPerSM[index].Cores;
+ index++;
+ }
+ printf("\nCan't determine number of cores. Unknown SM version %d.%d!\n", major, minor);
+ return 0;
+ }
+}
+
+void pcl::gpu::printCudaDeviceInfo(int device)
+{
+ int count = getCudaEnabledDeviceCount();
+ bool valid = (device >= 0) && (device < count);
+
+ int beg = valid ? device : 0;
+ int end = valid ? device+1 : count;
+
+ printf("*** CUDA Device Query (Runtime API) version (CUDART static linking) *** \n\n");
+ printf("Device count: %d\n", count);
+
+ int driverVersion = 0, runtimeVersion = 0;
+ cudaSafeCall( cudaDriverGetVersion(&driverVersion) );
+ cudaSafeCall( cudaRuntimeGetVersion(&runtimeVersion) );
+
+ const char *computeMode[] = {
+ "Default (multiple host threads can use ::cudaSetDevice() with device simultaneously)",
+ "Exclusive (only one host thread in one process is able to use ::cudaSetDevice() with this device)",
+ "Prohibited (no host thread can use ::cudaSetDevice() with this device)",
+ "Exclusive Process (many threads in one process is able to use ::cudaSetDevice() with this device)",
+ "Unknown",
+ NULL
+ };
+
+ for(int dev = beg; dev < end; ++dev)
+ {
+ cudaDeviceProp prop;
+ cudaSafeCall( cudaGetDeviceProperties(&prop, dev) );
+
+ int sm_cores = convertSMVer2Cores(prop.major, prop.minor);
+
+ printf("\nDevice %d: \"%s\"\n", dev, prop.name);
+ printf(" CUDA Driver Version / Runtime Version %d.%d / %d.%d\n", driverVersion/1000, driverVersion%100, runtimeVersion/1000, runtimeVersion%100);
+ printf(" CUDA Capability Major/Minor version number: %d.%d\n", prop.major, prop.minor);
+ printf(" Total amount of global memory: %.0f MBytes (%llu bytes)\n", (float)prop.totalGlobalMem/1048576.0f, (unsigned long long) prop.totalGlobalMem);
+ printf(" (%2d) Multiprocessors x (%2d) CUDA Cores/MP: %d CUDA Cores\n", prop.multiProcessorCount, sm_cores, sm_cores * prop.multiProcessorCount);
+ printf(" GPU Clock Speed: %.2f GHz\n", prop.clockRate * 1e-6f);
+
+#if (CUDART_VERSION >= 4000)
+ // This is not available in the CUDA Runtime API, so we make the necessary calls the driver API to support this for output
+ int memoryClock, memBusWidth, L2CacheSize;
+ getCudaAttribute<int>( &memoryClock, CU_DEVICE_ATTRIBUTE_MEMORY_CLOCK_RATE, dev );
+ getCudaAttribute<int>( &memBusWidth, CU_DEVICE_ATTRIBUTE_GLOBAL_MEMORY_BUS_WIDTH, dev );
+ getCudaAttribute<int>( &L2CacheSize, CU_DEVICE_ATTRIBUTE_L2_CACHE_SIZE, dev );
+
+ printf(" Memory Clock rate: %.2f Mhz\n", memoryClock * 1e-3f);
+ printf(" Memory Bus Width: %d-bit\n", memBusWidth);
+ if (L2CacheSize)
+ printf(" L2 Cache Size: %d bytes\n", L2CacheSize);
+
+ printf(" Max Texture Dimension Size (x,y,z) 1D=(%d), 2D=(%d,%d), 3D=(%d,%d,%d)\n",
+ prop.maxTexture1D, prop.maxTexture2D[0], prop.maxTexture2D[1],
+ prop.maxTexture3D[0], prop.maxTexture3D[1], prop.maxTexture3D[2]);
+ printf(" Max Layered Texture Size (dim) x layers 1D=(%d) x %d, 2D=(%d,%d) x %d\n",
+ prop.maxTexture1DLayered[0], prop.maxTexture1DLayered[1],
+ prop.maxTexture2DLayered[0], prop.maxTexture2DLayered[1], prop.maxTexture2DLayered[2]);
+#endif
+ printf(" Total amount of constant memory: %u bytes\n", (int)prop.totalConstMem);
+ printf(" Total amount of shared memory per block: %u bytes\n", (int)prop.sharedMemPerBlock);
+ printf(" Total number of registers available per block: %d\n", prop.regsPerBlock);
+ printf(" Warp size: %d\n", prop.warpSize);
+ printf(" Maximum number of threads per block: %d\n", prop.maxThreadsPerBlock);
+ printf(" Maximum sizes of each dimension of a block: %d x %d x %d\n", prop.maxThreadsDim[0], prop.maxThreadsDim[1], prop.maxThreadsDim[2]);
+ printf(" Maximum sizes of each dimension of a grid: %d x %d x %d\n", prop.maxGridSize[0], prop.maxGridSize[1], prop.maxGridSize[2]);
+ printf(" Maximum memory pitch: %u bytes\n", (int)prop.memPitch);
+ printf(" Texture alignment: %u bytes\n", (int)prop.textureAlignment);
+
+#if CUDART_VERSION >= 4000
+ printf(" Concurrent copy and execution: %s with %d copy engine(s)\n", (prop.deviceOverlap ? "Yes" : "No"), prop.asyncEngineCount);
+#else
+ printf(" Concurrent copy and execution: %s\n", prop.deviceOverlap ? "Yes" : "No");
+#endif
+ printf(" Run time limit on kernels: %s\n", prop.kernelExecTimeoutEnabled ? "Yes" : "No");
+ printf(" Integrated GPU sharing Host Memory: %s\n", prop.integrated ? "Yes" : "No");
+ printf(" Support host page-locked memory mapping: %s\n", prop.canMapHostMemory ? "Yes" : "No");
+
+ printf(" Concurrent kernel execution: %s\n", prop.concurrentKernels ? "Yes" : "No");
+ printf(" Alignment requirement for Surfaces: %s\n", prop.surfaceAlignment ? "Yes" : "No");
+ printf(" Device has ECC support enabled: %s\n", prop.ECCEnabled ? "Yes" : "No");
+ printf(" Device is using TCC driver mode: %s\n", prop.tccDriver ? "Yes" : "No");
+#if CUDART_VERSION >= 4000
+ printf(" Device supports Unified Addressing (UVA): %s\n", prop.unifiedAddressing ? "Yes" : "No");
+ printf(" Device PCI Bus ID / PCI location ID: %d / %d\n", prop.pciBusID, prop.pciDeviceID );
+#endif
+ printf(" Compute Mode:\n");
+ printf(" %s \n", computeMode[prop.computeMode]);
+ }
+
+ printf("\n");
+ printf("deviceQuery, CUDA Driver = CUDART");
+ printf(", CUDA Driver Version = %d.%d", driverVersion / 1000, driverVersion % 100);
+ printf(", CUDA Runtime Version = %d.%d", runtimeVersion/1000, runtimeVersion%100);
+ printf(", NumDevs = %d\n\n", count);
+ fflush(stdout);
+}
+
+void pcl::gpu::printShortCudaDeviceInfo(int device)
+{
+ int count = getCudaEnabledDeviceCount();
+ bool valid = (device >= 0) && (device < count);
+
+ int beg = valid ? device : 0;
+ int end = valid ? device+1 : count;
+
+ int driverVersion = 0, runtimeVersion = 0;
+ cudaSafeCall( cudaDriverGetVersion(&driverVersion) );
+ cudaSafeCall( cudaRuntimeGetVersion(&runtimeVersion) );
+
+ for(int dev = beg; dev < end; ++dev)
+ {
+ cudaDeviceProp prop;
+ cudaSafeCall( cudaGetDeviceProperties(&prop, dev) );
+
+ const char *arch_str = prop.major < 2 ? " (pre-Fermi)" : "";
+ printf("[pcl::gpu::printShortCudaDeviceInfo] : Device %d: \"%s\" %.0fMb", dev, prop.name, (float)prop.totalGlobalMem/1048576.0f);
+ printf(", sm_%d%d%s, %d cores", prop.major, prop.minor, arch_str, convertSMVer2Cores(prop.major, prop.minor) * prop.multiProcessorCount);
+ printf(", Driver/Runtime ver.%d.%d/%d.%d\n", driverVersion/1000, driverVersion%100, runtimeVersion/1000, runtimeVersion%100);
+ }
+ fflush(stdout);
+}
+
+#endif
--- /dev/null
+#include <pcl/gpu/octree/octree.hpp>
+#include <pcl/gpu/containers/device_array.hpp>
+
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <boost/shared_ptr.hpp>
+#include <pcl/visualization/cloud_viewer.h>
+#include <iostream>
+
+int main (int argc, char** argv)
+{
+ pcl::PointCloud<pcl::PointXYZ> cloud;
+ cloud.width = 500;
+ cloud.height = 200;
+ cloud.is_dense = false;
+
+ for (size_t w = 0; w < cloud.width; ++w)
+ {
+ for (size_t h = 0; h < cloud.height; ++h)
+ {
+ pcl::PointXYZ p;
+ p.x = w; p.y = h; p.z = 1;
+ cloud.points.push_back(p);
+ }
+ }
+
+ pcl::io::savePCDFileASCII ("input.pcd", cloud);
+ std::cout << "INFO: Saved " << cloud.points.size () << " data points to test_pcd.pcd." << std::endl;
+
+ pcl::gpu::Octree::PointCloud cloud_device;
+ cloud_device.upload(cloud.points);
+
+ pcl::gpu::Octree octree_device;
+ octree_device.setCloud(cloud_device);
+ octree_device.build();
+
+ // Create two query points
+ std::vector<pcl::PointXYZ> query_host;
+ query_host.resize (3);
+ query_host[0].x = 250;
+ query_host[0].y = 100;
+ query_host[0].z = 1;
+ query_host[1].x = 0;
+ query_host[1].y = 0;
+ query_host[1].z = 1;
+ query_host[2].x = 500;
+ query_host[2].y = 200;
+
+ pcl::gpu::Octree::Queries queries_device;
+ queries_device.upload(query_host);
+
+ // Take two identical radiuses
+ std::vector<float> radius;
+ radius.push_back(10.0);
+ radius.push_back(10.0);
+ radius.push_back(10.0);
+
+ pcl::gpu::Octree::Radiuses radiuses_device;
+ radiuses_device.upload(radius);
+
+ const int max_answers = 500*200;
+
+ // Output buffer on the device
+ pcl::gpu::NeighborIndices result_device(queries_device.size(), max_answers);
+
+ // Do the actual search
+ octree_device.radiusSearch(queries_device, radiuses_device, max_answers, result_device);
+
+ std::vector<int> sizes, data;
+ result_device.sizes.download(sizes);
+ result_device.data.download(data);
+
+ std::cout << "INFO: Data generated" << std::endl;
+
+ std::cout<< "INFO: found : " << data.size() << " data.size" << std::endl;
+ std::cout<< "INFO: found : " << sizes.size() << " sizes.size" << std::endl;
+
+ for (size_t i = 0; i < sizes.size (); ++i)
+ {
+ std::cout << "INFO: sizes : " << i << " size " << sizes[i] << std::endl;
+ if(sizes[i] != 0)
+ {
+ pcl::PointCloud<pcl::PointXYZ> cloud_result;
+ // Fill in the cloud data
+ cloud_result.height = 1;
+ cloud_result.is_dense = false;
+
+ for (size_t j = 0; j < sizes[i] ; ++j)
+ {
+ cloud_result.points.push_back(cloud.points[data[j + i * max_answers]]);
+ std::cout << "INFO: data : " << j << " " << j + i * max_answers << " data " << data[j+ i * max_answers] << std::endl;
+ }
+ std::stringstream ss;
+ ss << "cloud_cluster_" << i << ".pcd";
+ cloud_result.width = cloud_result.points.size();
+ pcl::io::savePCDFileASCII (ss.str(), cloud_result);
+ std::cout << "INFO: Saved " << cloud_result.points.size () << " data points to " << ss.str() << std::endl;
+ }
+ }
+ return 0;
+}
--- /dev/null
+#include <pcl/ModelCoefficients.h>
+#include <pcl/point_types.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/kdtree/kdtree.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/segmentation/extract_clusters.h>
+
+// The GPU specific stuff here
+#include <pcl/gpu/octree/octree.hpp>
+#include <pcl/gpu/containers/device_array.hpp>
+#include <pcl/gpu/segmentation/gpu_extract_clusters.h>
+#include <pcl/gpu/segmentation/impl/gpu_extract_clusters.hpp>
+
+#include <time.h>
+
+int
+main (int argc, char** argv)
+{
+ // Read in the cloud data
+ pcl::PCDReader reader;
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PCDWriter writer;
+ reader.read (argv[1], *cloud_filtered);
+
+/////////////////////////////////////////////
+/// CPU VERSION
+/////////////////////////////////////////////
+
+ std::cout << "INFO: PointCloud_filtered still has " << cloud_filtered->points.size() << " Points " << std::endl;
+ clock_t tStart = clock();
+ // Creating the KdTree object for the search method of the extraction
+ pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
+ tree->setInputCloud (cloud_filtered);
+
+ std::vector<pcl::PointIndices> cluster_indices;
+ pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
+ ec.setClusterTolerance (0.02); // 2cm
+ ec.setMinClusterSize (100);
+ ec.setMaxClusterSize (25000);
+ ec.setSearchMethod (tree);
+ ec.setInputCloud( cloud_filtered);
+ ec.extract (cluster_indices);
+
+ printf("CPU Time taken: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
+
+ int j = 0;
+ for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
+ {
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
+ for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
+ cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
+ cloud_cluster->width = cloud_cluster->points.size ();
+ cloud_cluster->height = 1;
+ cloud_cluster->is_dense = true;
+
+ std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
+ std::stringstream ss;
+ ss << "cloud_cluster_" << j << ".pcd";
+ writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
+ j++;
+ }
+
+/////////////////////////////////////////////
+/// GPU VERSION
+/////////////////////////////////////////////
+
+ std::cout << "INFO: starting with the GPU version" << std::endl;
+
+ tStart = clock();
+
+ pcl::gpu::Octree::PointCloud cloud_device;
+ cloud_device.upload(cloud_filtered->points);
+
+ pcl::gpu::Octree::Ptr octree_device (new pcl::gpu::Octree);
+ octree_device->setCloud(cloud_device);
+ octree_device->build();
+
+ std::vector<pcl::PointIndices> cluster_indices_gpu;
+ pcl::gpu::EuclideanClusterExtraction gec;
+ gec.setClusterTolerance (0.02); // 2cm
+ gec.setMinClusterSize (100);
+ gec.setMaxClusterSize (25000);
+ gec.setSearchMethod (octree_device);
+ gec.setHostCloud( cloud_filtered);
+ gec.extract (cluster_indices_gpu);
+// octree_device.clear();
+
+ printf("GPU Time taken: %.2fs\n", (double)(clock() - tStart)/CLOCKS_PER_SEC);
+ std::cout << "INFO: stopped with the GPU version" << std::endl;
+
+ j = 0;
+ for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices_gpu.begin (); it != cluster_indices_gpu.end (); ++it)
+ {
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster_gpu (new pcl::PointCloud<pcl::PointXYZ>);
+ for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
+ cloud_cluster_gpu->points.push_back (cloud_filtered->points[*pit]); //*
+ cloud_cluster_gpu->width = cloud_cluster_gpu->points.size ();
+ cloud_cluster_gpu->height = 1;
+ cloud_cluster_gpu->is_dense = true;
+
+ std::cout << "PointCloud representing the Cluster: " << cloud_cluster_gpu->points.size () << " data points." << std::endl;
+ std::stringstream ss;
+ ss << "gpu_cloud_cluster_" << j << ".pcd";
+ writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster_gpu, false); //*
+ j++;
+ }
+
+ return (0);
+}
+
--- /dev/null
+set(SUBSYS_NAME gpu_features)
+set(SUBSYS_PATH gpu/features)
+set(SUBSYS_DESC "Temporary GPU_common module. Weak CUDA dependency: a code that use this module requires CUDA Toolkit installed, but should be compiled without nvcc")
+set(SUBSYS_DEPS common gpu_containers gpu_utils gpu_octree geometry)
+
+set(build TRUE)
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS})
+PCL_SET_SUBSYS_INCLUDE_DIR("${SUBSYS_NAME}" "${SUBSYS_PATH}")
+mark_as_advanced("BUILD_${SUBSYS_NAME}")
+
+if (build)
+ FILE(GLOB incs include/pcl/gpu/features/*.hpp)
+ FILE(GLOB srcs src/*.cpp src/*.hpp)
+ FILE(GLOB cuda src/*.cu)
+ FILE(GLOB dev_incs include/pcl/gpu/features/device/*.h*)
+ source_group("Header Files\\device" FILES ${dev_incs})
+ source_group("Source Files\\cuda" FILES ${cuda} )
+ source_group("Source Files" FILES ${srcs} )
+
+ FILE(GLOB srcs_utils src/utils/*.hpp)
+ source_group("Source Files\\utils" FILES ${srcs_utils})
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include" "${CMAKE_CURRENT_SOURCE_DIR}/src")
+ PCL_CUDA_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${cuda} ${srcs_utils} ${dev_incs})
+ target_link_libraries("${LIB_NAME}" pcl_gpu_utils pcl_gpu_containers pcl_gpu_octree)
+
+ set(EXT_DEPS "")
+ #set(EXT_DEPS CUDA)
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "")
+
+ # Install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}" ${incs} ${dev_incs})
+
+ add_subdirectory(test)
+endif()
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// Eigen is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 3 of the License, or (at your option) any later version.
+//
+// Alternatively, you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of
+// the License, or (at your option) any later version.
+//
+// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+// The computeRoots function included in this is based on materials
+// covered by the following copyright and license:
+//
+// Geometric Tools, LLC
+// Copyright (c) 1998-2010
+// Distributed under the Boost Software License, Version 1.0.
+//
+// Permission is hereby granted, free of charge, to any person or organization
+// obtaining a copy of the software and accompanying documentation covered by
+// this license (the "Software") to use, reproduce, display, distribute,
+// execute, and transmit the Software, and to prepare derivative works of the
+// Software, and to permit third-parties to whom the Software is furnished to
+// do so, all subject to the following:
+//
+// The copyright notices in the Software and this entire statement, including
+// the above license grant, this restriction and the following disclaimer,
+// must be included in all copies of the Software, in whole or in part, and
+// all derivative works of the Software, unless such copies or derivative
+// works are solely in the form of machine-executable object code generated by
+// a source language processor.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+// DEALINGS IN THE SOFTWARE.
+
+#ifndef PCL_GPU_FEATURES_EIGEN_HPP_
+#define PCL_GPU_FEATURES_EIGEN_HPP_
+
+#include <pcl/gpu/utils/device/limits.hpp>
+#include <pcl/gpu/utils/device/algorithm.hpp>
+#include <pcl/gpu/utils/device/vector_math.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+ __device__ __forceinline__ void computeRoots2(const float& b, const float& c, float3& roots)
+ {
+ roots.x = 0.f;
+ float d = b * b - 4.f * c;
+ if (d < 0.f) // no real roots!!!! THIS SHOULD NOT HAPPEN!
+ d = 0.f;
+
+ float sd = sqrtf(d);
+
+ roots.z = 0.5f * (b + sd);
+ roots.y = 0.5f * (b - sd);
+ }
+
+ __device__ __forceinline__ void computeRoots3(float c0, float c1, float c2, float3& roots)
+ {
+ if ( fabsf(c0) < numeric_limits<float>::epsilon())// one root is 0 -> quadratic equation
+ {
+ computeRoots2 (c2, c1, roots);
+ }
+ else
+ {
+ const float s_inv3 = 1.f/3.f;
+ const float s_sqrt3 = sqrtf(3.f);
+ // Construct the parameters used in classifying the roots of the equation
+ // and in solving the equation for the roots in closed form.
+ float c2_over_3 = c2 * s_inv3;
+ float a_over_3 = (c1 - c2*c2_over_3)*s_inv3;
+ if (a_over_3 > 0.f)
+ a_over_3 = 0.f;
+
+ float half_b = 0.5f * (c0 + c2_over_3 * (2.f * c2_over_3 * c2_over_3 - c1));
+
+ float q = half_b * half_b + a_over_3 * a_over_3 * a_over_3;
+ if (q > 0.f)
+ q = 0.f;
+
+ // Compute the eigenvalues by solving for the roots of the polynomial.
+ float rho = sqrtf(-a_over_3);
+ float theta = atan2f (sqrtf (-q), half_b)*s_inv3;
+ float cos_theta = __cosf (theta);
+ float sin_theta = __sinf (theta);
+ roots.x = c2_over_3 + 2.f * rho * cos_theta;
+ roots.y = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
+ roots.z = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
+
+ // Sort in increasing order.
+ if (roots.x >= roots.y)
+ swap(roots.x, roots.y);
+
+ if (roots.y >= roots.z)
+ {
+ swap(roots.y, roots.z);
+
+ if (roots.x >= roots.y)
+ swap (roots.x, roots.y);
+ }
+ if (roots.x <= 0) // eigenval for symetric positive semi-definite matrix can not be negative! Set it to 0
+ computeRoots2 (c2, c1, roots);
+ }
+ }
+
+ struct Eigen33
+ {
+ public:
+ template<int Rows>
+ struct MiniMat
+ {
+ float3 data[Rows];
+ __device__ __host__ __forceinline__ float3& operator[](int i) { return data[i]; }
+ __device__ __host__ __forceinline__ const float3& operator[](int i) const { return data[i]; }
+ };
+ typedef MiniMat<3> Mat33;
+ typedef MiniMat<4> Mat43;
+
+
+ static __forceinline__ __device__ float3 unitOrthogonal (const float3& src)
+ {
+ float3 perp;
+ /* Let us compute the crossed product of *this with a vector
+ * that is not too close to being colinear to *this.
+ */
+
+ /* unless the x and y coords are both close to zero, we can
+ * simply take ( -y, x, 0 ) and normalize it.
+ */
+ if(!isMuchSmallerThan(src.x, src.z) || !isMuchSmallerThan(src.y, src.z))
+ {
+ float invnm = rsqrtf(src.x*src.x + src.y*src.y);
+ perp.x = -src.y * invnm;
+ perp.y = src.x * invnm;
+ perp.z = 0.0f;
+ }
+ /* if both x and y are close to zero, then the vector is close
+ * to the z-axis, so it's far from colinear to the x-axis for instance.
+ * So we take the crossed product with (1,0,0) and normalize it.
+ */
+ else
+ {
+ float invnm = rsqrtf(src.z * src.z + src.y * src.y);
+ perp.x = 0.0f;
+ perp.y = -src.z * invnm;
+ perp.z = src.y * invnm;
+ }
+
+ return perp;
+ }
+
+ __device__ __forceinline__ Eigen33(volatile float* mat_pkg_arg) : mat_pkg(mat_pkg_arg) {}
+ __device__ __forceinline__ void compute(Mat33& tmp, Mat33& vec_tmp, Mat33& evecs, float3& evals)
+ {
+ // Scale the matrix so its entries are in [-1,1]. The scaling is applied
+ // only when at least one matrix entry has magnitude larger than 1.
+
+ float max01 = fmaxf( fabsf(mat_pkg[0]), fabsf(mat_pkg[1]) );
+ float max23 = fmaxf( fabsf(mat_pkg[2]), fabsf(mat_pkg[3]) );
+ float max45 = fmaxf( fabsf(mat_pkg[4]), fabsf(mat_pkg[5]) );
+ float m0123 = fmaxf( max01, max23);
+ float scale = fmaxf( max45, m0123);
+
+ if (scale <= numeric_limits<float>::min())
+ scale = 1.f;
+
+ mat_pkg[0] /= scale;
+ mat_pkg[1] /= scale;
+ mat_pkg[2] /= scale;
+ mat_pkg[3] /= scale;
+ mat_pkg[4] /= scale;
+ mat_pkg[5] /= scale;
+
+ // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The
+ // eigenvalues are the roots to this equation, all guaranteed to be
+ // real-valued, because the matrix is symmetric.
+ float c0 = m00() * m11() * m22()
+ + 2.f * m01() * m02() * m12()
+ - m00() * m12() * m12()
+ - m11() * m02() * m02()
+ - m22() * m01() * m01();
+ float c1 = m00() * m11() -
+ m01() * m01() +
+ m00() * m22() -
+ m02() * m02() +
+ m11() * m22() -
+ m12() * m12();
+ float c2 = m00() + m11() + m22();
+
+ computeRoots3(c0, c1, c2, evals);
+
+ if(evals.z - evals.x <= numeric_limits<float>::epsilon())
+ {
+ evecs[0] = make_float3(1.f, 0.f, 0.f);
+ evecs[1] = make_float3(0.f, 1.f, 0.f);
+ evecs[2] = make_float3(0.f, 0.f, 1.f);
+ }
+ else if (evals.y - evals.x <= numeric_limits<float>::epsilon() )
+ {
+ // first and second equal
+ tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
+ tmp[0].x -= evals.z; tmp[1].y -= evals.z; tmp[2].z -= evals.z;
+
+ vec_tmp[0] = cross(tmp[0], tmp[1]);
+ vec_tmp[1] = cross(tmp[0], tmp[2]);
+ vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+ float len1 = dot (vec_tmp[0], vec_tmp[0]);
+ float len2 = dot (vec_tmp[1], vec_tmp[1]);
+ float len3 = dot (vec_tmp[2], vec_tmp[2]);
+
+ if (len1 >= len2 && len1 >= len3)
+ {
+ evecs[2] = vec_tmp[0] * rsqrtf (len1);
+ }
+ else if (len2 >= len1 && len2 >= len3)
+ {
+ evecs[2] = vec_tmp[1] * rsqrtf (len2);
+ }
+ else
+ {
+ evecs[2] = vec_tmp[2] * rsqrtf (len3);
+ }
+
+ evecs[1] = unitOrthogonal(evecs[2]);
+ evecs[0] = cross(evecs[1], evecs[2]);
+ }
+ else if (evals.z - evals.y <= numeric_limits<float>::epsilon() )
+ {
+ // second and third equal
+ tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
+ tmp[0].x -= evals.x; tmp[1].y -= evals.x; tmp[2].z -= evals.x;
+
+ vec_tmp[0] = cross(tmp[0], tmp[1]);
+ vec_tmp[1] = cross(tmp[0], tmp[2]);
+ vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+ float len1 = dot(vec_tmp[0], vec_tmp[0]);
+ float len2 = dot(vec_tmp[1], vec_tmp[1]);
+ float len3 = dot(vec_tmp[2], vec_tmp[2]);
+
+ if (len1 >= len2 && len1 >= len3)
+ {
+ evecs[0] = vec_tmp[0] * rsqrtf(len1);
+ }
+ else if (len2 >= len1 && len2 >= len3)
+ {
+ evecs[0] = vec_tmp[1] * rsqrtf(len2);
+ }
+ else
+ {
+ evecs[0] = vec_tmp[2] * rsqrtf(len3);
+ }
+
+ evecs[1] = unitOrthogonal( evecs[0] );
+ evecs[2] = cross(evecs[0], evecs[1]);
+ }
+ else
+ {
+
+ tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
+ tmp[0].x -= evals.z; tmp[1].y -= evals.z; tmp[2].z -= evals.z;
+
+ vec_tmp[0] = cross(tmp[0], tmp[1]);
+ vec_tmp[1] = cross(tmp[0], tmp[2]);
+ vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+ float len1 = dot(vec_tmp[0], vec_tmp[0]);
+ float len2 = dot(vec_tmp[1], vec_tmp[1]);
+ float len3 = dot(vec_tmp[2], vec_tmp[2]);
+
+ float mmax[3];
+
+ unsigned int min_el = 2;
+ unsigned int max_el = 2;
+ if (len1 >= len2 && len1 >= len3)
+ {
+ mmax[2] = len1;
+ evecs[2] = vec_tmp[0] * rsqrtf (len1);
+ }
+ else if (len2 >= len1 && len2 >= len3)
+ {
+ mmax[2] = len2;
+ evecs[2] = vec_tmp[1] * rsqrtf (len2);
+ }
+ else
+ {
+ mmax[2] = len3;
+ evecs[2] = vec_tmp[2] * rsqrtf (len3);
+ }
+
+ tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
+ tmp[0].x -= evals.y; tmp[1].y -= evals.y; tmp[2].z -= evals.y;
+
+ vec_tmp[0] = cross(tmp[0], tmp[1]);
+ vec_tmp[1] = cross(tmp[0], tmp[2]);
+ vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+ len1 = dot(vec_tmp[0], vec_tmp[0]);
+ len2 = dot(vec_tmp[1], vec_tmp[1]);
+ len3 = dot(vec_tmp[2], vec_tmp[2]);
+
+ if (len1 >= len2 && len1 >= len3)
+ {
+ mmax[1] = len1;
+ evecs[1] = vec_tmp[0] * rsqrtf (len1);
+ min_el = len1 <= mmax[min_el] ? 1 : min_el;
+ max_el = len1 > mmax[max_el] ? 1 : max_el;
+ }
+ else if (len2 >= len1 && len2 >= len3)
+ {
+ mmax[1] = len2;
+ evecs[1] = vec_tmp[1] * rsqrtf (len2);
+ min_el = len2 <= mmax[min_el] ? 1 : min_el;
+ max_el = len2 > mmax[max_el] ? 1 : max_el;
+ }
+ else
+ {
+ mmax[1] = len3;
+ evecs[1] = vec_tmp[2] * rsqrtf (len3);
+ min_el = len3 <= mmax[min_el] ? 1 : min_el;
+ max_el = len3 > mmax[max_el] ? 1 : max_el;
+ }
+
+ tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
+ tmp[0].x -= evals.x; tmp[1].y -= evals.x; tmp[2].z -= evals.x;
+
+ vec_tmp[0] = cross(tmp[0], tmp[1]);
+ vec_tmp[1] = cross(tmp[0], tmp[2]);
+ vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+ len1 = dot (vec_tmp[0], vec_tmp[0]);
+ len2 = dot (vec_tmp[1], vec_tmp[1]);
+ len3 = dot (vec_tmp[2], vec_tmp[2]);
+
+
+ if (len1 >= len2 && len1 >= len3)
+ {
+ mmax[0] = len1;
+ evecs[0] = vec_tmp[0] * rsqrtf (len1);
+ min_el = len3 <= mmax[min_el] ? 0 : min_el;
+ max_el = len3 > mmax[max_el] ? 0 : max_el;
+ }
+ else if (len2 >= len1 && len2 >= len3)
+ {
+ mmax[0] = len2;
+ evecs[0] = vec_tmp[1] * rsqrtf (len2);
+ min_el = len3 <= mmax[min_el] ? 0 : min_el;
+ max_el = len3 > mmax[max_el] ? 0 : max_el;
+ }
+ else
+ {
+ mmax[0] = len3;
+ evecs[0] = vec_tmp[2] * rsqrtf (len3);
+ min_el = len3 <= mmax[min_el] ? 0 : min_el;
+ max_el = len3 > mmax[max_el] ? 0 : max_el;
+ }
+
+ unsigned mid_el = 3 - min_el - max_el;
+ evecs[min_el] = normalized( cross( evecs[(min_el+1) % 3], evecs[(min_el+2) % 3] ) );
+ evecs[mid_el] = normalized( cross( evecs[(mid_el+1) % 3], evecs[(mid_el+2) % 3] ) );
+ }
+ // Rescale back to the original size.
+ evals *= scale;
+ }
+ private:
+ volatile float* mat_pkg;
+
+ __device__ __forceinline__ float m00() const { return mat_pkg[0]; }
+ __device__ __forceinline__ float m01() const { return mat_pkg[1]; }
+ __device__ __forceinline__ float m02() const { return mat_pkg[2]; }
+ __device__ __forceinline__ float m10() const { return mat_pkg[1]; }
+ __device__ __forceinline__ float m11() const { return mat_pkg[3]; }
+ __device__ __forceinline__ float m12() const { return mat_pkg[4]; }
+ __device__ __forceinline__ float m20() const { return mat_pkg[2]; }
+ __device__ __forceinline__ float m21() const { return mat_pkg[4]; }
+ __device__ __forceinline__ float m22() const { return mat_pkg[5]; }
+
+ __device__ __forceinline__ float3 row0() const { return make_float3( m00(), m01(), m02() ); }
+ __device__ __forceinline__ float3 row1() const { return make_float3( m10(), m11(), m12() ); }
+ __device__ __forceinline__ float3 row2() const { return make_float3( m20(), m21(), m22() ); }
+
+ __device__ __forceinline__ static bool isMuchSmallerThan (float x, float y)
+ {
+ // copied from <eigen>/include/Eigen/src/Core/NumTraits.h
+ const float prec_sqr = numeric_limits<float>::epsilon() * numeric_limits<float>::epsilon();
+ return x * x <= prec_sqr * y * y;
+ }
+
+ };
+ }
+}
+
+#endif /* PCL_GPU_FEATURES_EIGEN_HPP_ */
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_GPU_FEATURES_DEVICE_PAIR_FEATURES_HPP_
+#define PCL_GPU_FEATURES_DEVICE_PAIR_FEATURES_HPP_
+
+#include <pcl/gpu/utils/device/vector_math.hpp>
+#include <pcl/gpu/features/device/rodrigues.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+ __device__ __host__ __forceinline__
+ bool computePairFeatures (const float3& p1, const float3& n1, const float3& p2, const float3& n2, float &f1, float &f2, float &f3, float &f4)
+ {
+ f1 = f2 = f3 = f4 = 0.0f;
+
+ float3 dp2p1 = p2 - p1;
+ f4 = norm(dp2p1);
+
+ if (f4 == 0.f)
+ return false;
+
+ float3 n1_copy = n1, n2_copy = n2;
+ float angle1 = dot(n1_copy, dp2p1) / f4;
+
+
+ float angle2 = dot(n2_copy, dp2p1) / f4;
+ if (acos (fabs (angle1)) > acos (fabs (angle2)))
+ {
+ // switch p1 and p2
+ n1_copy = n2;
+ n2_copy = n1;
+ dp2p1 *= (-1);
+ f3 = -angle2;
+ }
+ else
+ f3 = angle1;
+
+ // Create a Darboux frame coordinate system u-v-w
+ // u = n1; v = (p_idx - q_idx) x u / || (p_idx - q_idx) x u ||; w = u x v
+ float3 v = cross(dp2p1, n1_copy);
+ float v_norm = norm(v);
+ if (v_norm == 0.0f)
+ return false;
+
+ // Normalize v
+ v *= 1.f/v_norm;
+
+ // Do not have to normalize w - it is a unit vector by construction
+ f2 = dot(v, n2_copy);
+
+ float3 w = cross(n1_copy, v);
+ // Compute f1 = arctan (w * n2, u * n2) i.e. angle of n2 in the x=u, y=w coordinate system
+ f1 = atan2f (dot(w, n2_copy), dot(n1_copy, n2_copy)); // @todo optimize this
+
+ return true;
+ }
+
+ __device__ __host__ __forceinline__
+ bool computeRGBPairFeatures (const float3& p1, const float3& n1, const int& colors1, const float3& p2, const float3& n2, const int& colors2,
+ float &f1, float &f2, float &f3, float &f4, float &f5, float &f6, float &f7)
+ {
+ float3 dp2p1 = p2 - p1;
+ f4 = norm(dp2p1);
+
+ if (f4 == 0.0f)
+ {
+ f1 = f2 = f3 = f4 = f5 = f6 = f7 = 0.0f;
+ return false;
+ }
+
+ float3 n1_copy = n1, n2_copy = n2;
+ float angle1 = dot(n1_copy, dp2p1) / f4;
+
+ f3 = angle1;
+
+ // Create a Darboux frame coordinate system u-v-w
+ // u = n1; v = (p_idx - q_idx) x u / || (p_idx - q_idx) x u ||; w = u x v
+ float3 v = cross(dp2p1, n1_copy);
+ float v_norm = norm(v);
+ if (v_norm == 0.0f)
+ {
+ f1 = f2 = f3 = f4 = f5 = f6 = f7 = 0.0f;
+ return false;
+ }
+ // Normalize v
+ v *= 1.f/v_norm;
+
+ float3 w = cross(n1_copy, v);
+ // Do not have to normalize w - it is a unit vector by construction
+
+ f2 = dot(v, n2_copy);
+
+ // Compute f1 = arctan (w * n2, u * n2) i.e. angle of n2 in the x=u, y=w coordinate system
+ f1 = atan2f (dot(w, n2_copy), dot (n1_copy, n2_copy));
+
+ // everything before was standard 4D-Darboux frame feature pair
+ // now, for the experimental color stuff
+
+ f5 = ((float) ((colors1 ) & 0xFF)) / ((colors2 ) & 0xFF);
+ f6 = ((float) ((colors1 >> 8) & 0xFF)) / ((colors2 >> 8) & 0xFF);
+ f7 = ((float) ((colors1 >> 16) & 0xFF)) / ((colors2 >> 16) & 0xFF);
+
+ // make sure the ratios are in the [-1, 1] interval
+ if (f5 > 1.f) f5 = - 1.f / f5;
+ if (f6 > 1.f) f6 = - 1.f / f6;
+ if (f7 > 1.f) f7 = - 1.f / f7;
+
+ return true;
+ }
+
+ __device__ __host__ __forceinline__
+ void computeRGBPairFeatures_RGBOnly (const int& colors1, const int& colors2, float &f5, float &f6, float &f7)
+ {
+ f5 = ((float) ((colors1 ) & 0xFF)) / ((colors2 ) & 0xFF);
+ f6 = ((float) ((colors1 >> 8) & 0xFF)) / ((colors2 >> 8) & 0xFF);
+ f7 = ((float) ((colors1 >> 16) & 0xFF)) / ((colors2 >> 16) & 0xFF);
+
+ // make sure the ratios are in the [-1, 1] interval
+ if (f5 > 1.f) f5 = - 1.f / f5;
+ if (f6 > 1.f) f6 = - 1.f / f6;
+ if (f7 > 1.f) f7 = - 1.f / f7;
+ }
+
+ __device__ __host__ __forceinline__ bool computePPFPairFeature(const float3& p1, const float3& n1, const float3& p2, const float3& n2,
+ float& f1, float& f2, float& f3, float& f4)
+ {
+ float3 delta = p2 - p1;
+
+ f4 = norm (delta);
+
+ delta.x /= f4;
+ delta.y /= f4;
+ delta.z /= f4;
+
+ f1 = dot(n1, delta);
+ f2 = dot(n2, delta);
+ f3 = dot(n1, n2);
+
+ return true;
+ }
+
+
+ __device__ __host__ __forceinline__ void computeAlfaM(const float3& model_reference_point, const float3& model_reference_normal,
+ const float3& model_point, float& alpha_m)
+ {
+ float acos_value = acos (model_reference_normal.x);
+
+ //float3 cross_vector = cross(model_reference_normal, Eigen::Vector3f::UnitX);
+ float3 cross_vector = make_float3(0, model_reference_normal.z, - model_reference_normal.y);
+ float3 cross_vector_norm = normalized(cross_vector);
+
+ //Eigen::AngleAxisf rotation_mg (acos_value, cross_vector_norm);
+ //Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg;
+
+ float3 row1, row2, row3; // == rotation_mg
+ AngleAxisf(acos_value, cross_vector_norm, row1, row2, row3);
+
+ float3 traslation;
+ //traslation.x = row1.x * -model_reference_point.x + row1.y * -model_reference_point.y + row1.z * -model_reference_point.z;
+ traslation.y = row2.x * -model_reference_point.x + row2.y * -model_reference_point.y + row2.z * -model_reference_point.z;
+ traslation.z = row3.x * -model_reference_point.x + row3.y * -model_reference_point.y + row3.z * -model_reference_point.z;
+
+ float3 model_point_transformed;// = transform_mg * model_point;
+ //model_point_transformed.x = traslation.x + row1.x * model_point.x + row1.y * model_point.y + row1.z * model_point.z;
+ model_point_transformed.y = traslation.y + row2.x * model_point.x + row2.y * model_point.y + row2.z * model_point.z;
+ model_point_transformed.z = traslation.z + row3.x * model_point.x + row3.y * model_point.y + row3.z * model_point.z;
+
+
+ float angle = atan2f ( -model_point_transformed.z, model_point_transformed.y);
+
+ if (sinf(angle) * model_point_transformed.z < 0.0f)
+ //if (angle * model_point_transformed.z < 0.ff)
+ angle *= -1;
+ alpha_m = -angle;
+ }
+ }
+}
+
+#endif /* PCL_GPU_FEATURES_DEVICE_PAIR_FEATURES_HPP_ */
\ No newline at end of file
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+
+#ifndef PCL_GPU_DEVICE_RODRIGUES_HPP_
+#define PCL_GPU_DEVICE_RODRIGUES_HPP_
+
+#include <pcl/gpu/utils/device/vector_math.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+ __device__ __host__ __forceinline__ void AngleAxisf(float angle, const float3& r, float3& row1, float3& row2, float3& row3)
+ {
+ float cosA, sinA;
+ sincosf(angle, &sinA, &cosA);
+
+ row1.x = cosA; row1.y = 0.f; row1.z = 0.f;
+ row2.x = 0.f; row2.y = cosA; row2.z = 0.f;
+ row3.x = 0.f; row3.y = 0.f; row3.z = cosA;
+
+ /* */ row1.y += -r.z * sinA; row1.z += r.y * sinA;
+ row2.x += r.z * sinA; /* */ row2.z += -r.x * sinA;
+ row3.x += -r.y * sinA; row3.y += r.x * sinA; /* */
+
+ row1.x += r.x * r.x * (1 - cosA); row1.y += r.x * r.y * (1 - cosA); row1.z += r.x * r.z * (1 - cosA);
+ row2.x += r.y * r.x * (1 - cosA); row2.y += r.y * r.y * (1 - cosA); row2.z += r.y * r.z * (1 - cosA);
+ row3.x += r.z * r.x * (1 - cosA); row3.y += r.z * r.y * (1 - cosA); row3.z += r.z * r.z * (1 - cosA);
+ }
+
+ __device__ __host__ __forceinline__ void Rodrigues(const float3& rvec, float3& row1, float3& row2, float3& row3)
+ {
+ float angle = norm(rvec);
+ float3 unit_axis = make_float3(rvec.x/angle, rvec.y/angle, rvec.z/angle);
+ AngleAxisf(angle, unit_axis, row1, row2, row3);
+ }
+ }
+}
+
+#endif /* PCL_GPU_DEVICE_RODRIGUES_HPP_ */
\ No newline at end of file
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef _PCL_GPU_FEATURES_HPP_
+#define _PCL_GPU_FEATURES_HPP_
+
+#include <pcl/pcl_macros.h>
+#include <pcl/point_types.h>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/octree/device_format.hpp>
+#include <pcl/gpu/octree/octree.hpp>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ ////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b Feature represents the base feature class. */
+
+ struct PCL_EXPORTS Feature
+ {
+ public:
+ typedef PointXYZ PointType;
+ typedef PointXYZ NormalType;
+
+ typedef DeviceArray< PointType> PointCloud;
+ typedef DeviceArray<NormalType> Normals;
+ typedef DeviceArray<int> Indices;
+
+ Feature();
+
+ void setInputCloud(const PointCloud& cloud);
+ void setSearchSurface(const PointCloud& surface);
+ void setIndices(const Indices& indices);
+ void setRadiusSearch(float radius, int max_results);
+ protected:
+ PointCloud cloud_;
+ PointCloud surface_;
+ Indices indices_;
+ float radius_;
+ int max_results_;
+
+ Octree octree_;
+ };
+
+ ////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b Feature represents the base feature class that takes normals as input also. */
+
+ struct PCL_EXPORTS FeatureFromNormals : public Feature
+ {
+ public:
+
+ void setInputNormals(const Normals& normals);
+ protected:
+ Normals normals_;
+ };
+
+ ////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b Class for normal estimation. */
+ class PCL_EXPORTS NormalEstimation : public Feature
+ {
+ public:
+ // float x, y, z, curvature; -> sizeof(PointXYZ) = 4 * sizeof(float)
+ typedef Feature::NormalType NormalType;
+
+ NormalEstimation();
+ void compute(Normals& normals);
+ void setViewPoint(float vpx, float vpy, float vpz);
+ void getViewPoint(float& vpx, float& vpy, float& vpz);
+
+ static void computeNormals(const PointCloud& cloud, const NeighborIndices& nn_indices, Normals& normals);
+ static void flipNormalTowardsViewpoint(const PointCloud& cloud, float vp_x, float vp_y, float vp_z, Normals& normals);
+ static void flipNormalTowardsViewpoint(const PointCloud& cloud, const Indices& indices, float vp_x, float vp_y, float vp_z, Normals& normals);
+ private:
+ float vpx_, vpy_, vpz_;
+ NeighborIndices nn_indices_;
+ };
+
+ ////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b Class for PFH estimation. */
+ class PCL_EXPORTS PFHEstimation : public FeatureFromNormals
+ {
+ public:
+ void compute(const PointCloud& cloud, const Normals& normals, const NeighborIndices& neighb_indices, DeviceArray2D<PFHSignature125>& features);
+ void compute(DeviceArray2D<PFHSignature125>& features);
+ private:
+ NeighborIndices nn_indices_;
+ DeviceArray2D<float> data_rpk;
+ int max_elems_rpk;
+ };
+
+ ////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b Class for PFHRGB estimation. */
+ class PCL_EXPORTS PFHRGBEstimation : public FeatureFromNormals
+ {
+ public:
+ typedef PointXYZ PointType; //16 bytes for xyzrgb
+ void compute(const PointCloud& cloud, const Normals& normals, const NeighborIndices& neighb_indices, DeviceArray2D<PFHRGBSignature250>& features);
+ void compute(DeviceArray2D<PFHRGBSignature250>& features);
+ private:
+ NeighborIndices nn_indices_;
+ DeviceArray2D<float> data_rpk;
+ int max_elems_rpk;
+ };
+
+ ////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief @b Class for FPFH estimation. */
+ class PCL_EXPORTS FPFHEstimation : public FeatureFromNormals
+ {
+ public:
+ FPFHEstimation();
+ virtual ~FPFHEstimation();
+
+ void compute(DeviceArray2D<FPFHSignature33>& features);
+
+ void compute(const PointCloud& cloud, const Normals& normals, const NeighborIndices& neighbours, DeviceArray2D<FPFHSignature33>& features);
+
+ private:
+ NeighborIndices nn_indices_, nn_indices2_;
+
+ DeviceArray<int> unique_indices_storage;
+ DeviceArray<int> lookup;
+
+ DeviceArray2D<FPFHSignature33> spfh;
+ };
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ ///** \brief @b Class for PPF estimation. */
+ class PCL_EXPORTS PPFEstimation : public FeatureFromNormals
+ {
+ public:
+ void compute(DeviceArray<PPFSignature>& features);
+ };
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ ///** \brief @b Class for PPFRGB estimation. */
+
+ class PCL_EXPORTS PPFRGBEstimation : public FeatureFromNormals
+ {
+ public:
+
+ typedef PointXYZ PointType; //16 bytes for xyzrgb
+ void compute(DeviceArray<PPFRGBSignature>& features);
+ };
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ ///** \brief @b Class for PPFRGBRegion estimation. */
+
+ class PCL_EXPORTS PPFRGBRegionEstimation : public FeatureFromNormals
+ {
+ public:
+ typedef PointXYZ PointType; //16 bytes for xyzrgb
+ void compute(DeviceArray<PPFRGBSignature>& features);
+
+ private:
+ NeighborIndices nn_indices_;
+ };
+
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ ///** \brief @b Class for PPFRGBRegion estimation. */
+
+ class PCL_EXPORTS PrincipalCurvaturesEstimation : public FeatureFromNormals
+ {
+ public:
+
+ void compute(DeviceArray<PrincipalCurvatures>& features);
+ private:
+ NeighborIndices nn_indices_;
+ DeviceArray2D<float> proj_normals_buf;
+ };
+
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ ///** \brief @b Class for Viewpoint Feature Histogramm estimation. */
+
+ class PCL_EXPORTS VFHEstimation : public FeatureFromNormals
+ {
+ public:
+
+ enum
+ {
+ BINS1_F1 = 45,
+ BINT2_F2 = 45,
+ BINS3_F3 = 45,
+ BINS4_F4 = 45,
+ BINS_VP = 128
+ };
+
+ VFHEstimation();
+
+ void setViewPoint(float vpx, float vpy, float vpz);
+ void getViewPoint(float& vpx, float& vpy, float& vpz);
+
+ void setUseGivenNormal (bool use);
+ void setNormalToUse (const NormalType& normal);
+ void setUseGivenCentroid (bool use);
+ void setCentroidToUse (const PointType& centroid);
+
+ void setNormalizeBins (bool normalize);
+ void setNormalizeDistance (bool normalize);
+ void setFillSizeComponent (bool fill_size);
+
+ void compute(DeviceArray<VFHSignature308>& feature);
+ private:
+
+ float vpx_, vpy_, vpz_;
+
+ bool use_given_normal_;
+ bool use_given_centroid_;
+ bool normalize_bins_;
+ bool normalize_distances_;
+ bool size_component_;
+
+ NormalType normal_to_use_;
+ PointType centroid_to_use_;
+ };
+
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+ ///** \brief @b Class for SpinImages estimation. */
+
+ class PCL_EXPORTS SpinImageEstimation : public FeatureFromNormals
+ {
+ public:
+ typedef Histogram<153> SpinImage;
+
+ SpinImageEstimation (unsigned int image_width = 8,
+ double support_angle_cos = 0.0, // when 0, this is bogus, so not applied
+ unsigned int min_pts_neighb = 0);
+
+ void setImageWidth (unsigned int bin_count);
+ void setSupportAngle (float support_angle_cos);
+ void setMinPointCountInNeighbourhood (unsigned int min_pts_neighb);
+ void setInputWithNormals (const PointCloud& input, const Normals& normals);
+ void setSearchSurfaceWithNormals (const PointCloud& surface, const Normals& normals);
+
+ void setRotationAxis (const NormalType& axis);
+ void setInputRotationAxes (const Normals& axes);
+ void useNormalsAsRotationAxis();
+ void setAngularDomain (bool is_angular = true);
+ void setRadialStructure (bool is_radial = true);
+
+ void compute(DeviceArray2D<SpinImage>& features, DeviceArray<unsigned char>& mask);
+
+ private:
+ Normals input_normals_;
+ Normals rotation_axes_cloud_;
+
+ bool is_angular_;
+
+ NormalType rotation_axis_;
+ bool use_custom_axis_;
+
+ /* use input normals as rotation axes*/
+ bool use_custom_axes_cloud_;
+
+ bool is_radial_;
+
+ unsigned int image_width_;
+ float support_angle_cos_;
+ unsigned int min_pts_neighb_;
+
+ bool fake_surface_;
+
+ NeighborIndices nn_indices_;
+ };
+ }
+};
+
+#endif /* _PCL_GPU_FEATURES_HPP_ */
+
+
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#include "internal.hpp"
+
+#include <thrust/device_ptr.h>
+#include <thrust/transform_reduce.h>
+#include <thrust/iterator/permutation_iterator.h>
+#include <thrust/iterator/counting_iterator.h>
+#include <thrust/iterator/zip_iterator.h>
+
+#include "pcl/gpu/utils/device/vector_math.hpp"
+
+using namespace thrust;
+
+namespace pcl
+{
+ namespace device
+ {
+ template<class PointT>
+ struct PointT2float3
+ {
+ __device__ __forceinline__ float3 operator()(const PointT& p) const { return make_float3(p.x, p.y, p.z); }
+ };
+
+ struct PlusFloat3
+ {
+ __device__ __forceinline__ float3 operator()(const float3& e1, const float3& e2) const
+ {
+ return make_float3(e1.x + e2.x, e1.y + e2.y, e1.z + e2.z);
+ }
+ };
+
+ struct TupleDistCvt
+ {
+ float3 pivot_;
+ TupleDistCvt(const float3& pivot) : pivot_(pivot) {}
+ __device__ __forceinline__ thrust::tuple<float, int> operator()(const thrust::tuple<float4, int>& t) const
+ {
+ float4 point = t.get<0>();
+
+ float dx = pivot_.x - point.x;
+ float dy = pivot_.y - point.y;
+ float dz = pivot_.z - point.z;
+ float dist = sqrt(dx*dx + dy*dy + dz*dz);
+
+ return thrust::tuple<float, int>(dist, t.get<1>());
+ }
+ };
+
+ }
+}
+
+template<typename PointT>
+void pcl::device::compute3DCentroid(const DeviceArray<PointT>& cloud, float3& centroid)
+{
+ thrust::device_ptr<PointT> src_beg((PointT*)cloud.ptr());
+ thrust::device_ptr<PointT> src_end = src_beg + cloud.size();
+
+ centroid = transform_reduce(src_beg, src_beg, PointT2float3<PointT>(), make_float3(0.f, 0.f, 0.f), PlusFloat3());
+ centroid *= 1.f/cloud.size();
+}
+
+template<typename PointT>
+void pcl::device::compute3DCentroid(const DeviceArray<PointT>& cloud, const Indices& indices, float3& centroid)
+{
+ if (indices.empty())
+ compute3DCentroid(cloud, centroid);
+ else
+ {
+ thrust::device_ptr<PointT> src_beg((PointT*)cloud.ptr());
+ thrust::device_ptr<int> map_beg((int*)indices.ptr());
+ thrust::device_ptr<int> map_end = map_beg + indices.size();
+
+
+ centroid = transform_reduce(make_permutation_iterator(src_beg, map_beg),
+ make_permutation_iterator(src_beg, map_end),
+ PointT2float3<PointT>(), make_float3(0.f, 0.f, 0.f), PlusFloat3());
+
+ centroid *= 1.f/indices.size();
+ }
+}
+
+template<typename PointT>
+float3 pcl::device::getMaxDistance(const DeviceArray<PointT>& cloud, const float3& pivot)
+{
+ thrust::device_ptr<PointT> src_beg((PointT*)cloud.ptr());
+ thrust::device_ptr<PointT> src_end = src_beg + cloud.size();
+
+ thrust::counting_iterator<int> cf(0) ;
+ thrust::counting_iterator<int> ce = cf + cloud.size();
+
+ thrust::tuple<float, int> init(0.f, 0);
+ thrust::maximum< tuple<float, int> > op;
+
+ tuple<float, int> res = transform_reduce(
+ make_zip_iterator(make_tuple( src_beg, cf )),
+ make_zip_iterator(make_tuple( src_beg, ce )),
+ TupleDistCvt(pivot), init, op);
+
+ float4 point = src_beg[res.get<1>()];
+
+ return make_float3(point.x, point.y, point.z);
+}
+
+
+template<typename PointT>
+float3 pcl::device::getMaxDistance(const DeviceArray<PointT>& cloud, const Indices& indices, const float3& pivot)
+{
+ if (indices.empty())
+ return getMaxDistance(cloud, pivot);
+
+ thrust::device_ptr<PointT> src_beg((PointT*)cloud.ptr());
+ thrust::device_ptr<int> map_beg((int*)indices.ptr());
+ thrust::device_ptr<int> map_end = map_beg + indices.size();
+
+ thrust::counting_iterator<int> cf(0) ;
+ thrust::counting_iterator<int> ce = cf + indices.size();
+
+ thrust::tuple<float, int> init(0.f, 0);
+ thrust::maximum< tuple<float, int> > op;
+
+ tuple<float, int> res = transform_reduce(
+ make_zip_iterator(make_tuple( make_permutation_iterator(src_beg, map_beg), cf )),
+ make_zip_iterator(make_tuple( make_permutation_iterator(src_beg, map_end), ce )),
+ TupleDistCvt(pivot), init, op);
+
+ float4 point = src_beg[map_beg[res.get<1>()]];
+
+ return make_float3(point.x, point.y, point.z);
+}
+
+template void pcl::device::compute3DCentroid<float4>(const DeviceArray<float4>& cloud, float3& centroid);
+template void pcl::device::compute3DCentroid<float4>(const DeviceArray<float4>& cloud, const Indices& indices, float3& centroid);
+
+template float3 pcl::device::getMaxDistance<float4>(const DeviceArray<float4>& cloud, const float3& pivot);
+template float3 pcl::device::getMaxDistance<float4>(const DeviceArray<float4>& cloud, const Indices& indices, const float3& pivot);
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include <pcl/gpu/containers/initialization.h>
+#include <pcl/gpu/features/features.hpp>
+#include <pcl/gpu/utils/device/static_check.hpp>
+#include "internal.hpp"
+
+#include <pcl/exceptions.h>
+#include <pcl/console/print.h>
+
+using namespace pcl::device;
+
+/////////////////////////////////////////////////////////////////////////
+/// Feature
+
+pcl::gpu::Feature::Feature() { radius_ = 0.f, max_results_ = 0; }
+void pcl::gpu::Feature::setInputCloud(const PointCloud& cloud) { cloud_ = cloud; }
+void pcl::gpu::Feature::setSearchSurface(const PointCloud& surface) { surface_ = surface; }
+void pcl::gpu::Feature::setIndices(const Indices& indices) { indices_ = indices; }
+void pcl::gpu::Feature::setRadiusSearch(float radius, int max_results) { radius_ = radius; max_results_ = max_results; }
+
+/////////////////////////////////////////////////////////////////////////
+/// FeatureFromNormals
+void pcl::gpu::FeatureFromNormals::setInputNormals(const Normals& normals) { normals_ = normals; }
+
+
+/////////////////////////////////////////////////////////////////////////
+/// NormalEstimation
+pcl::gpu::NormalEstimation::NormalEstimation() : vpx_(0), vpy_(0), vpz_(0) {}
+
+void pcl::gpu::NormalEstimation::computeNormals(const PointCloud& cloud, const NeighborIndices& nn_indices, Normals& normals)
+{
+ normals.create(nn_indices.neighboors_size());
+
+ const device::PointCloud& c = (const device::PointCloud&)cloud;
+ device::Normals& n = (device::Normals&)normals;
+
+ device::computeNormals(c, nn_indices, n);
+}
+
+void pcl::gpu::NormalEstimation::flipNormalTowardsViewpoint(const PointCloud& cloud, float vp_x, float vp_y, float vp_z, Normals& normals)
+{
+ const device::PointCloud& c = (const device::PointCloud&)cloud;
+ device::Normals& n = (device::Normals&)normals;
+
+ device::flipNormalTowardsViewpoint(c, make_float3(vp_x, vp_y, vp_z), n);
+}
+
+void pcl::gpu::NormalEstimation::flipNormalTowardsViewpoint(const PointCloud& cloud, const Indices& indices, float vp_x, float vp_y, float vp_z, Normals& normals)
+{
+ const device::PointCloud& c = (const device::PointCloud&)cloud;
+ device::Normals& n = (device::Normals&)normals;
+
+ device::flipNormalTowardsViewpoint(c, indices, make_float3(vp_x, vp_y, vp_z), n);
+}
+
+
+void pcl::gpu::NormalEstimation::setViewPoint (float vpx, float vpy, float vpz)
+{
+ vpx_ = vpx; vpy_ = vpy; vpz_ = vpz;
+}
+
+void pcl::gpu::NormalEstimation::getViewPoint (float &vpx, float &vpy, float &vpz)
+{
+ vpx = vpx_; vpy = vpy_; vpz = vpz_;
+}
+
+void pcl::gpu::NormalEstimation::compute(Normals& normals)
+{
+ assert(!cloud_.empty());
+
+ PointCloud& surface = surface_.empty() ? cloud_ : surface_;
+
+ octree_.setCloud(surface);
+ octree_.build();
+
+ if (indices_.empty() || (!indices_.empty() && indices_.size() == cloud_.size()))
+ {
+ octree_.radiusSearch(cloud_, radius_, max_results_, nn_indices_);
+ computeNormals(surface, nn_indices_, normals);
+ flipNormalTowardsViewpoint(cloud_, vpx_, vpy_, vpz_, normals);
+ }
+ else
+ {
+ octree_.radiusSearch(cloud_, indices_, radius_, max_results_, nn_indices_);
+ computeNormals(surface, nn_indices_, normals);
+ flipNormalTowardsViewpoint(cloud_, indices_, vpx_, vpy_, vpz_, normals);
+ }
+}
+
+
+/////////////////////////////////////////////////////////////////////////
+/// PFHEstimation
+
+void pcl::gpu::PFHEstimation::compute(const PointCloud& cloud, const Normals& normals, const NeighborIndices& neighbours, DeviceArray2D<PFHSignature125>& features)
+{
+ assert( cloud.size() == normals.size() );
+ assert( neighbours.validate(cloud.size()) );
+
+ const device::PointCloud& c = (const device::PointCloud&)cloud;
+ const device::Normals& n = (const device::Normals&)normals;
+
+ features.create (static_cast<int> (neighbours.sizes.size ()), 1);
+
+ DeviceArray2D<device::PFHSignature125>& f = (DeviceArray2D<device::PFHSignature125>&)features;
+
+ repackToAosForPfh(c, n, neighbours, data_rpk, max_elems_rpk);
+ computePfh125(data_rpk, max_elems_rpk, neighbours, f);
+}
+
+void pcl::gpu::PFHEstimation::compute(DeviceArray2D<PFHSignature125>& features)
+{
+ PointCloud& surface = surface_.empty() ? cloud_ : surface_;
+
+ octree_.setCloud(surface);
+ octree_.build();
+
+ assert( cloud_.size() == normals_.size());
+
+ if (indices_.empty() || (!indices_.empty() && indices_.size() == cloud_.size()))
+ {
+ octree_.radiusSearch(cloud_, radius_, max_results_, nn_indices_);
+ compute(surface, normals_, nn_indices_, features);
+ }
+ else
+ {
+ octree_.radiusSearch(cloud_, indices_, radius_, max_results_, nn_indices_);
+ compute(surface, normals_, nn_indices_, features);
+ }
+
+}
+
+void pcl::gpu::PFHRGBEstimation::compute(const PointCloud& cloud, const Normals& normals, const NeighborIndices& neighbours, DeviceArray2D<PFHRGBSignature250>& features)
+{
+ assert( cloud.size() == normals.size() );
+ assert( neighbours.validate(cloud.size()) );
+
+ const device::PointCloud& c = (const device::PointCloud&)cloud;
+ const device::Normals& n = (const device::Normals&)normals;
+
+ features.create (static_cast<int> (neighbours.sizes.size ()), 1);
+
+ DeviceArray2D<device::PFHRGBSignature250>& f = (DeviceArray2D<device::PFHRGBSignature250>&)features;
+
+ repackToAosForPfhRgb(c, n, neighbours, data_rpk, max_elems_rpk);
+ computePfhRgb250(data_rpk, max_elems_rpk, neighbours, f);
+}
+
+void pcl::gpu::PFHRGBEstimation::compute(DeviceArray2D<PFHRGBSignature250>& features)
+{
+ PointCloud& surface = surface_.empty() ? cloud_ : surface_;
+
+ octree_.setCloud(surface);
+ octree_.build();
+
+ assert( cloud_.size() == normals_.size());
+
+ if (indices_.empty() || (!indices_.empty() && indices_.size() == cloud_.size()))
+ {
+ octree_.radiusSearch(cloud_, radius_, max_results_, nn_indices_);
+ compute(surface, normals_, nn_indices_, features);
+ }
+ else
+ {
+ octree_.radiusSearch(cloud_, indices_, radius_, max_results_, nn_indices_);
+ compute(surface, normals_, nn_indices_, features);
+ }
+}
+
+/////////////////////////////////////////////////////////////////////////
+/// FPFHEstimation
+
+pcl::gpu::FPFHEstimation::FPFHEstimation()
+{
+ Static<sizeof(FPFHEstimation:: PointType) == sizeof(device:: PointType)>::check();
+ Static<sizeof(FPFHEstimation::NormalType) == sizeof(device::NormalType)>::check();
+}
+
+pcl::gpu::FPFHEstimation::~FPFHEstimation() {}
+
+
+void pcl::gpu::FPFHEstimation::compute(const PointCloud& cloud, const Normals& normals, const NeighborIndices& neighbours, DeviceArray2D<FPFHSignature33>& features)
+{
+ assert( cloud.size() == normals.size() );
+ assert( neighbours.validate(cloud.size()) );
+
+ const device::PointCloud& c = (const device::PointCloud&)cloud;
+ const device::Normals& n = (const device::Normals&)normals;
+
+ features.create (static_cast<int> (cloud.size ()), 1);
+ spfh.create (static_cast<int> (cloud.size ()), 1);
+
+ DeviceArray2D<device::FPFHSignature33>& s = (DeviceArray2D<device::FPFHSignature33>&)spfh;
+ DeviceArray2D<device::FPFHSignature33>& f = (DeviceArray2D<device::FPFHSignature33>&)features;
+
+ device::computeSPFH(c, n, device::Indices(), neighbours, s);
+ device::computeFPFH(c, neighbours, s, f);
+}
+
+void pcl::gpu::FPFHEstimation::compute(DeviceArray2D<FPFHSignature33>& features)
+{
+ bool hasInds = !indices_.empty() && indices_.size() != cloud_.size();
+ bool hasSurf = !surface_.empty();
+
+ features.create (static_cast<int> (hasInds ? indices_.size () : cloud_.size ()), 1);
+
+ if (!hasInds && !hasSurf)
+ {
+ features.create (static_cast<int> (cloud_.size ()), 1);
+ octree_.setCloud(cloud_);
+ octree_.build();
+ assert( cloud_.size() == normals_.size());
+ octree_.radiusSearch(cloud_, radius_, max_results_, nn_indices_);
+ compute(cloud_, normals_, nn_indices_, features);
+ return;
+ }
+
+ PointCloud& surface = surface_.empty() ? cloud_ : surface_;
+
+ octree_.setCloud(surface);
+ octree_.build();
+
+ if (hasInds)
+ octree_.radiusSearch(cloud_, indices_, radius_, max_results_, nn_indices_);
+ else
+ octree_.radiusSearch(cloud_, radius_, max_results_, nn_indices_);
+
+ int total = computeUniqueIndices(surface.size(), nn_indices_, unique_indices_storage, lookup);
+
+ DeviceArray<int> unique_indices(unique_indices_storage.ptr(), total);
+ octree_.radiusSearch(surface, unique_indices, radius_, max_results_, nn_indices2_);
+
+ DeviceArray2D<device::FPFHSignature33>& spfh33 = (DeviceArray2D<device::FPFHSignature33>&)spfh;
+ const device::PointCloud& c = (const device::PointCloud&)cloud_;
+ const device::PointCloud& s = (const device::PointCloud&)surface;
+ const device::Normals& n = (const device::Normals&)normals_;
+ device::computeSPFH(s, n, unique_indices, nn_indices2_, spfh33);
+
+ DeviceArray2D<device::FPFHSignature33>& f = (DeviceArray2D<device::FPFHSignature33>&)features;
+ device::computeFPFH(c, indices_, s, nn_indices_, lookup, spfh33, f);
+}
+
+
+/////////////////////////////////////////////////////////////////////////
+/// PPFEstimation
+
+void pcl::gpu::PPFEstimation::compute(DeviceArray<PPFSignature>& features)
+{
+ Static<sizeof(PPFEstimation:: PointType) == sizeof(device:: PointType)>::check();
+ Static<sizeof(PPFEstimation::NormalType) == sizeof(device::NormalType)>::check();
+
+ assert(this->surface_.empty() && !indices_.empty() && !cloud_.empty() && normals_.size() == cloud_.size());
+ features.create(indices_.size () * cloud_.size ());
+
+ const device::PointCloud& c = (const device::PointCloud&)cloud_;
+ const device::Normals& n = (const device::Normals&)normals_;
+
+ DeviceArray<device::PPFSignature>& f = (DeviceArray<device::PPFSignature>&)features;
+ device::computePPF(c, n, indices_, f);
+}
+
+/////////////////////////////////////////////////////////////////////////
+/// PPFRGBEstimation
+
+void pcl::gpu::PPFRGBEstimation::compute(DeviceArray<PPFRGBSignature>& features)
+{
+ Static<sizeof(PPFEstimation:: PointType) == sizeof(device:: PointType)>::check();
+ Static<sizeof(PPFEstimation::NormalType) == sizeof(device::NormalType)>::check();
+
+ assert(this->surface_.empty() && !indices_.empty() && !cloud_.empty() && normals_.size() == cloud_.size());
+ features.create(indices_.size () * cloud_.size ());
+
+ const device::PointCloud& c = (const device::PointCloud&)cloud_;
+ const device::Normals& n = (const device::Normals&)normals_;
+
+ DeviceArray<device::PPFRGBSignature>& f = (DeviceArray<device::PPFRGBSignature>&)features;
+ device::computePPFRGB(c, n, indices_, f);
+}
+
+/////////////////////////////////////////////////////////////////////////
+/// PPFRGBRegionEstimation
+
+void pcl::gpu::PPFRGBRegionEstimation::compute(DeviceArray<PPFRGBSignature>& features)
+{
+ Static<sizeof(PPFRGBRegionEstimation:: PointType) == sizeof(device:: PointType)>::check();
+ Static<sizeof(PPFRGBRegionEstimation::NormalType) == sizeof(device::NormalType)>::check();
+
+ assert(this->surface_.empty() && !indices_.empty() && !cloud_.empty() && normals_.size() == cloud_.size());
+
+ features.create(indices_.size());
+
+ octree_.setCloud(cloud_);
+ octree_.build();
+
+ octree_.radiusSearch(cloud_, indices_, radius_, max_results_, nn_indices_);
+
+ const device::PointCloud& c = (const device::PointCloud&)cloud_;
+ const device::Normals& n = (const device::Normals&)normals_;
+
+ DeviceArray<device::PPFRGBSignature>& f = (DeviceArray<device::PPFRGBSignature>&)features;
+
+ device::computePPFRGBRegion(c, n, indices_, nn_indices_, f);
+}
+
+/////////////////////////////////////////////////////////////////////////
+/// PrincipalCurvaturesEstimation
+
+void pcl::gpu::PrincipalCurvaturesEstimation::compute(DeviceArray<PrincipalCurvatures>& features)
+{
+ Static<sizeof(PPFRGBRegionEstimation:: PointType) == sizeof(device:: PointType)>::check();
+ Static<sizeof(PPFRGBRegionEstimation::NormalType) == sizeof(device::NormalType)>::check();
+
+ assert(/*!indices_.empty() && */!cloud_.empty() && max_results_ > 0 && radius_ > 0.f);
+ assert(surface_.empty() ? normals_.size() == cloud_.size() : normals_.size() == surface_.size());
+
+ PointCloud& surface = surface_.empty() ? cloud_ : surface_;
+
+ octree_.setCloud(surface);
+ octree_.build();
+
+ if(indices_.empty())
+ octree_.radiusSearch(cloud_, radius_, max_results_, nn_indices_);
+ else
+ octree_.radiusSearch(cloud_, indices_, radius_, max_results_, nn_indices_);
+
+ const device::Normals& n = (const device::Normals&)normals_;
+
+ features.create(normals_.size());
+
+ DeviceArray<device::PrincipalCurvatures>& f = (DeviceArray<device::PrincipalCurvatures>&)features;
+
+ device::computePointPrincipalCurvatures(n, indices_, nn_indices_, f, proj_normals_buf);
+}
+
+
+/////////////////////////////////////////////////////////////////////////
+/// VFHEstimation
+
+pcl::gpu::VFHEstimation::VFHEstimation()
+{
+ vpx_ = vpy_ = vpz_ = 0.f;
+
+ //default parameters to compute VFH
+ use_given_normal_ = false;
+ use_given_centroid_ = false;
+ normalize_bins_ = true;
+ normalize_distances_ = false;
+ size_component_ = false;
+}
+
+void pcl::gpu::VFHEstimation::setViewPoint(float vpx, float vpy, float vpz) { vpx_ = vpx; vpy_ = vpy; vpz_ = vpz; }
+void pcl::gpu::VFHEstimation::getViewPoint(float& vpx, float& vpy, float& vpz) { vpx = vpx_; vpy = vpy_; vpz = vpz_; }
+
+void pcl::gpu::VFHEstimation::setUseGivenNormal (bool use) { use_given_normal_ = use; }
+void pcl::gpu::VFHEstimation::setNormalToUse (const NormalType& normal) { normal_to_use_ = normal; }
+void pcl::gpu::VFHEstimation::setUseGivenCentroid (bool use) { use_given_centroid_ = use; }
+void pcl::gpu::VFHEstimation::setCentroidToUse (const PointType& centroid) { centroid_to_use_ = centroid; }
+
+void pcl::gpu::VFHEstimation::setNormalizeBins (bool normalize) { normalize_bins_ = normalize; }
+void pcl::gpu::VFHEstimation::setNormalizeDistance (bool normalize) { normalize_distances_ = normalize; }
+void pcl::gpu::VFHEstimation::setFillSizeComponent (bool fill_size) { size_component_ = fill_size; }
+
+////////////////////////////////////////////////////////////////////////////////////////////
+
+void pcl::gpu::VFHEstimation::compute(DeviceArray<VFHSignature308>& feature)
+{
+ assert(!surface_.empty() && normals_.size() == surface_.size() && cloud_.empty());
+
+ Static<sizeof(VFHEstimation:: PointType) == sizeof(device:: PointType)>::check();
+ Static<sizeof(VFHEstimation::NormalType) == sizeof(device::NormalType)>::check();
+
+ feature.create(1);
+
+ VFHEstimationImpl impl;
+
+ const device::PointCloud& s = (const device::PointCloud&)surface_;
+ const device::Normals& n = (const device::Normals&)normals_;
+
+ if (use_given_centroid_)
+ {
+ impl.xyz_centroid.x = centroid_to_use_.x;
+ impl.xyz_centroid.y = centroid_to_use_.y;
+ impl.xyz_centroid.z = centroid_to_use_.z;
+ }
+ else
+ {
+ compute3DCentroid(s, indices_, impl.xyz_centroid);
+
+ }
+ if (use_given_normal_)
+ {
+ impl.normal_centroid.x = normal_to_use_.x;
+ impl.normal_centroid.y = normal_to_use_.y;
+ impl.normal_centroid.z = normal_to_use_.z;
+ }
+ else
+ compute3DCentroid (n, indices_, impl.normal_centroid);
+
+ impl.viewpoint = make_float3(vpx_, vpy_, vpz_);
+
+
+ impl.indices = indices_;
+ impl.points = s;
+ impl.normals = n;
+
+ impl.normalize_distances = normalize_distances_;
+ impl.size_component = size_component_;
+ impl.normalize_bins = normalize_bins_;
+
+ DeviceArray<device::VFHSignature308>& f = (DeviceArray<device::VFHSignature308>&)feature;
+ impl.compute(f);
+}
+
+/////////////////////////////////////////////////////////////////////////
+/// SpinImageEstimation
+
+void pcl::gpu::SpinImageEstimation::setImageWidth (unsigned int bin_count) { image_width_ = bin_count; }
+void pcl::gpu::SpinImageEstimation::setSupportAngle (float support_angle_cos)
+{
+ if (0.f > support_angle_cos || support_angle_cos > 1.f) // may be permit negative cosine?
+ pcl::gpu::error("Cosine of support angle should be between 0 and 1", __FILE__, __LINE__);
+ support_angle_cos_ = support_angle_cos;
+}
+
+void pcl::gpu::SpinImageEstimation::setMinPointCountInNeighbourhood (unsigned int min_pts_neighb) { min_pts_neighb_ = min_pts_neighb; }
+void pcl::gpu::SpinImageEstimation::setInputWithNormals(const PointCloud& input, const Normals& normals)
+{
+ setInputCloud (input);
+ input_normals_ = normals;
+}
+
+void pcl::gpu::SpinImageEstimation::setSearchSurfaceWithNormals (const PointCloud& surface, const Normals& normals)
+{
+ setSearchSurface (surface);
+ setInputNormals (normals);
+}
+
+void pcl::gpu::SpinImageEstimation::setRotationAxis (const NormalType& axis)
+{
+ rotation_axis_ = axis;
+ use_custom_axis_ = true;
+ use_custom_axes_cloud_ = false;
+}
+
+void pcl::gpu::SpinImageEstimation::setInputRotationAxes (const Normals& axes)
+{
+ rotation_axes_cloud_ = axes;
+ use_custom_axes_cloud_ = true;
+ use_custom_axis_ = false;
+}
+
+void pcl::gpu::SpinImageEstimation::useNormalsAsRotationAxis () { use_custom_axis_ = false; use_custom_axes_cloud_ = false; }
+void pcl::gpu::SpinImageEstimation::setAngularDomain (bool is_angular) { is_angular_ = is_angular; }
+void pcl::gpu::SpinImageEstimation::setRadialStructure (bool is_radial) { is_radial_ = is_radial; }
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+
+pcl::gpu::SpinImageEstimation::SpinImageEstimation (unsigned int image_width, double support_angle_cos, unsigned int min_pts_neighb)
+: is_angular_(false), use_custom_axis_(false), use_custom_axes_cloud_(false), is_radial_(false),
+image_width_(image_width), support_angle_cos_(support_angle_cos), min_pts_neighb_(min_pts_neighb)
+{
+ assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+void pcl::gpu::SpinImageEstimation::compute(DeviceArray2D<SpinImage>& features, DeviceArray<unsigned char>& mask)
+{
+ assert(!indices_.empty());
+
+ if (image_width_ != 8)
+ pcl::gpu::error("Currently only image_width = 8 is supported (less is possible right now, more - need to allocate more memory)", __FILE__, __LINE__);
+
+ Static<sizeof(SpinImageEstimation:: PointType) == sizeof(device:: PointType)>::check();
+ Static<sizeof(SpinImageEstimation::NormalType) == sizeof(device::NormalType)>::check();
+
+ features.create (static_cast<int> (indices_.size ()), 1);
+ mask.create(indices_.size());
+
+ //////////////////////////////
+ if (!surface_)
+ {
+ surface_ = cloud_;
+ normals_ = input_normals_;
+ fake_surface_ = true;
+ }
+
+ assert(!(use_custom_axis_ && use_custom_axes_cloud_));
+
+ if (!use_custom_axis_ && !use_custom_axes_cloud_ && !input_normals_)
+ pcl::gpu::error("No normals for input cloud were given!", __FILE__, __LINE__);
+
+ if ((is_angular_ || support_angle_cos_ > 0.0) && !input_normals_)
+ pcl::gpu::error("No normals for input cloud were given!", __FILE__, __LINE__);
+
+ if (use_custom_axes_cloud_ && rotation_axes_cloud_.size () != cloud_.size ())
+ pcl::gpu::error("Rotation axis cloud have different size from input!", __FILE__, __LINE__);
+
+ ///////////////////////////////////////////////
+ octree_.setCloud(surface_);
+ octree_.build();
+ octree_.radiusSearch(cloud_, indices_, radius_, max_results_, nn_indices_);
+
+ // OK, we are interested in the points of the cylinder of height 2*r and base radius r, where r = m_dBinSize * in_iImageWidth
+ // it can be embedded to the sphere of radius sqrt(2) * m_dBinSize * in_iImageWidth
+ // suppose that points are uniformly distributed, so we lose ~40% // according to the volumes ratio
+ float bin_size = radius_ / image_width_;
+ if (!is_radial_)
+ bin_size /= sqrt(2.f);
+
+
+ const device::PointCloud& s = (const device::PointCloud&)surface_;
+ const device::PointCloud& c = (const device::PointCloud&)cloud_;
+ const device::Normals& in = (const device::Normals&)input_normals_;
+ const device::Normals& n = (const device::Normals&)normals_;
+
+
+ if (use_custom_axis_)
+ {
+ float3 axis = make_float3(rotation_axis_.x, rotation_axis_.y, rotation_axis_.z);
+ computeSpinImagesCustomAxes(is_radial_, is_angular_, support_angle_cos_, indices_, c, in,
+ s, n, nn_indices_, min_pts_neighb_, image_width_, bin_size, axis, features);
+ }
+ else if (use_custom_axes_cloud_)
+ {
+ const device::Normals& axes = (const device::Normals&)rotation_axes_cloud_;
+
+ computeSpinImagesCustomAxesCloud(is_radial_, is_angular_, support_angle_cos_, indices_, c, in,
+ s, n, nn_indices_, min_pts_neighb_, image_width_, bin_size, axes, features);
+ }
+ else
+ {
+ computeSpinImagesOrigigNormal(is_radial_, is_angular_, support_angle_cos_, indices_, c, in,
+ s, n, nn_indices_, min_pts_neighb_, image_width_, bin_size, features);
+ }
+
+ computeMask(nn_indices_, min_pts_neighb_, mask);
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include "internal.hpp"
+
+#include "pcl/gpu/utils/safe_call.hpp"
+#include "pcl/gpu/utils/device/warp.hpp"
+#include "pcl/gpu/utils/device/functional.hpp"
+#include "pcl/gpu/utils/device/funcattrib.hpp"
+#include "pcl/gpu/utils/timers_cuda.hpp"
+#include "pcl/gpu/features/device/pair_features.hpp"
+
+#include <iostream>
+
+using namespace pcl::gpu;
+using namespace pcl::device;
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846f
+#endif
+
+namespace pcl
+{
+ namespace device
+ {
+ struct KernelBase
+ {
+ enum
+ {
+ CTA_SIZE = 256,
+ WAPRS = CTA_SIZE/Warp::WARP_SIZE,
+
+ bins1 = 11,
+ bins2 = 11,
+ bins3 = 11,
+
+ FSize = bins1 + bins2 + bins3
+ };
+
+ const PointType* point_cloud;
+ size_t work_size;
+
+ PtrStep<int> gindices;
+ const int* sizes;
+
+ template<class It>
+ __device__ __forceinline__ float3 fetch(It ptr, int index) const
+ {
+ //return tr(ptr[index]);
+ return *(float3*)&ptr[index];
+ }
+ };
+
+ struct Spfh : public KernelBase
+ {
+ const NormalType* normals;
+ mutable PtrStep<float> output;
+ const int *indices;
+
+ __device__ __forceinline__ void operator()() const
+ {
+ __shared__ float shists[WAPRS * FSize];
+
+ __shared__ float3 current_point[WAPRS];
+ __shared__ float3 current_nomal[WAPRS];
+
+ int lane = Warp::laneId();
+ int warp_idx = Warp::id();
+ int idx = WAPRS * blockIdx.x + warp_idx;
+
+ if (idx >= work_size)
+ return;
+
+ int point_idx = indices ? indices[idx] : idx;
+
+ float* shist = shists + warp_idx * FSize;
+ float* shist_b1 = shist;
+ float* shist_b2 = shist_b1 + bins1;
+ float* shist_b3 = shist_b2 + bins2;
+
+ Warp::fill(shist, shist + FSize, 0.f);
+
+ if (lane == 0)
+ {
+ current_point[warp_idx] = fetch(point_cloud, point_idx);
+ current_nomal[warp_idx] = fetch(normals, point_idx);
+ }
+
+ const int *ginds = gindices.ptr(idx);
+ int size = sizes[idx];
+
+ float hist_incr = 100.f / (float)(size - 1); // or 100/(size - 1) ???
+
+ //now [inds, inds + size) contains indices of neighb points for idx-th point in cloud
+ //this list also contains idx itseelf.
+
+ for(int i = lane; i < size; i += Warp::STRIDE)
+ {
+ int point_index = ginds[i];
+ if (point_index != point_idx) // skip itself
+ {
+ float3 p = fetch(point_cloud, point_index);
+ float3 n = fetch(normals, point_index);
+
+ int h_index;
+ float f1, f2, f3, f4;
+ if (computePairFeatures (current_point[warp_idx], current_nomal[warp_idx], p, n, f1, f2, f3, f4))
+ {
+ // Normalize the f1, f2, f3 features and push them in the histogram
+ h_index = floorf (bins1 * ((f1 + M_PI) * (1.0f / (2.0f * M_PI))));
+ h_index = min(bins1 - 1, max(0, h_index));
+ atomicAdd(shist_b1 + h_index, hist_incr);
+
+ h_index = floorf (bins2 * ((f2 + 1.0f) * 0.5f));
+ h_index = min(bins2 - 1, max (0, h_index));
+ atomicAdd(shist_b2 + h_index, hist_incr);
+
+ h_index = floorf (bins3 * ((f3 + 1.0f) * 0.5f));
+ h_index = min(bins3 - 1, max (0, h_index));
+
+ atomicAdd(shist_b3 + h_index, hist_incr);
+ }
+ }
+ }
+ float *out = output.ptr(idx);
+ Warp::copy(shist, shist + FSize, out);
+ }
+ };
+
+ __global__ void SpfhKernel(const Spfh spfh33) { spfh33(); }
+ }
+}
+
+void pcl::device::computeSPFH(const PointCloud& surface, const Normals& normals, const Indices& indices, const NeighborIndices& neighbours, DeviceArray2D<FPFHSignature33>& spfh33)
+{
+ spfh33.create(indices.empty() ? (int)surface.size() : (int)indices.size(), 1);
+
+ std::vector<int> inds;
+ indices.download(inds);
+
+ Spfh spfh;
+ spfh.point_cloud = surface;
+ spfh.normals = normals;
+ spfh.indices = indices;
+ spfh.work_size = spfh33.rows();
+
+ spfh.sizes = neighbours.sizes;
+ spfh.gindices = neighbours;
+ spfh.output = spfh33;
+
+ int block = KernelBase::CTA_SIZE;
+ int grid = divUp((int)spfh.work_size, KernelBase::WAPRS);
+
+ SpfhKernel<<<grid, block>>>(spfh);
+
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall(cudaDeviceSynchronize());
+}
+
+namespace pcl
+{
+ namespace device
+ {
+ struct Fpfh : public KernelBase
+ {
+ const PtrStep<float> spfh;
+ const PointType* surface;
+ const int* indices;
+ const int* lookup;
+ mutable PtrStep<float> fpfh;
+
+ Fpfh(PtrStep<float> spfh_arg) : spfh(spfh_arg) {}
+
+ __device__ __forceinline__ void operator()() const
+ {
+ int lane = Warp::laneId();
+ int warp_idx = Warp::id();
+ int idx = WAPRS * blockIdx.x + warp_idx; // "index in neighbours" == "index in output" == "index in indices".
+
+ if (idx >= work_size)
+ return;
+
+ __shared__ float3 current_point[WAPRS];
+ __shared__ float features[WAPRS * FSize];
+ __shared__ int sindices[CTA_SIZE];
+
+ int point_idx = indices ? indices[idx] : idx; //index in cloud
+
+ if (lane == 0)
+ current_point[warp_idx] = fetch(point_cloud, point_idx);
+
+ volatile float *feature_beg = features + FSize * warp_idx ;
+ volatile float *feature_end = feature_beg + FSize;
+
+ Warp::fill(feature_beg, feature_end, 0.f);
+
+ const int *ginds = gindices.ptr(idx);
+ int *sinds = sindices + Warp::WARP_SIZE * warp_idx;
+ int size = sizes[idx];
+
+ for(int i = lane; __any(i < size); i += Warp::STRIDE)
+ {
+ if (i < size)
+ sinds[lane] = ginds[i];
+
+ int inds_num = __popc(__ballot(i < size));
+
+ for(int j = 0; j < inds_num; ++j)
+ {
+ int point_index = sinds[j]; // index in surface
+
+ if (surface == point_cloud)
+ {
+ if(point_index != point_idx) //surface == cloud -> point_index and idx are indeces both for the same array.
+ {
+ float3 p = fetch(point_cloud, point_index);
+ //float dist = norm(p, current_point[warp_idx]);
+
+ float dx = p.x - current_point[warp_idx].x;
+ float dy = p.y - current_point[warp_idx].y;
+ float dz = p.z - current_point[warp_idx].z;
+
+ float dist = dx * dx + dy * dy + dz * dz;
+ float weight = 1.f / dist;
+
+ const float *spfh_ptr = spfh.ptr( lookup ? lookup[point_index] : point_index );
+
+ Warp::transform(feature_beg, feature_end, spfh_ptr, feature_beg, plusWeighted<volatile float, float>(weight));
+ }
+ }
+ else
+ {
+ float3 p = fetch(surface, point_index);
+
+ float dx = p.x - current_point[warp_idx].x;
+ float dy = p.y - current_point[warp_idx].y;
+ float dz = p.z - current_point[warp_idx].z;
+
+ float dist = dx * dx + dy * dy + dz * dz;
+
+ if (dist == 0)
+ continue;
+
+ float weight = 1.f / dist;
+
+ const float *spfh_ptr = spfh.ptr( lookup[point_index] );
+
+ Warp::transform(feature_beg, feature_end, spfh_ptr, feature_beg, plusWeighted<volatile float, float>(weight));
+ }
+ }
+ }
+
+ float *buffer = (float*)&sindices[threadIdx.x - lane];
+
+ normalizeFeature<bins1>(feature_beg, buffer, lane);
+ normalizeFeature<bins2>(feature_beg + bins1, buffer, lane);
+ normalizeFeature<bins3>(feature_beg + bins1 + bins2, buffer, lane);
+
+ Warp::copy(feature_beg, feature_end, fpfh.ptr(idx));
+ }
+
+ template<int bins>
+ __device__ __forceinline__ void normalizeFeature(volatile float *feature, volatile float *buffer, int lane) const
+ {
+ //nomalize buns
+ float value = (lane < bins) ? feature[lane] : 0.f;
+ float sum = Warp::reduce(buffer, value, plus<volatile float>());
+
+ if (sum != 0)
+ sum = 100.0 / sum;
+
+ if (lane < bins)
+ feature[lane] *= sum;
+ }
+ };
+
+ __global__ void FpfhKernel(const Fpfh fpfh33) { fpfh33(); }
+ }
+}
+
+
+void pcl::device::computeFPFH(const PointCloud& cloud, const NeighborIndices& neighbours, const DeviceArray2D<FPFHSignature33>& spfh, DeviceArray2D<FPFHSignature33>& features)
+{
+ Fpfh fpfh(spfh);
+ fpfh.point_cloud = cloud;
+ fpfh.surface = cloud;
+ fpfh.work_size = neighbours.sizes.size();
+ fpfh.lookup = 0;
+ fpfh.indices = 0;
+
+ fpfh.sizes = neighbours.sizes;
+ fpfh.gindices = neighbours;
+ fpfh.fpfh = features;
+
+ int block = KernelBase::CTA_SIZE;
+ int grid = divUp((int)fpfh.work_size, KernelBase::WAPRS);
+
+ FpfhKernel<<<grid, block>>>(fpfh);
+
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall(cudaDeviceSynchronize());
+}
+
+void pcl::device::computeFPFH(const PointCloud& cloud, const Indices& indices, const PointCloud& surface,
+ const NeighborIndices& neighbours, DeviceArray<int>& lookup, const DeviceArray2D<FPFHSignature33>& spfh, DeviceArray2D<FPFHSignature33>& features)
+{
+ Fpfh fpfh(spfh);
+ fpfh.point_cloud = cloud;
+ fpfh.surface = surface;
+ fpfh.work_size = neighbours.sizes.size();
+ fpfh.indices = indices;
+ fpfh.lookup = lookup;
+
+ fpfh.sizes = neighbours.sizes;
+ fpfh.gindices = neighbours;
+ fpfh.fpfh = features;
+
+ int block = KernelBase::CTA_SIZE;
+ int grid = divUp((int)fpfh.work_size, KernelBase::WAPRS);
+
+ FpfhKernel<<<grid, block>>>(fpfh);
+
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall(cudaDeviceSynchronize());
+
+}
+
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#ifndef PCL_GPU_FEATURES_INTERNAL_HPP_
+#define PCL_GPU_FEATURES_INTERNAL_HPP_
+
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/octree/device_format.hpp>
+
+#include <cuda_runtime.h>
+
+#undef PI
+#ifndef PI
+ #define PI 3.1415926535897931f
+#endif
+
+namespace pcl
+{
+ namespace device
+ {
+ using pcl::gpu::DeviceArray;
+ using pcl::gpu::DeviceArray2D;
+ using pcl::gpu::NeighborIndices;
+
+ typedef float4 PointType;
+ typedef float4 NormalType;
+ typedef float4 PointXYZRGB;
+
+ typedef DeviceArray< PointType> PointCloud;
+ typedef DeviceArray<NormalType> Normals;
+ typedef DeviceArray<int> Indices;
+
+ typedef DeviceArray< PointType> PointXYZRGBCloud;
+
+ template <int N> struct Histogram
+ {
+ float histogram[N];
+ };
+
+ typedef Histogram<125> PFHSignature125;
+ typedef Histogram<250> PFHRGBSignature250;
+ typedef Histogram<33> FPFHSignature33;
+ typedef Histogram<308> VFHSignature308;
+
+ struct PPFSignature
+ {
+ float f1, f2, f3, f4;
+ float alpha_m;
+ };
+
+ struct PPFRGBSignature
+ {
+ float f1, f2, f3, f4;
+ float r_ratio, g_ratio, b_ratio;
+ float alpha_m;
+ };
+
+ struct PrincipalCurvatures
+ {
+ union
+ {
+ float principal_curvature[3];
+ struct
+ {
+ float principal_curvature_x;
+ float principal_curvature_y;
+ float principal_curvature_z;
+ };
+ };
+ float pc1;
+ float pc2;
+ };
+
+ // normals estimation
+ void computeNormals(const PointCloud& cloud, const NeighborIndices& nn_indices, Normals& normals);
+ void flipNormalTowardsViewpoint(const PointCloud& cloud, const float3& vp, Normals& normals);
+ void flipNormalTowardsViewpoint(const PointCloud& cloud, const Indices& indices, const float3& vp, Normals& normals);
+
+ // pfh estimation
+ void repackToAosForPfh(const PointCloud& cloud, const Normals& normals, const NeighborIndices& neighbours, DeviceArray2D<float>& data_rpk, int& max_elems_rpk);
+ void computePfh125(const DeviceArray2D<float>& data_rpk, int max_elems_rpk, const NeighborIndices& neighbours, DeviceArray2D<PFHSignature125>& features);
+
+ void repackToAosForPfhRgb(const PointCloud& cloud, const Normals& normals, const NeighborIndices& neighbours, DeviceArray2D<float>& data_rpk, int& max_elems_rpk);
+ void computePfhRgb250(const DeviceArray2D<float>& data_rpk, int max_elems_rpk, const NeighborIndices& neighbours, DeviceArray2D<PFHRGBSignature250>& features);
+
+
+ // fpfh estimation
+ void computeSPFH(const PointCloud& surface, const Normals& normals, const Indices& indices, const NeighborIndices& neighbours, DeviceArray2D<FPFHSignature33>& spfh33);
+ void computeFPFH(const PointCloud& cloud, const NeighborIndices& neighbours, const DeviceArray2D<FPFHSignature33>& spfh, DeviceArray2D<FPFHSignature33>& features);
+
+ void computeFPFH(const PointCloud& cloud, const Indices& indices, const PointCloud& surface,
+ const NeighborIndices& neighbours, DeviceArray<int>& lookup, const DeviceArray2D<FPFHSignature33>& spfh, DeviceArray2D<FPFHSignature33>& features);
+
+ int computeUniqueIndices(size_t surface_size, const NeighborIndices& neighbours, DeviceArray<int>& unique_indices, DeviceArray<int>& lookup);
+
+ // ppf estimation
+ void computePPF(const PointCloud& input, const Normals& normals, const Indices& indices, DeviceArray<PPFSignature>& output);
+ void computePPFRGB(const PointXYZRGBCloud& input, const Normals& normals, const Indices& indices, DeviceArray<PPFRGBSignature>& output);
+ void computePPFRGBRegion(const PointXYZRGBCloud& cloud, const Normals& normals, const Indices& indices,
+ const NeighborIndices& nn_indices, DeviceArray<PPFRGBSignature>& output);
+
+ //PrincipalCurvatures estimation
+ void computePointPrincipalCurvatures(const Normals& normals, const Indices& indices, const NeighborIndices& neighbours,
+ DeviceArray<PrincipalCurvatures>& output, DeviceArray2D<float>& proj_normals_buf);
+
+
+ //vfh estimation
+ template<typename PointT> void compute3DCentroid(const DeviceArray<PointT>& cloud, float3& centroid);
+ template<typename PointT> void compute3DCentroid(const DeviceArray<PointT>& cloud, const Indices& indices, float3& centroid);
+
+ template<typename PointT> float3 getMaxDistance(const DeviceArray<PointT>& cloud, const float3& pivot);
+ template<typename PointT> float3 getMaxDistance(const DeviceArray<PointT>& cloud, const Indices& indices, const float3& pivot);
+
+ struct VFHEstimationImpl
+ {
+ float3 xyz_centroid;
+ float3 normal_centroid;
+ float3 viewpoint;
+
+ Indices indices;
+ PointCloud points;
+ Normals normals;
+
+ bool normalize_distances;
+ bool size_component;
+ bool normalize_bins;
+
+ void compute(DeviceArray<VFHSignature308>& feature);
+ };
+
+ //spinimages estimation
+ void computeSpinImagesOrigigNormal(bool radial, bool angular, float support_angle_cos, const Indices& indices, const PointCloud& input_cloud, const Normals& input_normals,
+ const PointCloud& surface, const Normals& normals, const NeighborIndices& neighbours, int min_neighb, int image_width, float bin_size, PtrStep<float> output);
+
+ void computeSpinImagesCustomAxes(bool radial, bool angular, float support_angle_cos, const Indices& indices, const PointCloud& input_cloud, const Normals& input_normals,
+ const PointCloud& surface, const Normals& normals, const NeighborIndices& neighbours, int min_neighb, int image_width, float bin_size, const float3& rotation_axis, PtrStep<float> output);
+
+ void computeSpinImagesCustomAxesCloud(bool radial, bool angular, float support_angle_cos, const Indices& indices, const PointCloud& input_cloud, const Normals& input_normals,
+ const PointCloud& surface, const Normals& normals, const NeighborIndices& neighbours, int min_neighb, int image_width, float bin_size, const Normals& rotation_axes_cloud, PtrStep<float> output);
+
+ void computeMask(const NeighborIndices& neighbours, int min_neighb, DeviceArray<unsigned char>& mask);
+ }
+}
+
+#endif /* PCL_GPU_FEATURES_INTERNAL_HPP_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include "internal.hpp"
+
+#include "pcl/gpu/utils/safe_call.hpp"
+#include "pcl/gpu/utils/device/warp.hpp"
+#include "pcl/gpu/utils/device/funcattrib.hpp"
+
+#include "pcl/gpu/features/device/eigen.hpp"
+
+using namespace pcl::gpu;
+
+namespace pcl
+{
+ namespace device
+ {
+ struct NormalsEstimator
+ {
+ enum
+ {
+ CTA_SIZE = 256,
+ WAPRS = CTA_SIZE / Warp::WARP_SIZE,
+
+ MIN_NEIGHBOORS = 1
+ };
+
+ struct plus
+ {
+ __forceinline__ __device__ float operator()(const float &lhs, const volatile float& rhs) const { return lhs + rhs; }
+ };
+
+ PtrStep<int> indices;
+ const int *sizes;
+ const PointType *points;
+
+ PtrSz<NormalType> normals;
+
+ __device__ __forceinline__ void operator()() const
+ {
+ __shared__ float cov_buffer[6][CTA_SIZE + 1];
+
+ int warp_idx = Warp::id();
+ int idx = blockIdx.x * WAPRS + warp_idx;
+
+ if (idx >= normals.size)
+ return;
+
+ int size = sizes[idx];
+ int lane = Warp::laneId();
+
+ if (size < MIN_NEIGHBOORS)
+ {
+ const float NaN = numeric_limits<float>::quiet_NaN();
+ if (lane == 0)
+ normals.data[idx] = make_float4(NaN, NaN, NaN, NaN);
+ }
+
+ const int *ibeg = indices.ptr(idx);
+ const int *iend = ibeg + size;
+
+ //copmpute centroid
+ float3 c = make_float3(0.f, 0.f, 0.f);
+ for(const int *t = ibeg + lane; t < iend; t += Warp::STRIDE)
+ c += fetch(*t);
+
+ volatile float *buffer = &cov_buffer[0][threadIdx.x - lane];
+
+ c.x = Warp::reduce(buffer, c.x, plus());
+ c.y = Warp::reduce(buffer, c.y, plus());
+ c.z = Warp::reduce(buffer, c.z, plus());
+ c *= 1.f/size;
+
+ //nvcc bug workaround. if comment this => c.z == 0 at line: float3 d = fetch(*t) - c;
+ __threadfence_block();
+
+ //compute covariance matrix
+ int tid = threadIdx.x;
+
+ for(int i = 0; i < 6; ++i)
+ cov_buffer[i][tid] = 0.f;
+
+ for(const int *t = ibeg + lane; t < iend; t += Warp::STRIDE)
+ {
+ //float3 d = fetch(*t) - c;
+
+ float3 p = fetch(*t);
+ float3 d = p - c;
+
+ cov_buffer[0][tid] += d.x * d.x; //cov (0, 0)
+ cov_buffer[1][tid] += d.x * d.y; //cov (0, 1)
+ cov_buffer[2][tid] += d.x * d.z; //cov (0, 2)
+ cov_buffer[3][tid] += d.y * d.y; //cov (1, 1)
+ cov_buffer[4][tid] += d.y * d.z; //cov (1, 2)
+ cov_buffer[5][tid] += d.z * d.z; //cov (2, 2)
+ }
+
+ Warp::reduce(&cov_buffer[0][tid - lane], plus());
+ Warp::reduce(&cov_buffer[1][tid - lane], plus());
+ Warp::reduce(&cov_buffer[2][tid - lane], plus());
+ Warp::reduce(&cov_buffer[3][tid - lane], plus());
+ Warp::reduce(&cov_buffer[4][tid - lane], plus());
+ Warp::reduce(&cov_buffer[5][tid - lane], plus());
+
+ volatile float *cov = &cov_buffer[0][tid-lane];
+ if (lane < 6)
+ cov[lane] = cov_buffer[lane][tid-lane];
+
+ //solvePlaneParameters
+ if (lane == 0)
+ {
+ // Extract the eigenvalues and eigenvectors
+ typedef Eigen33::Mat33 Mat33;
+ Eigen33 eigen33(&cov[lane]);
+
+ Mat33& tmp = (Mat33&)cov_buffer[1][tid - lane];
+ Mat33& vec_tmp = (Mat33&)cov_buffer[2][tid - lane];
+ Mat33& evecs = (Mat33&)cov_buffer[3][tid - lane];
+ float3 evals;
+
+ eigen33.compute(tmp, vec_tmp, evecs, evals);
+ //evecs[0] - eigenvector with the lowerst eigenvalue
+
+ // Compute the curvature surface change
+ float eig_sum = evals.x + evals.y + evals.z;
+ float curvature = (eig_sum == 0) ? 0 : fabsf( evals.x / eig_sum );
+
+ NormalType output;
+ output.w = curvature;
+
+ // The normalization is not necessary, since the eigenvectors from Eigen33 are already normalized
+ output.x = evecs[0].x;
+ output.y = evecs[0].y;
+ output.z = evecs[0].z;
+
+ normals.data[idx] = output;
+ }
+ }
+
+ __device__ __forceinline__ float3 fetch(int idx) const
+ {
+ /*PointType p = points[idx];
+ return make_float3(p.x, p.y, p.z);*/
+ return *(float3*)&points[idx];
+ }
+
+ };
+
+ __global__ void EstimateNormaslKernel(const NormalsEstimator est) { est(); }
+
+
+ struct FlipNormal
+ {
+ const PointType* cloud;
+ float3 vp;
+ mutable PtrSz<NormalType> normals;
+
+ __device__ __forceinline__ void operator()(int idx, const float3& p) const
+ {
+ NormalType n = normals[idx];
+
+ float vp_x = vp.x - p.x;
+ float vp_y = vp.y - p.y;
+ float vp_z = vp.z - p.z;
+
+ // Dot product between the (viewpoint - point) and the plane normal
+ float cos_theta = vp_x * n.x + vp_y * n.y + vp_z * n.z;
+
+ // Flip the plane normal
+ if (cos_theta < 0)
+ {
+ n.x *= -1;
+ n.y *= -1;
+ n.z *= -1;
+ normals[idx] = n;
+ }
+ }
+ };
+
+ __global__ void flipNormalTowardsViewpointKernel(const FlipNormal flip)
+ {
+ int idx = threadIdx.x + blockIdx.x * blockDim.x;
+
+ if (idx < flip.normals.size)
+ {
+ float3 p = *(float3*)&flip.cloud[idx];
+ flip(idx, p);
+ }
+ }
+ __global__ void flipNormalTowardsViewpointKernel(const FlipNormal flip, const int* indices)
+ {
+ int idx = threadIdx.x + blockIdx.x * blockDim.x;
+
+ if (idx < flip.normals.size)
+ {
+ float3 p = *(float3*)&flip.cloud[indices[idx]];
+ flip(idx, p);
+ }
+ }
+ }
+}
+
+void pcl::device::computeNormals(const PointCloud& cloud, const NeighborIndices& nn_indices, Normals& normals)
+{
+ NormalsEstimator est;
+ est.indices = nn_indices;
+ est.sizes = nn_indices.sizes;
+ est.points = cloud;
+ est.normals = normals;
+
+ //printFuncAttrib(EstimateNormaslKernel);
+
+ int block = NormalsEstimator::CTA_SIZE;
+ int grid = divUp((int)normals.size(), NormalsEstimator::WAPRS);
+ EstimateNormaslKernel<<<grid, block>>>(est);
+
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall(cudaDeviceSynchronize());
+}
+
+void pcl::device::flipNormalTowardsViewpoint(const PointCloud& cloud, const float3& vp, Normals& normals)
+{
+ int block = 256;
+ int grid = divUp((int)normals.size(), block);
+
+ FlipNormal flip;
+ flip.cloud = cloud;
+ flip.vp = vp;
+ flip.normals = normals;
+
+ flipNormalTowardsViewpointKernel<<<grid, block>>>(flip);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall(cudaDeviceSynchronize());
+}
+
+void pcl::device::flipNormalTowardsViewpoint(const PointCloud& cloud, const Indices& indices, const float3& vp, Normals& normals)
+{
+ int block = 256;
+ int grid = divUp((int)normals.size(), block);
+
+ FlipNormal flip;
+ flip.cloud = cloud;
+ flip.vp = vp;
+ flip.normals = normals;
+
+ flipNormalTowardsViewpointKernel<<<grid, block>>>(flip, indices.ptr());
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall(cudaDeviceSynchronize());
+}
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include "internal.hpp"
+
+#include "pcl/gpu/utils/safe_call.hpp"
+#include "pcl/gpu/utils/device/warp.hpp"
+#include "pcl/gpu/utils/device/block.hpp"
+#include "utils/vector_operations.hpp"
+#include "pcl/gpu/utils/device/funcattrib.hpp"
+#include "pcl/gpu/features/device/pair_features.hpp"
+
+using namespace pcl::gpu;
+
+namespace pcl
+{
+ namespace device
+ {
+ template<bool pack_rgb>
+ struct Repack
+ {
+ enum
+ {
+ CTA_SIZE = 256,
+ WARPS = CTA_SIZE/Warp::WARP_SIZE
+ };
+
+ const PointType* cloud;
+ const NormalType* normals;
+ int work_size;
+
+ PtrStep<int> gindices;
+ const int* sizes;
+
+ mutable PtrStep<float> output;
+ int max_elems;
+
+ __device__ void operator()() const
+ {
+ int idx = WARPS * blockIdx.x + Warp::id();
+
+ if (idx >= work_size)
+ return;
+
+ const int *nbeg = gindices.ptr(idx);
+ int size = sizes[idx];
+ int idx_shift = max_elems * idx;
+
+ for(int i = Warp::laneId(); i < size; i += Warp::STRIDE)
+ {
+ int cloud_index = nbeg[i];
+
+ float3 p;
+
+ if (pack_rgb)
+ {
+ int color;
+ p = fetchXYZRGB(cloud, cloud_index, color);
+ output.ptr(6)[i + idx_shift] = __int_as_float(color);
+ }
+ else
+ p = fetch(cloud, cloud_index);
+
+ output.ptr(0)[i + idx_shift] = p.x;
+ output.ptr(1)[i + idx_shift] = p.y;
+ output.ptr(2)[i + idx_shift] = p.z;
+
+
+ float3 n = fetch(normals, cloud_index);
+
+ output.ptr(3)[i + idx_shift] = n.x;
+ output.ptr(4)[i + idx_shift] = n.y;
+ output.ptr(5)[i + idx_shift] = n.z;
+ }
+ }
+
+ template<class It>
+ __device__ __forceinline__ float3 fetch(It ptr, int index) const
+ {
+ //return tr(ptr[index]);
+ return *(float3*)&ptr[index];
+ }
+
+ __forceinline__ __device__ float3 fetchXYZRGB(const PointXYZRGB* data, int index, int& color) const
+ {
+ float4 xyzrgb = data[index];
+ color = __float_as_int(xyzrgb.w);
+ return make_float3(xyzrgb.x, xyzrgb.y, xyzrgb.z);
+ }
+ };
+
+ template<bool enable_rgb>
+ struct Pfh125
+ {
+ enum
+ {
+ CTA_SIZE = 256,
+
+ NR_SPLIT = 5,
+ NR_SPLIT_2 = NR_SPLIT * NR_SPLIT,
+ NR_SPLIT_3 = NR_SPLIT_2 * NR_SPLIT,
+
+ FSize = NR_SPLIT * NR_SPLIT * NR_SPLIT * (enable_rgb ? 2 : 1)
+ };
+
+ size_t work_size;
+ const int* sizes;
+
+ PtrStep<float> rpk;
+ int max_elems;
+
+ mutable PtrStep<float> output;
+
+ __device__ __forceinline__ void operator()() const
+ {
+ int idx = blockIdx.x;
+
+ if (idx >= work_size)
+ return;
+
+ int size = sizes[idx];
+ int size2 = size * size;
+ int idx_shift = max_elems * idx;
+
+ float hist_incr = 100.f / (size2 - 1);
+
+ __shared__ float pfh_histogram[FSize];
+ Block::fill(pfh_histogram, pfh_histogram + FSize, 0.f);
+ __syncthreads();
+
+ // Iterate over all the points in the neighborhood
+ int i = threadIdx.y * blockDim.x + threadIdx.x;
+ int stride = Block::stride();
+
+ for( ; i < size2; i += stride )
+ {
+ int i_idx = i / size + idx_shift;
+ int j_idx = i % size + idx_shift;
+
+ if (i_idx != j_idx)
+ {
+ float3 pi, ni, pj, nj;
+ pi.x = rpk.ptr(0)[i_idx];
+ pj.x = rpk.ptr(0)[j_idx];
+
+ pi.y = rpk.ptr(1)[i_idx];
+ pj.y = rpk.ptr(1)[j_idx];
+
+ pi.z = rpk.ptr(2)[i_idx];
+ pj.z = rpk.ptr(2)[j_idx];
+
+ ni.x = rpk.ptr(3)[i_idx];
+ nj.x = rpk.ptr(3)[j_idx];
+
+ ni.y = rpk.ptr(4)[i_idx];
+ nj.y = rpk.ptr(4)[j_idx];
+
+ ni.z = rpk.ptr(5)[i_idx];
+ nj.z = rpk.ptr(5)[j_idx];
+
+ float f1, f2, f3, f4;
+ // Compute the pair NNi to NNj
+ computePairFeatures (pi, ni, pj, nj, f1, f2, f3, f4);
+ //if (computePairFeatures (pi, ni, pj, nj, f1, f2, f3, f4))
+ {
+ // Normalize the f1, f2, f3 features and push them in the histogram
+ int find0 = floor( NR_SPLIT * ((f1 + PI) * (1.f / (2.f * PI))) );
+ find0 = min(NR_SPLIT - 1, max(0, find0));
+
+ int find1 = floor( NR_SPLIT * ( (f2 + 1.f) * 0.5f ) );
+ find1 = min(NR_SPLIT - 1, max(0, find1));
+
+ int find2 = floor( NR_SPLIT * ( (f3 + 1.f) * 0.5f ) );
+ find2 = min(NR_SPLIT - 1, max(0, find2));
+
+ int h_index = find0 + NR_SPLIT * find1 + NR_SPLIT_2 * find2;
+ atomicAdd(pfh_histogram + h_index, hist_incr);
+
+ if (enable_rgb)
+ {
+ int ci = __float_as_int(rpk.ptr(6)[i_idx]);
+ int cj = __float_as_int(rpk.ptr(6)[j_idx]);
+
+ float f5, f6, f7;
+ computeRGBPairFeatures_RGBOnly(ci, cj, f5, f6, f7);
+
+ // color ratios are in [-1, 1]
+ int find4 = floor (NR_SPLIT * ((f5 + 1.f) * 0.5f));
+ find4 = min(NR_SPLIT - 1, max(0, find4));
+
+ int find5 = floor (NR_SPLIT * ((f6 + 1.f) * 0.5f));
+ find5 = min(NR_SPLIT - 1, max(0, find5));
+
+ int find6 = floor (NR_SPLIT * ((f7 + 1.f) * 0.5f));
+ find6 = min(NR_SPLIT - 1, max(0, find6));
+
+ // and the colors
+ h_index = NR_SPLIT_3 + find4 + NR_SPLIT * find5 + NR_SPLIT_2 * find6;
+ atomicAdd(pfh_histogram + h_index, hist_incr);
+ }
+ }
+ }
+ }
+ __syncthreads();
+ Block::copy(pfh_histogram, pfh_histogram + FSize, output.ptr(idx));
+ }
+
+ template<class It>
+ __device__ __forceinline__ float3 fetch(It ptr, int index) const
+ {
+ //return tr(ptr[index]);
+ return *(float3*)&ptr[index];
+ }
+ };
+
+ __global__ void repackKernel(const Repack<false> repack) { repack(); }
+ __global__ void pfhKernel(const Pfh125<false> pfh125) { pfh125(); }
+ }
+}
+
+void pcl::device::repackToAosForPfh(const PointCloud& cloud, const Normals& normals, const NeighborIndices& neighbours, DeviceArray2D<float>& data_rpk, int& max_elems_rpk)
+{
+ max_elems_rpk = (neighbours.max_elems/32 + 1) * 32;
+ data_rpk.create(6, (int)neighbours.sizes.size() * max_elems_rpk);
+
+ Repack<false> rpk;
+ rpk.sizes = neighbours.sizes;
+ rpk.gindices = neighbours;
+
+ rpk.cloud = cloud;
+ rpk.normals = normals;
+ rpk.work_size = (int)neighbours.sizes.size();
+
+ rpk.output = data_rpk;
+ rpk.max_elems = max_elems_rpk;
+
+ int block = Repack<false>::CTA_SIZE;
+ int grid = divUp(rpk.work_size, Repack<false>::WARPS);
+
+ device::repackKernel<<<grid, block>>>(rpk);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+
+ //printFuncAttrib(repackKernel);
+}
+
+void pcl::device::computePfh125(const DeviceArray2D<float>& data_rpk, int max_elems_rpk, const NeighborIndices& neighbours, DeviceArray2D<PFHSignature125>& features)
+{
+ Pfh125<false> fph;
+ fph.work_size = neighbours.sizes.size();
+ fph.sizes = neighbours.sizes;
+ fph.rpk = data_rpk;
+ fph.max_elems = max_elems_rpk;
+ fph.output = features;
+
+ int block = Pfh125<false>::CTA_SIZE;
+ int grid = (int)fph.work_size;
+
+ device::pfhKernel<<<grid, block>>>(fph);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+
+ //printFuncAttrib(pfhKernel);
+}
+
+
+namespace pcl
+{
+ namespace device
+ {
+
+ __global__ void repackRgbKernel(const Repack<true> repack) { repack(); }
+ __global__ void pfhRgbKernel(const Pfh125<true> pfhrgb125) { pfhrgb125(); }
+ }
+}
+
+
+void pcl::device::repackToAosForPfhRgb(const PointCloud& cloud, const Normals& normals, const NeighborIndices& neighbours, DeviceArray2D<float>& data_rpk, int& max_elems_rpk)
+{
+ max_elems_rpk = (neighbours.max_elems/32 + 1) * 32;
+ data_rpk.create(7, (int)neighbours.sizes.size() * max_elems_rpk);
+
+ Repack<true> rpk;
+ rpk.sizes = neighbours.sizes;
+ rpk.gindices = neighbours;
+
+ rpk.cloud = cloud;
+ rpk.normals = normals;
+ rpk.work_size = (int)neighbours.sizes.size();
+
+ rpk.output = data_rpk;
+ rpk.max_elems = max_elems_rpk;
+
+ int block = Repack<true>::CTA_SIZE;
+ int grid = divUp(rpk.work_size, Repack<true>::WARPS);
+
+ device::repackRgbKernel<<<grid, block>>>(rpk);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+
+ //printFuncAttrib(repackRgbKernel);
+}
+
+
+void pcl::device::computePfhRgb250(const DeviceArray2D<float>& data_rpk, int max_elems_rpk, const NeighborIndices& neighbours, DeviceArray2D<PFHRGBSignature250>& features)
+{
+ Pfh125<true> pfhrgb;
+ pfhrgb.work_size = neighbours.sizes.size();
+ pfhrgb.sizes = neighbours.sizes;
+ pfhrgb.rpk = data_rpk;
+ pfhrgb.max_elems = max_elems_rpk;
+ pfhrgb.output = features;
+
+ int block = Pfh125<true>::CTA_SIZE;
+ int grid = (int)pfhrgb.work_size;
+
+ device::pfhRgbKernel<<<grid, block>>>(pfhrgb);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+
+ //printFuncAttrib(pfhRgbKernel);
+
+}
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include "internal.hpp"
+#include "pcl/gpu/utils/safe_call.hpp"
+#include "utils/vector_operations.hpp"
+#include "pcl/gpu/utils/device/funcattrib.hpp"
+#include "pcl/gpu/utils/device/warp.hpp"
+
+#include "pcl/gpu/features/device/rodrigues.hpp"
+#include "pcl/gpu/features/device/pair_features.hpp"
+
+namespace pcl
+{
+ namespace device
+ {
+ struct PpfImpl
+ {
+ enum
+ {
+ CTA_SIZE = 256
+ };
+
+ PtrSz<PointType> points;
+ const NormalType *normals;
+ PtrSz<int> indices;
+
+ mutable PPFSignature* output;
+
+ __device__ __forceinline__ void operator()() const
+ {
+ int total = points.size * indices.size;
+ int idx = blockIdx.x * CTA_SIZE + threadIdx.x;
+
+ if (idx > total)
+ return;
+
+ int index_i = idx / points.size; // indices
+
+ int j = idx % points.size; // points
+ int i = indices.data[index_i];
+
+ PPFSignature out;
+
+ if (i != j)
+ {
+ float3 pi = fetch(points.data, i);
+ float3 ni = fetch(normals, i);
+
+ float3 pj = fetch(points.data, j);
+ float3 nj = fetch(normals, j);
+
+ //if (computePPFPairFeature(pi, ni, pj, nj, out.f1, out.f2, out.f3, out.f4))
+ if (computePairFeatures(pi, ni, pj, nj, out.f1, out.f2, out.f3, out.f4))
+ computeAlfaM(pi, ni, pj, out.alpha_m);
+ else
+ out.f1 = out.f2 = out.f3 = out.f4 = out.alpha_m = 0.f;
+ }
+ else
+ out.f1 = out.f2 = out.f3 = out.f4 = out.alpha_m = 0.f;
+
+ output[idx] = out;
+ }
+
+ template<class T> __forceinline__ __device__ float3 fetch(const T* data, int index) const
+ {
+ //return *(float3*)&data[index];
+ T t = data[index];
+ return make_float3(t.x, t.y, t.z);
+ }
+ };
+
+ __global__ void estimatePpfKernel(const PpfImpl ppf) { ppf(); }
+
+
+
+ struct PpfRgbImpl
+ {
+ enum
+ {
+ CTA_SIZE = 256
+ };
+
+ PtrSz<PointXYZRGB> points;
+ const NormalType *normals;
+ PtrSz<int> indices;
+
+ mutable PPFRGBSignature* output;
+
+ __device__ __forceinline__ void operator()() const
+ {
+ int total = points.size * indices.size;
+ int idx = blockIdx.x * CTA_SIZE + threadIdx.x;
+
+ if (idx > total)
+ return;
+
+ int index_i = idx / points.size; // indices
+
+ int j = idx % points.size; // points
+ int i = indices.data[index_i];
+
+ PPFRGBSignature out;
+
+ if (i != j)
+ {
+ int ci;
+ float3 pi = fetchXYZRGB(points.data, i, ci);
+ float3 ni = fetch(normals, i);
+
+ int cj;
+ float3 pj = fetchXYZRGB(points.data, j, cj);
+ float3 nj = fetch(normals, j);
+
+ if (computeRGBPairFeatures(pi, ni, ci, pj, nj, cj, out.f1, out.f2, out.f3, out.f4, out.r_ratio, out.g_ratio, out.b_ratio))
+ //if (computePairFeatures(pi, ni, pj, nj, out.f1, out.f2, out.f3, out.f4))
+ {
+ computeAlfaM(pi, ni, pj, out.alpha_m);
+ //computeRGBPairFeatures_RGBOnly(ci, cj, out.r_ratio, out.g_ratio, out.b_ratio);
+ }
+ else
+ out.f1 = out.f2 = out.f3 = out.f4 = out.r_ratio = out.g_ratio = out.b_ratio = out.alpha_m = 0.f;
+ }
+ else
+ out.f1 = out.f2 = out.f3 = out.f4 = out.r_ratio = out.g_ratio = out.b_ratio = out.alpha_m = 0.f;
+
+
+
+ output[idx] = out;
+ }
+
+ template<class T> __forceinline__ __device__ float3 fetch(const T* data, int index) const
+ {
+ //return *(float3*)&data[index];
+ T t = data[index];
+ return make_float3(t.x, t.y, t.z);
+ }
+
+ __forceinline__ __device__ float3 fetchXYZRGB(const PointXYZRGB* data, int index, int& color) const
+ {
+ float4 xyzrgb = data[index];
+ color = __float_as_int(xyzrgb.w);
+ return make_float3(xyzrgb.x, xyzrgb.y, xyzrgb.z);
+ }
+ };
+
+ __global__ void estimatePpfRgbKernel(const PpfRgbImpl ppfrgb) { ppfrgb(); }
+
+ }
+}
+
+void pcl::device::computePPF(const PointCloud& input, const Normals& normals, const Indices& indices, DeviceArray<PPFSignature>& output)
+{
+ int total = (int)input.size() * (int)indices.size();
+ output.create(total);
+
+ PpfImpl ppf;
+ ppf.points = input;
+ ppf.normals = normals;
+ ppf.indices = indices;
+ ppf.output = output;
+
+ int block = PpfImpl::CTA_SIZE;
+ int grid = divUp(total, block);
+ estimatePpfKernel<<<grid, block>>>(ppf);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+
+ //printFuncAttrib(estimatePpfKernel);
+}
+
+
+void pcl::device::computePPFRGB(const PointXYZRGBCloud& input, const Normals& normals, const Indices& indices, DeviceArray<PPFRGBSignature>& output)
+{
+ int total = (int)input.size() * (int)indices.size();
+ output.create(total);
+
+ PpfRgbImpl ppfrgb;
+ ppfrgb.points = input;
+ ppfrgb.normals = normals;
+ ppfrgb.indices = indices;
+ ppfrgb.output = output;
+
+ int block = PpfRgbImpl::CTA_SIZE;
+ int grid = divUp(total, block);
+ estimatePpfRgbKernel<<<grid, block>>>(ppfrgb);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+
+ //printFuncAttrib(estimatePpfRgbKernel);
+}
+
+
+namespace pcl
+{
+ namespace device
+ {
+ struct PpfRgbRegionImpl
+ {
+ enum
+ {
+ CTA_SIZE = 256,
+ WARPS = CTA_SIZE / Warp::WARP_SIZE,
+ FSize = sizeof(PPFRGBSignature)/sizeof(float),
+ FSizeWithoutAlfaM = FSize - 1
+ };
+
+ struct plus
+ {
+ __forceinline__ __device__ float operator()(const float &lhs, const volatile float& rhs) const { return lhs + rhs; }
+ };
+
+ const PointXYZRGB* points;
+ const NormalType* normals;
+ PtrSz<int> indices;
+
+ PtrStep<int> gindices;
+ const int *sizes;
+
+ mutable PPFRGBSignature* output;
+
+ __device__ __forceinline__ void operator()() const
+ {
+ int tid = threadIdx.x;
+ int warpid = Warp::id();
+ int index_i = blockIdx.x * WARPS + warpid;
+
+ if (index_i >= indices.size)
+ return;
+
+ int i = indices[index_i];
+ int size = sizes[index_i];
+ const int* ginds = gindices.ptr(index_i);
+
+ int lane = Warp::laneId();
+
+ __shared__ float3 points_buf[WARPS];
+ __shared__ float3 normasl_buf[WARPS];
+ __shared__ int colors_buf[WARPS];
+
+ if (lane == 0)
+ {
+ points_buf[warpid] = fetchXYZRGB(points, i, colors_buf[warpid]);
+ normasl_buf[warpid] = fetch(normals, i);
+ }
+
+ __shared__ float cta_buf[7][CTA_SIZE + 1];
+ cta_buf[0][tid] = cta_buf[1][tid] = cta_buf[2][tid] = cta_buf[3][tid] = 0.f;
+ cta_buf[4][tid] = cta_buf[5][tid] = cta_buf[6][tid] = 0.f;
+
+ for(int c = lane; c < size; c+= Warp::STRIDE)
+ {
+ int j = ginds[c];
+
+ if (i != j)
+ {
+ int cj;
+ float3 pj = fetchXYZRGB(points, j, cj);
+ float3 nj = fetch(normals, j);
+
+ float f1, f2, f3, f4, r_ratio, g_ratio, b_ratio;
+ if (computeRGBPairFeatures(points_buf[warpid], normasl_buf[warpid], colors_buf[warpid], pj, nj, cj, f1, f2, f3, f4, r_ratio, g_ratio, b_ratio))
+ //computeRGBPairFeatures(points_buf[warpid], normasl_buf[warpid], colors_buf[warpid], pj, nj, cj, f1, f2, f3, f4, r_ratio, g_ratio, b_ratio);
+ {
+ cta_buf[0][tid] += f1;
+ cta_buf[1][tid] += f2;
+ cta_buf[2][tid] += f3;
+ cta_buf[3][tid] += f4;
+ cta_buf[4][tid] += r_ratio;
+ cta_buf[5][tid] += g_ratio;
+ cta_buf[6][tid] += b_ratio;
+ }
+ }
+ }
+
+ Warp::reduce(&cta_buf[0][tid - lane], plus());
+ Warp::reduce(&cta_buf[1][tid - lane], plus());
+ Warp::reduce(&cta_buf[2][tid - lane], plus());
+ Warp::reduce(&cta_buf[3][tid - lane], plus());
+
+ Warp::reduce(&cta_buf[4][tid - lane], plus());
+ Warp::reduce(&cta_buf[5][tid - lane], plus());
+ Warp::reduce(&cta_buf[6][tid - lane], plus());
+
+ float val = 0.f;
+ if (lane < FSizeWithoutAlfaM)
+ val = cta_buf[lane][tid - lane]/size;
+
+ float *ptr = (float*)&output[index_i];
+ if (lane < FSize)
+ ptr[lane] = val;
+ }
+
+ __forceinline__ __device__ float3 fetchXYZRGB(const PointXYZRGB* data, int index, int& color) const
+ {
+ float4 xyzrgb = data[index];
+ color = __float_as_int(xyzrgb.w);
+ return make_float3(xyzrgb.x, xyzrgb.y, xyzrgb.z);
+ }
+ template<class T> __forceinline__ __device__ float3 fetch(const T* data, int index) const
+ {
+ //return *(float3*)&data[index];
+ T t = data[index];
+ return make_float3(t.x, t.y, t.z);
+ }
+ };
+ __global__ void estiamtePpfRgbRegionKernel(const PpfRgbRegionImpl impl) { impl(); }
+ }
+}
+
+void pcl::device::computePPFRGBRegion(const PointXYZRGBCloud& cloud, const Normals& normals, const Indices& indices, const NeighborIndices& nn_indices, DeviceArray<PPFRGBSignature>& output)
+{
+ output.create(nn_indices.sizes.size());
+
+ PpfRgbRegionImpl impl;
+ impl.points = cloud;
+ impl.normals = normals;
+ impl.indices = indices;
+ impl.gindices = nn_indices;
+ impl.sizes = nn_indices.sizes;
+ impl.output = output;
+
+ int block = PpfRgbRegionImpl::CTA_SIZE;
+ int grid = divUp((int)impl.indices.size, PpfRgbRegionImpl::WARPS);
+
+ estiamtePpfRgbRegionKernel<<<grid, block>>>(impl);
+
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall(cudaDeviceSynchronize());
+
+ //printFuncAttrib(estiamtePpfRgbRegionKernel);
+}
\ No newline at end of file
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#include "internal.hpp"
+#include "pcl/gpu/utils/device/funcattrib.hpp"
+#include "pcl/gpu/utils/device/warp.hpp"
+#include "pcl/gpu/utils/safe_call.hpp"
+#include "pcl/gpu/utils/device/block.hpp"
+#include "pcl/gpu/features/device/eigen.hpp"
+
+//#define TWO_STEP_ALG
+
+namespace pcl
+{
+ namespace device
+ {
+
+ __device__ __forceinline__ float3 operator*(const Eigen33::Mat33& m, float3 v)
+ {
+ float3 r;
+ r.x = m[0].x * v.x + m[0].y * v.y + m[0].z * v.z;
+ r.y = m[1].x * v.x + m[1].y * v.y + m[1].z * v.z;
+ r.z = m[2].x * v.x + m[2].y * v.y + m[2].z * v.z;
+ return r;
+ }
+
+ struct PrincipalCurvaturesImpl
+ {
+ enum
+ {
+ CTA_SIZE = 256,
+ WARPS = CTA_SIZE/Warp::WARP_SIZE
+ };
+
+ struct plus
+ {
+ __forceinline__ __device__ float operator()(const float &lhs, const volatile float& rhs) const { return lhs + rhs; }
+ };
+
+ const NormalType *normals;
+ const int *indices;
+
+ PtrStep<int> neighbours;
+ const int *sizes;
+
+ int work_size;
+
+ mutable PtrStep<float> proj_normals;
+ int max_elems;
+
+ mutable PrincipalCurvatures* output;
+
+ __device__ __forceinline__ void operator()() const
+ {
+ __shared__ float cov_buffer[6][CTA_SIZE + 1];
+
+ int warpid = Warp::id();
+ int f = blockIdx.x * WARPS + warpid;
+
+ if (f >= work_size)
+ return;
+
+ int lane = Warp::laneId();
+ int size = sizes[f];
+
+ int p_idx = indices ? indices[f] : f;
+
+ float3 n_idx_f3 = fetch(normals, p_idx);
+ float n_idx[] = { n_idx_f3.x, n_idx_f3.y, n_idx_f3.z};
+
+ __shared__ Eigen33::Mat33 mats[WARPS];
+
+ Eigen33::Mat33& M = mats[warpid];
+
+ /*if (lane == 0)
+ {
+ M[0].x = -n_idx.x * n_idx.x; M[0].y = -n_idx.x * n_idx.y; M[0].z = -n_idx.x * n_idx.z;
+ M[1].x = -n_idx.y * n_idx.x; M[1].y = -n_idx.y * n_idx.y; M[1].z = -n_idx.y * n_idx.z;
+ M[2].x = -n_idx.z * n_idx.x; M[2].y = -n_idx.z * n_idx.y; M[2].z = -n_idx.z * n_idx.z;
+
+ M[0].x += 1.f; M[1].y += 1.f; M[3].z += 1.f;
+ }*/
+
+ if (lane < 9)
+ {
+ float unit = ((lane & 3) == 0) ? 1.f : 0.f;
+
+ int y = lane / 3;
+ int x = lane - y * 3;
+
+ float* mat = (float*)&M;
+ mat[lane] = unit - n_idx[y] * n_idx[x];
+ }
+
+ const int* neighbs = neighbours.ptr(p_idx);
+
+ int idx_shift = max_elems * f;
+
+ float3 centroid = make_float3(0.f, 0.f, 0.f);
+ for(int i = lane; i < size; i += Warp::STRIDE)
+ {
+ int neighb_idx = neighbs[i];
+
+ float3 normal = fetch(normals, neighb_idx);
+
+ float3 proj_normal = M * normal;
+
+ centroid.x += proj_normal.x;
+ centroid.y += proj_normal.y;
+ centroid.z += proj_normal.z;
+
+ proj_normals.ptr(0)[i + idx_shift] = proj_normal.x;
+ proj_normals.ptr(1)[i + idx_shift] = proj_normal.y;
+ proj_normals.ptr(2)[i + idx_shift] = proj_normal.z;
+ }
+
+ volatile float *buffer = &cov_buffer[0][threadIdx.x - lane];
+ centroid.x = Warp::reduce(buffer, centroid.x, plus());
+ centroid.y = Warp::reduce(buffer, centroid.y, plus());
+ centroid.z = Warp::reduce(buffer, centroid.z, plus());
+ centroid *= 1.f/size;
+
+ //nvcc bug work workaround.
+ __threadfence_block();
+
+ //compute covariance matrix
+ int tid = threadIdx.x;
+
+ for(int i = 0; i < 6; ++i)
+ cov_buffer[i][tid] = 0.f;
+
+ for(int i = lane; i < size; i += Warp::STRIDE)
+ {
+ float3 proj_normal;
+ proj_normal.x = proj_normals.ptr(0)[i + idx_shift];
+ proj_normal.y = proj_normals.ptr(1)[i + idx_shift];
+ proj_normal.z = proj_normals.ptr(2)[i + idx_shift];
+
+ float3 d = proj_normal - centroid;
+
+ cov_buffer[0][tid] += d.x * d.x; //cov (0, 0)
+ cov_buffer[1][tid] += d.x * d.y; //cov (0, 1)
+ cov_buffer[2][tid] += d.x * d.z; //cov (0, 2)
+ cov_buffer[3][tid] += d.y * d.y; //cov (1, 1)
+ cov_buffer[4][tid] += d.y * d.z; //cov (1, 2)
+ cov_buffer[5][tid] += d.z * d.z; //cov (2, 2)
+ }
+
+ Warp::reduce(&cov_buffer[0][tid - lane], plus());
+ Warp::reduce(&cov_buffer[1][tid - lane], plus());
+ Warp::reduce(&cov_buffer[2][tid - lane], plus());
+ Warp::reduce(&cov_buffer[3][tid - lane], plus());
+ Warp::reduce(&cov_buffer[4][tid - lane], plus());
+ Warp::reduce(&cov_buffer[5][tid - lane], plus());
+
+ volatile float *cov = &cov_buffer[0][tid-lane];
+ if (lane < 6)
+ cov[lane] = cov_buffer[lane][tid-lane];
+
+ if (lane == 0)
+ {
+ // Extract the eigenvalues and eigenvectors
+ Eigen33 eigen33(cov);
+
+ Eigen33::Mat33& tmp = (Eigen33::Mat33&)cov_buffer[1][tid - lane];
+ Eigen33::Mat33& vec_tmp = (Eigen33::Mat33&)cov_buffer[2][tid - lane];
+ Eigen33::Mat33& evecs = (Eigen33::Mat33&)cov_buffer[3][tid - lane];
+
+ float3 evals;
+ eigen33.compute(tmp, vec_tmp, evecs, evals);
+
+ PrincipalCurvatures out;
+ out.principal_curvature_x = evecs[2].x;
+ out.principal_curvature_y = evecs[2].y;
+ out.principal_curvature_z = evecs[2].z;
+ float indices_size_inv = 1.f / size;
+ out.pc1 = evals.z * indices_size_inv;
+ out.pc2 = evals.y * indices_size_inv;
+
+ output[f] = out;
+ }
+ }
+
+ template<class T> __forceinline__ __device__ float3 fetch(const T* data, int index) const
+ {
+ //return *(float3*)&data[index];
+ T t = data[index];
+ return make_float3(t.x, t.y, t.z);
+ }
+ };
+
+ __global__ void principalCurvaturesKernel(const PrincipalCurvaturesImpl impl) { impl(); }
+ }
+}
+
+void pcl::device::computePointPrincipalCurvatures(const Normals& normals, const Indices& indices, const NeighborIndices& neighbours, DeviceArray<PrincipalCurvatures>& output, DeviceArray2D<float>& proj_normals)
+{
+ proj_normals.create(3, (int)neighbours.data.size());
+
+ PrincipalCurvaturesImpl impl;
+ impl.normals = normals;
+ impl.indices = indices;
+
+ impl.neighbours = neighbours;
+ impl.sizes = neighbours.sizes;
+
+ impl.proj_normals = proj_normals;
+ impl.max_elems = neighbours.max_elems;
+
+ impl.output = output;
+
+ impl.work_size = indices.empty() ? (int)normals.size() : (int)indices.size();
+
+ int block = PrincipalCurvaturesImpl::CTA_SIZE;
+ int grid = divUp(impl.work_size, PrincipalCurvaturesImpl::WARPS);
+ principalCurvaturesKernel<<<grid, block>>>(impl);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+
+ //printFuncAttrib(principalCurvaturesKernel);
+ //printFuncAttrib(principalCurvaturesStep2<256>);
+}
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#include "internal.hpp"
+#include "pcl/gpu/utils/device/warp.hpp"
+#include "pcl/gpu/utils/device/block.hpp"
+#include "pcl/gpu/utils/device/limits.hpp"
+#include "pcl/gpu/utils/device/vector_math.hpp"
+#include "pcl/gpu/utils/device/functional.hpp"
+
+#include "pcl/gpu/utils/safe_call.hpp"
+
+#include "thrust/transform.h"
+#include "thrust/device_ptr.h"
+
+namespace pcl
+{
+ namespace device
+ {
+ //[spinimage][angles] = [0..FSize][..FSize]
+ extern __shared__ float simage_angles[];
+
+ template<class It> __device__ __forceinline__ float3 fetch(It ptr, int index) { return *(float3*)&ptr[index]; }
+ //template<class It> __device__ __forceinline__ float3 fetch(It ptr, int index) { return tr(ptr[index]); }
+
+ struct UseCustomAxis
+ {
+ float3 rotation_axis;
+ __device__ __forceinline__ float3 getRotationAxes(int /*index*/, const float3& /*normal*/) const { return rotation_axis; }
+ };
+
+ struct UseCustomAxesCloud
+ {
+ const NormalType* rotation_axes_cloud;
+ __device__ __forceinline__ float3 getRotationAxes(int index, const float3& /*normal*/) const { return fetch(rotation_axes_cloud, index); }
+ };
+
+ struct UseOriginNormal
+ {
+ __device__ __forceinline__ float3 getRotationAxes(int /*index*/, const float3& normal) const { return normal; }
+ };
+
+ struct Div12eps
+ {
+ __device__ __forceinline__ float operator()(float v1, float v2) const { return (float)(v1 / ( v2 + numeric_limits<double>::epsilon() )); }
+ };
+
+ struct DivValIfNonZero
+ {
+ float val;
+ __device__ __forceinline__ DivValIfNonZero(float value) : val(value) {}
+ __device__ __forceinline__ float operator()(float v) const { return val == 0 ? v : v/val; }
+ };
+
+ template<bool radial, bool angular, typename AxesStrategy>
+ struct SpinImpl : public AxesStrategy
+ {
+ enum
+ {
+ CTA_SIZE = 192
+ };
+
+ int work_size;
+ const int* indices;
+
+ const PointType* input_cloud;
+ const NormalType* input_normals;
+
+ const PointType* surface;
+ const NormalType* normals;
+
+ PtrStep<int> neighbor_indices;
+ const int* neighbor_indices_sizes;
+ float support_angle_cos;
+
+ int min_neighb;
+ int image_width;
+ float bin_size;
+ int FSize;
+
+ mutable PtrStep<float> output;
+
+ static __device__ __host__ __forceinline__ int computeFSize(int image_width)
+ {
+ int cols = 1 + image_width * 2;
+ int rows = 1 + image_width;
+ return cols * rows;
+ }
+
+ __device__ __forceinline__ void operator()() const
+ {
+ int i_input = blockIdx.x + gridDim.x * blockIdx.y;
+
+ int index = indices[i_input];
+
+ int neighb_count = neighbor_indices_sizes[i_input];
+ const int *ginds = neighbor_indices.ptr (i_input);
+
+ if (neighb_count < min_neighb)
+ return;
+
+ //set zeros to spin image
+ Block::fill(simage_angles, simage_angles + FSize, 0.f);
+ if (angular) //set zeros to angles
+ Block::fill(simage_angles + FSize, simage_angles + FSize + FSize, 0.f);
+
+ __syncthreads();
+
+ float3 origin_point = fetch(input_cloud, index);
+ float3 origin_normal = input_normals ? fetch(input_normals, index) : make_float3(0.f, 0.f, 0.f);
+ origin_normal = normalized_safe(origin_normal); //normalize if non-zero
+
+ float3 rotation_axis = AxesStrategy::getRotationAxes(index, origin_normal);
+ rotation_axis = normalized_safe(rotation_axis); //normalize if non-zero
+
+ const float eps = numeric_limits<float>::epsilon ();
+
+ for(int i_neighb = threadIdx.x; i_neighb < neighb_count; i_neighb += CTA_SIZE)
+ {
+ int neighb_index = ginds[i_neighb];
+
+ // first, skip the points with distant normals
+ float cos_between_normals = -2.f;
+ if (angular || support_angle_cos > 0.f) // not bogus
+ {
+ float3 normal = normalized(fetch(normals, neighb_index));
+ cos_between_normals = dot(origin_normal, normal);
+ cos_between_normals = fmax (-1.f, fmin (1.f, cos_between_normals));
+
+ if (fabs(cos_between_normals) < support_angle_cos) // allow counter-directed normals
+ continue;
+
+ cos_between_normals = fabs(cos_between_normals); // the normal is not used explicitly from now
+ }
+
+ // now compute the coordinate in cylindric coordinate system associated with the origin point
+ float3 direction = fetch(surface, neighb_index) - origin_point;
+ float direction_norm = norm (direction);
+
+ // ignore the point itself; it does not contribute really
+ if (direction_norm < 10 * eps)
+ continue;
+
+ // the angle between the normal vector and the direction to the point
+ float cos_dir_axis = dot(direction, rotation_axis) / direction_norm;
+ cos_dir_axis = fmax(-1.f, fmin(1.f, cos_dir_axis));
+
+ // compute coordinates w.r.t. the reference frame
+ float beta = numeric_limits<float>::quiet_NaN();
+ float alpha = numeric_limits<float>::quiet_NaN();
+
+ if (radial) // radial spin image structure
+ {
+ beta = asinf(cos_dir_axis); // yes, arc sine! to get the angle against tangent, not normal!
+ alpha = direction_norm;
+ }
+ else // rectangular spin-image structure
+ {
+ beta = direction_norm * cos_dir_axis;
+ alpha = direction_norm * sqrt (1.0 - cos_dir_axis*cos_dir_axis);
+
+ if (fabs (beta) >= bin_size * image_width || alpha >= bin_size * image_width)
+ continue; // outside the cylinder
+ }
+
+ // bilinear interpolation
+ float beta_bin_size = radial ? (PI*0.5f/image_width) : bin_size;
+ int beta_bin = floorf(beta / beta_bin_size) + image_width;
+ int alpha_bin = floorf(alpha / bin_size);
+
+ //alpha_bin = min(simage_cols, max(0, alpha_bin));
+ //beta_bin = min(simage_rows, max(0, beta_bin));
+
+ if (alpha_bin == image_width) // border points
+ {
+ alpha_bin--;
+ // HACK: to prevent a > 1
+ alpha = bin_size * (alpha_bin + 1) - eps;
+ }
+ if (beta_bin == 2*image_width ) // border points
+ {
+ beta_bin--;
+ // HACK: to prevent b > 1
+ beta = beta_bin_size * (beta_bin - image_width + 1) - eps;
+ }
+
+ float a = alpha/bin_size - alpha_bin;
+ float b = beta/beta_bin_size - float(beta_bin-image_width);
+
+ incSpinI(alpha_bin, beta_bin, (1-a) * (1-b));
+ incSpinI(alpha_bin+1, beta_bin, a * (1-b));
+ incSpinI(alpha_bin, beta_bin+1, (1-a) * b );
+ incSpinI(alpha_bin+1, beta_bin+1, a * b );
+
+ if (angular)
+ {
+ float anlge_betwwn_normals = acos(cos_between_normals);
+ incAngle(alpha_bin, beta_bin, anlge_betwwn_normals * (1-a) * (1-b));
+ incAngle(alpha_bin+1, beta_bin, anlge_betwwn_normals * a * (1-b));
+ incAngle(alpha_bin, beta_bin+1, anlge_betwwn_normals * (1-a) * b );
+ incAngle(alpha_bin+1, beta_bin+1, anlge_betwwn_normals * a * b );
+ }
+ } /* for(int i_neighb = threadIdx.x; i_neighb < neighb_count; i_neighb += CTA_SIZE) */
+
+ __syncthreads();
+
+ if (angular)
+ {
+ //transform sum to average dividing angle/spinimage element-wize.
+ const float *amgles_beg = simage_angles + FSize;
+ const float *amgles_end = amgles_beg + FSize;
+ const float *images_beg = simage_angles;
+
+ Block::transfrom(amgles_beg, amgles_end, images_beg, output.ptr(i_input), Div12eps());
+ ////Block::copy(amgles_beg, amgles_end, output.ptr(i_input));
+ //Block::copy(images_beg, images_beg + FSize, output.ptr(i_input));
+ }
+ else
+ {
+ // copy to compute sum
+ Block::copy(simage_angles, simage_angles + FSize, simage_angles + FSize);
+ __syncthreads();
+
+ //compute sum
+ Block::reduce_n(simage_angles + FSize, FSize, pcl::device::plus<float>());
+ __syncthreads();
+
+ float sum = simage_angles[FSize];
+ Block::transfrom(simage_angles, simage_angles + FSize, output.ptr(i_input), DivValIfNonZero(sum));
+ }
+ }
+
+ __device__ __forceinline__ void incSpinI(int y, int x, float value) const { atomicAdd(simage_angles + y * (2*image_width + 1) + x, value); }
+ __device__ __forceinline__ void incAngle(int y, int x, float value) const { atomicAdd(simage_angles+FSize + y * (2*image_width + 1) + x, value); }
+ };
+
+ template<typename Impl>
+ __global__ void computeSpinKernel(const Impl impl) { impl(); }
+
+ template<typename Impl>
+ inline void computeSpinImages_caller(Impl& impl, float support_angle_cos, const Indices& indices, const PointCloud& input_cloud, const Normals& input_normals,
+ const PointCloud& surface, const Normals& normals, const NeighborIndices& neighbours, int min_neighb, int image_width, float bin_size, PtrStep<float> output)
+ {
+ impl.work_size = (int)indices.size();
+ impl.indices = indices;
+ impl.input_cloud = input_cloud;
+ impl.input_normals = input_normals;
+
+ impl.surface = surface;
+ impl.normals = normals;
+
+ impl.neighbor_indices = neighbours;
+ impl.neighbor_indices_sizes = neighbours.sizes;
+
+ impl.min_neighb = min_neighb;
+ impl.image_width = image_width;
+ impl.bin_size = bin_size;
+ impl.support_angle_cos = support_angle_cos;
+ impl.FSize = Impl::computeFSize(image_width);
+
+ impl.output = output;
+
+ const int total = (int)indices.size();
+ const int max_grid_dim = 65535;
+ const int smem_size = 2 * Impl::computeFSize(image_width) * sizeof(float);
+
+ dim3 block(Impl::CTA_SIZE);
+ dim3 grid(min(total, max_grid_dim), divUp(total, max_grid_dim));
+
+ computeSpinKernel<Impl><<<grid, block, smem_size>>>(impl);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+ }
+
+ template<bool radial, bool angular>
+ void computeSpinImagesOriginNormalEx(float support_angle_cos, const Indices& indices, const PointCloud& input_cloud, const Normals& input_normals,
+ const PointCloud& surface, const Normals& normals, const NeighborIndices& neighbours,
+ int min_neighb, int image_width, float bin_size, PtrStep<float> output)
+ {
+ SpinImpl<radial, angular, UseOriginNormal> si;
+ computeSpinImages_caller(si, support_angle_cos, indices, input_cloud, input_normals, surface, normals, neighbours, min_neighb, image_width, bin_size, output);
+ }
+
+ template<bool radial, bool angular>
+ void computeSpinImagesCustomAxesEx(float support_angle_cos, const Indices& indices, const PointCloud& input_cloud, const Normals& input_normals,
+ const PointCloud& surface, const Normals& normals, const NeighborIndices& neighbours,
+ int min_neighb, int image_width, float bin_size, const float3& rotation_axis, PtrStep<float> output)
+ {
+ SpinImpl<radial, angular, UseCustomAxis> si;
+ si.rotation_axis = rotation_axis;
+ computeSpinImages_caller(si, support_angle_cos, indices, input_cloud, input_normals, surface, normals, neighbours, min_neighb, image_width, bin_size, output);
+ }
+
+ template<bool radial, bool angular>
+ void computeSpinImagesCustomAxesCloudEx(float support_angle_cos, const Indices& indices, const PointCloud& input_cloud, const Normals& input_normals,
+ const PointCloud& surface, const Normals& normals, const NeighborIndices& neighbours,
+ int min_neighb, int image_width, float bin_size, const Normals& rotation_axes_cloud, PtrStep<float> output)
+ {
+
+ SpinImpl<radial, angular, UseCustomAxesCloud> si;
+ si.rotation_axes_cloud = rotation_axes_cloud;
+ computeSpinImages_caller(si, support_angle_cos, indices, input_cloud, input_normals, surface, normals, neighbours, min_neighb, image_width, bin_size, output);
+ }
+ }
+}
+
+void pcl::device::computeSpinImagesOrigigNormal(bool radial, bool angular, float support_angle_cos, const Indices& indices, const PointCloud& input_cloud, const Normals& input_normals,
+ const PointCloud& surface, const Normals& normals, const NeighborIndices& neighbours, int min_neighb, int image_width, float bin_size, PtrStep<float> output)
+{
+ typedef void (*originNormal)(float, const Indices&, const PointCloud&, const Normals&, const PointCloud&, const Normals&, const NeighborIndices&, int , int , float, PtrStep<float>);
+
+ const originNormal table[2][2] =
+ {
+ { computeSpinImagesOriginNormalEx<false, false>, computeSpinImagesOriginNormalEx<false, true> },
+ { computeSpinImagesOriginNormalEx<true, false>, computeSpinImagesOriginNormalEx<true, true> }
+ };
+
+ table[(int)radial][(int)angular](support_angle_cos, indices, input_cloud, input_normals, surface, normals, neighbours, min_neighb, image_width, bin_size, output);
+}
+
+void pcl::device::computeSpinImagesCustomAxes(bool radial, bool angular, float support_angle_cos, const Indices& indices, const PointCloud& input_cloud, const Normals& input_normals,
+ const PointCloud& surface, const Normals& normals, const NeighborIndices& neighbours, int min_neighb, int image_width, float bin_size, const float3& rotation_axis, PtrStep<float> output)
+{
+ typedef void (*customAxes)(float, const Indices&, const PointCloud&, const Normals&, const PointCloud&, const Normals&, const NeighborIndices&, int, int, float, const float3&, PtrStep<float>);
+
+ const customAxes table[2][2] =
+ {
+ { computeSpinImagesCustomAxesEx<false, false>, computeSpinImagesCustomAxesEx<false, true> },
+ { computeSpinImagesCustomAxesEx<true, false>, computeSpinImagesCustomAxesEx<true, true> }
+ };
+
+ table[(int)radial][(int)angular](support_angle_cos, indices, input_cloud, input_normals, surface, normals, neighbours, min_neighb, image_width, bin_size, rotation_axis, output);
+}
+
+
+void pcl::device::computeSpinImagesCustomAxesCloud(bool radial, bool angular, float support_angle_cos, const Indices& indices, const PointCloud& input_cloud, const Normals& input_normals,
+ const PointCloud& surface, const Normals& normals, const NeighborIndices& neighbours, int min_neighb, int image_width, float bin_size, const Normals& rotation_axes_cloud, PtrStep<float> output)
+{
+ typedef void (*customAxesCloud)(float, const Indices&, const PointCloud&, const Normals&, const PointCloud&, const Normals&, const NeighborIndices&, int, int, float, const Normals&, PtrStep<float>);
+
+ const customAxesCloud table[2][2] =
+ {
+ { computeSpinImagesCustomAxesCloudEx<false, false>, computeSpinImagesCustomAxesCloudEx<false, true> },
+ { computeSpinImagesCustomAxesCloudEx<true, false>, computeSpinImagesCustomAxesCloudEx<true, true> }
+ };
+
+ table[(int)radial][(int)angular](support_angle_cos, indices, input_cloud, input_normals, surface, normals, neighbours, min_neighb, image_width, bin_size, rotation_axes_cloud, output);
+};
+
+namespace pcl
+{
+ namespace device
+ {
+ struct GtThan
+ {
+ int val;
+ GtThan(int value) : val(value) {}
+ __device__ __forceinline__ unsigned char operator()(int size) const { return size > val ? 1 : 0; }
+ };
+ }
+}
+
+void pcl::device::computeMask(const NeighborIndices& neighbours, int min_neighb, DeviceArray<unsigned char>& mask)
+{
+ thrust::device_ptr<int> beg((int*)neighbours.sizes.ptr());
+ thrust::device_ptr<int> end = beg + neighbours.sizes.size();
+ thrust::device_ptr<unsigned char> out(mask.ptr());
+
+ thrust::transform(beg, end, out, GtThan(min_neighb));
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include "internal.hpp"
+#include "pcl/gpu/utils/safe_call.hpp"
+#include "pcl/gpu/utils/device/warp.hpp"
+
+#include <thrust/scan.h>
+#include <thrust/device_ptr.h>
+#include <thrust/device_vector.h>
+#include <thrust/sort.h>
+#include <thrust/unique.h>
+
+namespace pcl
+{
+ namespace device
+ {
+ __device__ int total_after_repack;
+
+ struct IndsRepack
+ {
+ enum
+ {
+ CTA_SIZE = 256,
+ WARPS = CTA_SIZE/Warp::WARP_SIZE
+ };
+
+ const int *offsets;
+ PtrStep<int> nindices;
+ const int *sizes;
+ int work_size;
+
+ mutable int *output;
+
+ __device__ void operator()() const
+ {
+ int idx = WARPS * blockIdx.x + Warp::id();
+
+ if (idx >= work_size)
+ return;
+
+ int size = sizes[idx];
+ const int *ninds_beg = nindices.ptr(idx);
+ const int *ninds_end = ninds_beg + size;
+ const int before = offsets[idx];
+
+ Warp::copy(ninds_beg, ninds_end, output + before);
+
+ if (idx == work_size - 1 && Warp::laneId() == 0)
+ total_after_repack = before + size;
+ }
+ };
+
+ __global__ void IndsRepackKernel(const IndsRepack irpk) { irpk(); }
+
+ __global__ void createLookupKernel(const int* inds, int total, int* output)
+ {
+ int idx = threadIdx.x + blockIdx.x * blockDim.x;
+
+ if (idx < total)
+ output[inds[idx]] = idx;
+ }
+ }
+
+}
+
+
+int pcl::device::computeUniqueIndices(size_t surface_size, const NeighborIndices& neighbours, DeviceArray<int>& unique_indices, DeviceArray<int>& lookup)
+{
+ unique_indices.create(neighbours.data.size());
+ lookup.create(surface_size);
+
+ thrust::device_vector<int> scan(neighbours.sizes.size());
+ thrust::device_ptr<int> beg((int*)neighbours.sizes.ptr());
+ thrust::device_ptr<int> end = beg + neighbours.sizes.size();
+ thrust::exclusive_scan(beg, end, scan.begin());
+
+ IndsRepack irpk;
+ irpk.offsets = raw_pointer_cast(&scan[0]);
+ irpk.sizes = neighbours.sizes;
+ irpk.nindices = neighbours;
+ irpk.output = unique_indices;
+ irpk.work_size = (int)neighbours.sizes.size();
+
+ int block = IndsRepack::CTA_SIZE;
+ int grid = divUp((int)neighbours.sizes.size(), IndsRepack::WARPS);
+
+ IndsRepackKernel<<<grid, block>>>(irpk);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall(cudaDeviceSynchronize());
+
+ int total;
+ cudaSafeCall( cudaMemcpyFromSymbol(&total, total_after_repack, sizeof(total)) );
+ cudaSafeCall(cudaDeviceSynchronize());
+
+ thrust::device_ptr<int> begu(unique_indices.ptr());
+ thrust::device_ptr<int> endu = begu + total;
+
+ thrust::sort(begu, endu);
+ total = (int)(thrust::unique(begu, endu) - begu);
+
+ thrust::device_ptr<int> begl(lookup.ptr());
+ thrust::device_ptr<int> endl = begl + lookup.size();
+ thrust::fill(begl, endl, 0);
+
+ createLookupKernel<<<divUp((int)unique_indices.size(), 256), 256>>>(unique_indices, total, lookup);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall(cudaDeviceSynchronize());
+
+ return total;
+}
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#ifndef PCL_GPU_FEATURS_DEVICE_VECTOR_OPERATIONS_HPP_
+#define PCL_GPU_FEATURS_DEVICE_VECTOR_OPERATIONS_HPP_
+
+#include <pcl/gpu/features/device/rodrigues.hpp>
+#include <pcl/gpu/utils/device/vector_math.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+ __forceinline__ __device__ __host__ float3 operator/(const float3& vec, float val)
+ {
+ return make_float3(vec.x/val, vec.y/val, vec.z/val);
+ }
+
+ __device__ __host__ __forceinline__ float3& operator/=(float3& v, const float& value)
+ {
+ v.x /= value;
+ v.y /= value;
+ v.z /= value;
+ return v;
+ }
+
+ __device__ __host__ __forceinline__ float norm(const float3& v1, const float3& v2)
+ {
+ float dx = v1.x - v2.x;
+ float dy = v1.y - v2.y;
+ float dz = v1.z - v2.z;
+ return sqrtf(dx*dx + dy*dy + dz*dz);
+ }
+
+ template<typename T> __device__ __forceinline__ float3 tr(const T& v)
+ {
+ return make_float3(v.x, v.y, v.z);
+ }
+
+ inline __host__ __device__ float3 normalize(const float3& v)
+ {
+ return v * inverse_norm(v);
+ }
+ }
+}
+
+#endif /* PCL_GPU_FEATURS_DEVICE_VECTOR_OPERATIONS_HPP_ */
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include "internal.hpp"
+#include "pcl/gpu/utils/device/funcattrib.hpp"
+#include "pcl/gpu/utils/device/warp.hpp"
+#include "pcl/gpu/utils/device/block.hpp"
+#include "pcl/gpu/utils/safe_call.hpp"
+
+#include "pcl/gpu/features/device/pair_features.hpp"
+
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846f
+#endif
+
+
+namespace pcl
+{
+ namespace device
+ {
+ __device__ unsigned int count = 0;
+
+ struct VfhDevice
+ {
+ enum
+ {
+ CTA_SIZE = 256,
+
+ bins1 = 45,
+ bins2 = 45,
+ bins3 = 45,
+ bins4 = 45,
+ bins_vp = 128,
+
+ FSize = bins1 + bins2 + bins3 + bins4 + bins_vp
+ };
+
+ float distance_normalization_factor_inv;
+ float hist_incr; // Factorization constant
+ float hist_incr_size_component;
+
+ bool normalize_distances;
+
+ float3 centroid_p;
+ float3 centroid_n;
+
+ float3 d_vp_p;
+
+
+ float hist_incr_vp;
+
+ PtrSz<int> indices;
+
+ const PointType *points;
+ const NormalType *normals;
+
+ mutable PtrStep<float> global_buffer;
+ mutable float* output;
+
+ template<typename T> __device__ __forceinline__ float3 fetch(const T* data, int index) const
+ {
+ T t = data[index];
+ return make_float3(t.x, t.y, t.z);
+ }
+
+ __device__ __forceinline__ void compute(float *shist_b1, float *shist_b2, float *shist_b3, float *shist_b4, float *shist_vp) const
+ {
+ int idx = threadIdx.x + blockIdx.x * CTA_SIZE;
+
+ if (idx > indices.size)
+ return;
+ }
+
+ __device__ void operator()() const
+ {
+ __shared__ float shist[FSize];
+ __shared__ bool lastBlock;
+
+ Block::fill(shist, shist + FSize, 0.f);
+ __syncthreads();
+
+ float *const shist_b1 = shist;
+ float *const shist_b2 = shist_b1 + bins1;
+ float *const shist_b3 = shist_b2 + bins2;
+ float *const shist_b4 = shist_b3 + bins3;
+ float *const shist_vp = shist_b4 + bins4;
+
+ int STRIDE = gridDim.x * CTA_SIZE;
+ int idx = blockIdx.x * CTA_SIZE + threadIdx.x;
+
+ for (; idx < indices.size; idx += STRIDE)
+ {
+ int index = indices.data[idx];
+
+ float3 p = fetch(points, index);
+ float3 n = fetch(normals, index);
+
+ // spfh
+ int h_index;
+ float f1, f2, f3, f4;
+
+ if (computePairFeatures(centroid_p, centroid_n, p, n, f1, f2, f3, f4))
+ {
+ // Normalize the f1, f2, f3, f4 features and push them in the histogram
+ h_index = floor (bins1 * ((f1 + M_PI) * (1.f / (2.f * M_PI))));
+ h_index = min(bins1 - 1, max(0, h_index));
+ atomicAdd(shist_b1 + h_index, hist_incr);
+
+ h_index = floor (bins2 * ((f2 + 1.f) * 0.5f));
+ h_index = min(bins2 - 1, max (0, h_index));
+ atomicAdd(shist_b2 + h_index, hist_incr);
+
+ h_index = floor (bins3 * ((f3 + 1.f) * 0.5f));
+ h_index = min(bins3 - 1, max (0, h_index));
+ atomicAdd(shist_b3 + h_index, hist_incr);
+
+ if (normalize_distances)
+ h_index = floor (bins4 * (f4 * distance_normalization_factor_inv));
+ else
+ h_index = __float2int_rn (f4 * 100);
+
+ h_index = min(bins4 - 1, max (0, h_index));
+ atomicAdd(shist_b4 + h_index, hist_incr_size_component);
+ }
+
+ // viewpoint component
+ float alfa = ((dot(n, d_vp_p) + 1.f) * 0.5f);
+ h_index = floor (bins_vp * alfa);
+ h_index = min(bins_vp - 1, max (0, h_index));
+ atomicAdd(shist_vp + h_index, hist_incr_vp);
+
+ } /* for (; idx < indices.size; idx += STRIDE) */
+
+ __syncthreads();
+
+ Block::copy(shist, shist + FSize, global_buffer.ptr(blockIdx.x));
+
+ __threadfence();
+
+ if (threadIdx.x == 0)
+ {
+ unsigned int value = atomicInc(&count, gridDim.x);
+ lastBlock = (value == (gridDim.x - 1));
+ }
+ __syncthreads();
+
+ if (lastBlock)
+ {
+ int total_rows = gridDim.x;
+
+ for(int x = threadIdx.x; x < FSize; x += CTA_SIZE)
+ {
+ float sum = 0.f;
+ for(int y = 0; y < total_rows; ++y)
+ sum += global_buffer.ptr(y)[x];
+
+ output[x] = sum;
+ }
+
+ if (threadIdx.x == 0)
+ count = 0;
+ }
+ }
+ };
+
+ __global__ void estimateVfhKernel(const VfhDevice vfh) { vfh(); }
+ }
+}
+
+
+void pcl::device::VFHEstimationImpl::compute(DeviceArray<VFHSignature308>& feature)
+{
+ feature.create(1);
+ VfhDevice vfh;
+
+ vfh.centroid_p = xyz_centroid;
+ vfh.centroid_n = normal_centroid;
+ vfh.indices = indices;
+ vfh.points = points;
+ vfh.normals = normals;
+ vfh.normalize_distances = normalize_distances;
+
+ // Compute the direction of view from the viewpoint to the centroid
+ vfh.d_vp_p = normalized (viewpoint - xyz_centroid);
+
+ vfh.distance_normalization_factor_inv = 1.f;
+ if ( normalize_distances )
+ {
+ float3 max_pt = getMaxDistance (points, indices, xyz_centroid);
+ vfh.distance_normalization_factor_inv = 1.f / norm(xyz_centroid - max_pt);
+ }
+
+ // Factorization constant
+ vfh.hist_incr = normalize_bins ? (100.f / (indices.size() - 1)) : 1.f;
+ vfh.hist_incr_size_component = size_component ? vfh.hist_incr : 0.f;
+
+ vfh.hist_incr_vp = normalize_bins ? (100.f / indices.size()) : 1.f;
+
+
+ int device;
+ cudaSafeCall( cudaGetDevice(&device) );
+
+ cudaDeviceProp prop;
+ cudaSafeCall( cudaGetDeviceProperties(&prop, device) );
+
+ int total = static_cast<int> (indices.empty() ? points.size() : indices.size());
+ int total_lenght_in_blocks = (total + VfhDevice::CTA_SIZE - 1) / VfhDevice::CTA_SIZE;
+
+ int block = VfhDevice::CTA_SIZE;
+ int grid = min(total_lenght_in_blocks, prop.multiProcessorCount * prop.maxThreadsPerMultiProcessor / VfhDevice::CTA_SIZE);
+
+ DeviceArray2D<float> global_buffer(grid, VfhDevice::FSize);
+
+ vfh.global_buffer = global_buffer;
+ vfh.output = (float*)feature.ptr();
+
+ estimateVfhKernel<<<grid, block>>>(vfh);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
+
--- /dev/null
+if(BUILD_TESTS)
+
+ include_directories("${PCL_SOURCE_DIR}/test/gtest-1.6.0/include"
+ "${PCL_SOURCE_DIR}/test/gtest-1.6.0")
+
+ set(pcl_gtest_sources "${PCL_SOURCE_DIR}/test/gtest-1.6.0/src/gtest-all.cc")
+
+ set(the_test_target test_gpu_features)
+
+ get_filename_component(DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
+ get_filename_component(INC1 "${DIR}/../../../io/include" REALPATH)
+ get_filename_component(INC2 "${DIR}/../../../features/include" REALPATH)
+ get_filename_component(INC3 "${DIR}/../../../kdtree/include" REALPATH)
+ get_filename_component(INC4 "${DIR}/../../../visualization/include" REALPATH)
+ get_filename_component(INC5 "${DIR}/../../../common/include" REALPATH)
+ get_filename_component(INC6 "${DIR}/../../../search/include" REALPATH)
+ get_filename_component(INC7 "${DIR}/../../../octree/include" REALPATH)
+
+
+ #find_library(VTK)
+
+ #set(INC6 "d:/svn/pcl_3rdparty/vtk-5.6.1/Common/" "d:/svn/pcl_3rdparty/vtk-5.6.1/bin/" "d:/svn/pcl_3rdparty/vtk-5.6.1/Rendering/")
+
+ FILE(GLOB test_src *.cpp *.hpp)
+ list(APPEND test_src ${pcl_gtest_sources})
+ include_directories("${INC1}" "${INC2}" "${INC3}" "${INC4}" "${INC5}" "${INC6}" "${INC7}" ${VTK_INCLUDE_DIRS})
+ #PCL_ADD_TEST(a_gpu_features_test ${the_test_target} FILES ${test_src} LINK_WITH pcl_gpu_containers pcl_gpu_octree pcl_gpu_features pcl_io pcl_features pcl_visualization pcl_common)
+
+ #add_dependencies(${the_test_target} pcl_gpu_octree pcl_gpu_containes pcl_gpu_features pcl_io pcl_features pcl_visualization pcl_common)
+endif(BUILD_TESTS)
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+
+#ifndef PCL_GPU_FEATURES_TEST_DATA_SORUCE_HPP_
+#define PCL_GPU_FEATURES_TEST_DATA_SORUCE_HPP_
+
+#include<string>
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/common/common.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/gpu/containers/kernel_containers.h>
+#include <pcl/search/search.h>
+
+#include <Eigen/StdVector>
+
+#if defined (_WIN32) || defined(_WIN64)
+ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(pcl::PointXYZ)
+ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(pcl::Normal)
+#endif
+
+#include <algorithm>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ struct DataSource
+ {
+ const static int k = 32;
+ const static int max_elements = 500;
+
+ PointCloud<PointXYZ>::Ptr cloud;
+ PointCloud<PointXYZ>::Ptr surface;
+ IndicesPtr indices;
+
+ PointCloud<Normal>::Ptr normals;
+ PointCloud<Normal>::Ptr normals_surface;
+ float radius;
+
+ std::vector< std::vector<int> > neighbors_all;
+ std::vector<int> sizes;
+ int max_nn_size;
+
+ DataSource(const std::string& file = "d:/office_chair_model.pcd")
+ : cloud(new PointCloud<PointXYZ>()), surface(new PointCloud<PointXYZ>()), indices( new std::vector<int>() ),
+ normals(new PointCloud<Normal>()), normals_surface(new PointCloud<Normal>())
+ {
+ PCDReader pcd;
+ pcd.read(file, *cloud);
+
+ PointXYZ minp, maxp;
+ pcl::getMinMax3D(*cloud, minp, maxp);
+ float sz = (maxp.x - minp.x + maxp.y - minp.y + maxp.z - minp.z) / 3;
+ radius = sz / 15;
+ }
+
+ void generateColor()
+ {
+ size_t cloud_size = cloud->points.size();
+ for(size_t i = 0; i < cloud_size; ++i)
+ {
+ PointXYZ& p = cloud->points[i];
+
+ int r = std::max(1, std::min(255, static_cast<int>((double(rand())/RAND_MAX)*255)));
+ int g = std::max(1, std::min(255, static_cast<int>((double(rand())/RAND_MAX)*255)));
+ int b = std::max(1, std::min(255, static_cast<int>((double(rand())/RAND_MAX)*255)));
+
+ *reinterpret_cast<int*>(&p.data[3]) = (b << 16) + (g << 8) + r;
+ }
+ }
+
+ void estimateNormals()
+ {
+ pcl::NormalEstimation<PointXYZ, Normal> ne;
+ ne.setInputCloud (cloud);
+ ne.setSearchMethod (pcl::search::KdTree<PointXYZ>::Ptr (new pcl::search::KdTree<PointXYZ>));
+ ne.setKSearch (k);
+ //ne.setRadiusSearch (radius);
+
+ ne.compute (*normals);
+ }
+
+ void runCloudViewer() const
+ {
+ pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
+ viewer.showCloud (cloud);
+ while (!viewer.wasStopped ()) {}
+ }
+
+ void findKNNeghbors()
+ {
+ KdTreeFLANN<PointXYZ>::Ptr kdtree(new KdTreeFLANN<PointXYZ>);
+ kdtree->setInputCloud(cloud);
+
+ size_t cloud_size = cloud->points.size();
+
+ std::vector<float> dists;
+ neighbors_all.resize(cloud_size);
+ for(size_t i = 0; i < cloud_size; ++i)
+ {
+ kdtree->nearestKSearch(cloud->points[i], k, neighbors_all[i], dists);
+ sizes.push_back((int)neighbors_all[i].size());
+ }
+ max_nn_size = *max_element(sizes.begin(), sizes.end());
+ }
+
+ void findRadiusNeghbors(float radius = -1)
+ {
+ radius = radius == -1 ? this->radius : radius;
+
+ KdTreeFLANN<PointXYZ>::Ptr kdtree(new KdTreeFLANN<PointXYZ>);
+ kdtree->setInputCloud(cloud);
+
+ size_t cloud_size = cloud->points.size();
+
+ std::vector<float> dists;
+ neighbors_all.resize(cloud_size);
+ for(size_t i = 0; i < cloud_size; ++i)
+ {
+ kdtree->radiusSearch(cloud->points[i], radius, neighbors_all[i], dists);
+ sizes.push_back((int)neighbors_all[i].size());
+ }
+ max_nn_size = *max_element(sizes.begin(), sizes.end());
+ }
+
+ void getNeghborsArray(std::vector<int>& data)
+ {
+ data.resize(max_nn_size * neighbors_all.size());
+ pcl::gpu::PtrStep<int> ps(&data[0], max_nn_size * sizeof(int));
+ for(size_t i = 0; i < neighbors_all.size(); ++i)
+ copy(neighbors_all[i].begin(), neighbors_all[i].end(), ps.ptr(i));
+ }
+
+ void generateSurface()
+ {
+ surface->points.clear();
+ for(size_t i = 0; i < cloud->points.size(); i+= 10)
+ surface->points.push_back(cloud->points[i]);
+ surface->width = surface->points.size();
+ surface->height = 1;
+
+ if (!normals->points.empty())
+ {
+ normals_surface->points.clear();
+ for(size_t i = 0; i < normals->points.size(); i+= 10)
+ normals_surface->points.push_back(normals->points[i]);
+
+ normals_surface->width = surface->points.size();
+ normals_surface->height = 1;
+ }
+ }
+
+ void generateIndices(size_t step = 100)
+ {
+ indices->clear();
+ for(size_t i = 0; i < cloud->points.size(); i += step)
+ indices->push_back(i);
+ }
+
+ struct Normal2PointXYZ
+ {
+ PointXYZ operator()(const Normal& n) const
+ {
+ PointXYZ xyz;
+ xyz.x = n.normal[0];
+ xyz.y = n.normal[1];
+ xyz.z = n.normal[2];
+ return xyz;
+ }
+ };
+ };
+ }
+}
+
+#endif /* PCL_GPU_FEATURES_TEST_DATA_SORUCE_HPP_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#if (defined(__GNUC__) && !defined(__CUDACC__) && (GTEST_GCC_VER_ >= 40000))
+ #define GTEST_USE_OWN_TR1_TUPLE 0
+#endif
+
+#if defined(_MSC_VER) && (_MSC_VER >= 1500)
+ #define GTEST_USE_OWN_TR1_TUPLE 0
+#endif
+
+#include "gtest/gtest.h"
+
+#include <pcl/point_types.h>
+#include <pcl/features/fpfh.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/common/common.h>
+#include <pcl/gpu/features/features.hpp>
+#include "data_source.hpp"
+#include <pcl/search/pcl_search.h>
+
+using namespace std;
+using namespace pcl;
+using namespace pcl::gpu;
+
+//TEST(PCL_FeaturesGPU, DISABLED_fpfh_low_level)
+TEST(PCL_FeaturesGPU, fpfh_low_level)
+{
+ DataSource source;
+
+ source.estimateNormals();
+ source.findRadiusNeghbors();
+ cout << "max_radius_nn_size: " << source.max_nn_size << endl;
+
+ vector<int> data;
+ source.getNeghborsArray(data);
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(source.normals->points.begin(), source.normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+ pcl::gpu::FPFHEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::FPFHEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ pcl::gpu::NeighborIndices indices;
+ indices.upload(data, source.sizes, source.max_nn_size);
+
+ DeviceArray2D<FPFHSignature33> fpfh33_features;
+
+ gpu::FPFHEstimation fpfh_gpu;
+ fpfh_gpu.compute(cloud_gpu, normals_gpu, indices, fpfh33_features);
+
+ int stub;
+ vector<FPFHSignature33> downloaded;
+ fpfh33_features.download(downloaded, stub);
+
+ pcl::FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fe;
+ fe.setInputCloud (source.cloud);
+ fe.setInputNormals (source.normals);
+ fe.setSearchMethod (pcl::search::KdTree<PointXYZ>::Ptr (new pcl::search::KdTree<PointXYZ>));
+ //fe.setKSearch (k);
+ fe.setRadiusSearch (source.radius);
+
+ PointCloud<FPFHSignature33> fpfhs;
+ fe.compute (fpfhs);
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ FPFHSignature33& gpu = downloaded[i];
+ FPFHSignature33& cpu = fpfhs.points[i];
+
+ size_t FSize = sizeof(FPFHSignature33)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+ ASSERT_EQ(norm_diff/norm < 0.01f/FSize, true);
+ }
+}
+
+//TEST(PCL_FeaturesGPU, DISABLED_fpfh_high_level1)
+TEST(PCL_FeaturesGPU, fpfh_high_level1)
+{
+ DataSource source;
+ source.estimateNormals();
+
+ //source.generateSurface();
+ //source.generateIndices();
+ cout << "!indices, !surface" << endl;
+
+ PointCloud<Normal>::Ptr& normals = source.normals;
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(normals->points.begin(), normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+ pcl::gpu::FPFHEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::FPFHEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ //pcl::gpu::FPFHEstimation::Indices indices_gpu;
+ //indices_gpu.upload(*source.indices);
+
+ //pcl::gpu::FPFHEstimation::PointCloud surface_gpu;
+ //surface_gpu.upload(source.surface->points);
+
+ //GPU call
+ pcl::gpu::FPFHEstimation fe_gpu;
+ fe_gpu.setInputCloud (cloud_gpu);
+ fe_gpu.setInputNormals (normals_gpu);
+ fe_gpu.setRadiusSearch (source.radius, source.max_elements);
+ //fe_gpu.setIndices(indices_gpu);
+ //fe_gpu.setSearchSurface(surface_gpu);
+
+ DeviceArray2D<FPFHSignature33> fpfhs_gpu;
+ fe_gpu.compute(fpfhs_gpu);
+
+ // CPU call
+ pcl::FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fe;
+ fe.setInputCloud (source.cloud);
+ fe.setInputNormals (normals);
+ fe.setSearchMethod (pcl::search::KdTree<PointXYZ>::Ptr (new pcl::search::KdTree<PointXYZ>));
+ //fe.setKSearch (k);
+ fe.setRadiusSearch (source.radius);
+ //fe.setIndices(source.indices);
+ //fe.setSearchSurface(source.surface);
+
+ PointCloud<FPFHSignature33> fpfhs;
+ fe.compute (fpfhs);
+
+ int stub;
+ vector<FPFHSignature33> downloaded;
+ fpfhs_gpu.download(downloaded, stub);
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ FPFHSignature33& gpu = downloaded[i];
+ FPFHSignature33& cpu = fpfhs.points[i];
+
+ size_t FSize = sizeof(FPFHSignature33)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+
+ if (norm != 0)
+ ASSERT_EQ(norm_diff/norm < 0.01f/FSize, true);
+ }
+}
+
+
+//TEST(PCL_FeaturesGPU, DISABLED_fpfh_high_level2)
+TEST(PCL_FeaturesGPU, fpfh_high_level2)
+{
+ DataSource source;
+ source.estimateNormals();
+
+ //source.generateSurface();
+ source.generateIndices();
+ cout << "indices, !surface" << endl;
+
+ PointCloud<Normal>::Ptr& normals = source.normals;
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(normals->points.begin(), normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+ pcl::gpu::FPFHEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::FPFHEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ pcl::gpu::FPFHEstimation::Indices indices_gpu;
+ indices_gpu.upload(*source.indices);
+
+ //pcl::gpu::FPFHEstimation::PointCloud surface_gpu;
+ //surface_gpu.upload(source.surface->points);
+
+ //GPU call
+ pcl::gpu::FPFHEstimation fe_gpu;
+ fe_gpu.setInputCloud (cloud_gpu);
+ fe_gpu.setInputNormals (normals_gpu);
+ fe_gpu.setRadiusSearch (source.radius, source.max_elements);
+ fe_gpu.setIndices(indices_gpu);
+ //fe_gpu.setSearchSurface(surface_gpu);
+
+ DeviceArray2D<FPFHSignature33> fpfhs_gpu;
+ fe_gpu.compute(fpfhs_gpu);
+
+ // CPU call
+ pcl::FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fe;
+ fe.setInputCloud (source.cloud);
+ fe.setInputNormals (normals);
+ fe.setSearchMethod (pcl::search::KdTree<PointXYZ>::Ptr (new pcl::search::KdTree<PointXYZ>));
+ //fe.setKSearch (k);
+ fe.setRadiusSearch (source.radius);
+ fe.setIndices(source.indices);
+ //fe.setSearchSurface(source.surface);
+
+ PointCloud<FPFHSignature33> fpfhs;
+ fe.compute (fpfhs);
+
+ int stub;
+ vector<FPFHSignature33> downloaded;
+ fpfhs_gpu.download(downloaded, stub);
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ FPFHSignature33& gpu = downloaded[i];
+ FPFHSignature33& cpu = fpfhs.points[i];
+
+ size_t FSize = sizeof(FPFHSignature33)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+ //cout << i << "->"<< norm_diff/norm << endl;
+ if (norm != 0)
+ ASSERT_EQ(norm_diff/norm < 0.01f/FSize, true);
+ }
+}
+
+//TEST(PCL_FeaturesGPU, DISABLED_fpfh_high_level3)
+TEST(PCL_FeaturesGPU, fpfh_high_level3)
+{
+ DataSource source;
+ source.estimateNormals();
+
+ source.generateSurface();
+ //source.generateIndices();
+ cout << "!indices, surface" << endl;
+
+ PointCloud<Normal>::Ptr& normals = source.normals_surface;
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(normals->points.begin(), normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+ pcl::gpu::FPFHEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::FPFHEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ //pcl::gpu::FPFHEstimation::Indices indices_gpu;
+ //indices_gpu.upload(*source.indices);
+
+ pcl::gpu::FPFHEstimation::PointCloud surface_gpu;
+ surface_gpu.upload(source.surface->points);
+
+ //GPU call
+ pcl::gpu::FPFHEstimation fe_gpu;
+ fe_gpu.setInputCloud (cloud_gpu);
+ fe_gpu.setInputNormals (normals_gpu);
+ fe_gpu.setRadiusSearch (source.radius, source.max_elements);
+ //fe_gpu.setIndices(indices_gpu);
+ fe_gpu.setSearchSurface(surface_gpu);
+
+ DeviceArray2D<FPFHSignature33> fpfhs_gpu;
+ fe_gpu.compute(fpfhs_gpu);
+
+ // CPU call
+ pcl::FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fe;
+ fe.setInputCloud (source.cloud);
+ fe.setInputNormals (normals);
+ fe.setSearchMethod (pcl::search::KdTree<PointXYZ>::Ptr (new pcl::search::KdTree<PointXYZ>));
+ //fe.setKSearch (k);
+ fe.setRadiusSearch (source.radius);
+ //fe.setIndices(source.indices);
+ fe.setSearchSurface(source.surface);
+
+ PointCloud<FPFHSignature33> fpfhs;
+ fe.compute (fpfhs);
+
+ int stub;
+ vector<FPFHSignature33> downloaded;
+ fpfhs_gpu.download(downloaded, stub);
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ FPFHSignature33& gpu = downloaded[i];
+ FPFHSignature33& cpu = fpfhs.points[i];
+
+ size_t FSize = sizeof(FPFHSignature33)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+ //cout << i << "->"<< norm_diff/norm << endl;
+ if (norm != 0)
+ ASSERT_EQ(norm_diff/norm < 0.01f/FSize, true);
+ }
+}
+
+
+//TEST(PCL_FeaturesGPU, DISABLED_fpfh_high_level4)
+TEST(PCL_FeaturesGPU, fpfh_high_level4)
+{
+ DataSource source;
+ source.estimateNormals();
+
+ source.generateSurface();
+ source.generateIndices();
+ cout << "indices, surface" << endl;
+
+ PointCloud<Normal>::Ptr& normals = source.normals_surface;
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(normals->points.begin(), normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+ pcl::gpu::FPFHEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::FPFHEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ pcl::gpu::FPFHEstimation::Indices indices_gpu;
+ indices_gpu.upload(*source.indices);
+
+ pcl::gpu::FPFHEstimation::PointCloud surface_gpu;
+ surface_gpu.upload(source.surface->points);
+
+ //GPU call
+ pcl::gpu::FPFHEstimation fe_gpu;
+ fe_gpu.setInputCloud (cloud_gpu);
+ fe_gpu.setInputNormals (normals_gpu);
+ fe_gpu.setRadiusSearch (source.radius, source.max_elements);
+ fe_gpu.setIndices(indices_gpu);
+ fe_gpu.setSearchSurface(surface_gpu);
+
+ DeviceArray2D<FPFHSignature33> fpfhs_gpu;
+ fe_gpu.compute(fpfhs_gpu);
+
+ // CPU call
+ pcl::FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fe;
+ fe.setInputCloud (source.cloud);
+ fe.setInputNormals (normals);
+ fe.setSearchMethod (pcl::search::KdTree<PointXYZ>::Ptr (new pcl::search::KdTree<PointXYZ>));
+ //fe.setKSearch (k);
+ fe.setRadiusSearch (source.radius);
+ fe.setIndices(source.indices);
+ fe.setSearchSurface(source.surface);
+
+ PointCloud<FPFHSignature33> fpfhs;
+ fe.compute (fpfhs);
+
+ int stub;
+ vector<FPFHSignature33> downloaded;
+ fpfhs_gpu.download(downloaded, stub);
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ FPFHSignature33& gpu = downloaded[i];
+ FPFHSignature33& cpu = fpfhs.points[i];
+
+ size_t FSize = sizeof(FPFHSignature33)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+ if (norm != 0)
+ ASSERT_EQ(norm_diff/norm < 0.01f/FSize, true);
+ }
+}
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#if (defined(__GNUC__) && !defined(__CUDACC__) && (GTEST_GCC_VER_ >= 40000))
+ #define GTEST_USE_OWN_TR1_TUPLE 0
+#endif
+
+#if defined(_MSC_VER) && (_MSC_VER >= 1500)
+ #define GTEST_USE_OWN_TR1_TUPLE 0
+#endif
+
+#include <gtest/gtest.h>
+#include "data_source.hpp"
+#include <iostream>
+
+#include <pcl/gpu/features/features.hpp>
+#include <pcl/gpu/containers/initialization.h>
+#include <pcl/search/search.h>
+
+using namespace std;
+using namespace pcl;
+using namespace pcl::gpu;
+
+//TEST(PCL_FeaturesGPU, DISABLED_normals_lowlevel)
+TEST(PCL_FeaturesGPU, normals_lowlevel)
+{
+ DataSource source;
+ cout << "Cloud size: " << source.cloud->points.size() << endl;
+ cout << "Radius: " << source.radius << endl;
+ cout << "K: " << source.k << endl;
+
+ //source.runCloudViewer();
+
+ source.estimateNormals();
+ source.findKNNeghbors();
+
+ gpu::NormalEstimation::PointCloud cloud;
+ cloud.upload(source.cloud->points);
+
+ // convert to single array format
+ vector<int> neighbors_all(source.max_nn_size * cloud.size());
+ PtrStep<int> ps(&neighbors_all[0], source.max_nn_size * PtrStep<int>::elem_size);
+ for(size_t i = 0; i < cloud.size(); ++i)
+ copy(source.neighbors_all[i].begin(), source.neighbors_all[i].end(), ps.ptr(i));
+
+ NeighborIndices indices;
+ indices.upload(neighbors_all, source.sizes, source.max_nn_size);
+
+ gpu::NormalEstimation::Normals normals;
+ gpu::NormalEstimation::computeNormals(cloud, indices, normals);
+ gpu::NormalEstimation::flipNormalTowardsViewpoint(cloud, 0.f, 0.f, 0.f, normals);
+
+ vector<PointXYZ> downloaded;
+ normals.download(downloaded);
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ Normal n = source.normals->points[i];
+
+ PointXYZ xyz = downloaded[i];
+ float curvature = xyz.data[3];
+
+ float abs_error = 0.01f;
+ ASSERT_NEAR(n.normal_x, xyz.x, abs_error);
+ ASSERT_NEAR(n.normal_y, xyz.y, abs_error);
+ ASSERT_NEAR(n.normal_z, xyz.z, abs_error);
+
+ float abs_error_curv = 0.01f;
+ ASSERT_NEAR(n.curvature, curvature, abs_error_curv);
+ }
+}
+
+//TEST(PCL_FeaturesGPU, DISABLED_normals_highlevel_1)
+TEST(PCL_FeaturesGPU, normals_highlevel_1)
+{
+ DataSource source;
+ cout << "Cloud size: " << source.cloud->points.size() << endl;
+ cout << "Radius: " << source.radius << endl;
+ cout << "Max_elems: " << source.max_elements << endl;
+
+ cout << "!indices, !surface" << endl;
+
+ //source.runCloudViewer();
+
+ //source.generateSurface();
+ //source.generateIndices();
+
+ pcl::NormalEstimation<PointXYZ, Normal> ne;
+ ne.setInputCloud (source.cloud);
+ ne.setSearchMethod (pcl::search::KdTree<PointXYZ>::Ptr (new pcl::search::KdTree<PointXYZ>));
+ //ne.setKSearch (k);
+ ne.setRadiusSearch (source.radius);
+ //ne.setSearchSurface(source.surface);
+ //ne.setIndices(source.indices);
+
+ PointCloud<Normal>::Ptr normals(new PointCloud<Normal>());
+ ne.compute (*normals);
+
+ pcl::gpu::NormalEstimation::PointCloud cloud_device;
+ cloud_device.upload(source.cloud->points);
+
+ //pcl::gpu::NormalEstimation::PointCloud surface_device;
+ //surface_device.upload(source.surface->points);
+
+ //pcl::gpu::NormalEstimation::Indices indices_device;
+ //indices_device.upload(source.indices);
+
+ pcl::gpu::NormalEstimation ne_device;
+ ne_device.setInputCloud(cloud_device);
+ ne_device.setRadiusSearch(source.radius, source.max_elements);
+ //ne_device.setSearchSurface(surface_device);
+ //ne_device.setIndices(indices_device);
+
+ pcl::gpu::NormalEstimation::Normals normals_device;
+ ne_device.compute(normals_device);
+
+ vector<PointXYZ> downloaded;
+ normals_device.download(downloaded);
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ Normal n = normals->points[i];
+
+ PointXYZ xyz = downloaded[i];
+ float curvature = xyz.data[3];
+
+ float abs_error = 0.01f;
+ ASSERT_NEAR(n.normal_x, xyz.x, abs_error);
+ ASSERT_NEAR(n.normal_y, xyz.y, abs_error);
+ ASSERT_NEAR(n.normal_z, xyz.z, abs_error);
+
+ float abs_error_curv = 0.01f;
+ ASSERT_NEAR(n.curvature, curvature, abs_error_curv);
+ }
+}
+
+//TEST(PCL_FeaturesGPU, DISABLED_normals_highlevel_2)
+TEST(PCL_FeaturesGPU, normals_highlevel_2)
+{
+ DataSource source;
+ cout << "Cloud size: " << source.cloud->points.size() << endl;
+ cout << "Radius: " << source.radius << endl;
+ cout << "Max_elems: " << source.max_elements << endl;
+
+ cout << "indices, !surface" << endl;
+
+ //source.runCloudViewer();
+
+ //source.generateSurface();
+ source.generateIndices();
+
+ pcl::NormalEstimation<PointXYZ, Normal> ne;
+ ne.setInputCloud (source.cloud);
+ ne.setSearchMethod (pcl::search::KdTree<PointXYZ>::Ptr (new pcl::search::KdTree<PointXYZ>));
+ //ne.setKSearch (k);
+ ne.setRadiusSearch (source.radius);
+ //ne.setSearchSurface(source.surface);
+ ne.setIndices(source.indices);
+
+ PointCloud<Normal>::Ptr normals(new PointCloud<Normal>());
+ ne.compute (*normals);
+
+ pcl::gpu::NormalEstimation::PointCloud cloud_device;
+ cloud_device.upload(source.cloud->points);
+
+ //pcl::gpu::NormalEstimation::PointCloud surface_device;
+ //surface_device.upload(source.surface->points);
+
+ pcl::gpu::NormalEstimation::Indices indices_device;
+ indices_device.upload(*source.indices);
+
+ pcl::gpu::NormalEstimation ne_device;
+ ne_device.setInputCloud(cloud_device);
+ ne_device.setRadiusSearch(source.radius, source.max_elements);
+ //ne_device.setSearchSurface(surface_device);
+ ne_device.setIndices(indices_device);
+
+
+ pcl::gpu::NormalEstimation::Normals normals_device;
+ ne_device.compute(normals_device);
+
+ vector<PointXYZ> downloaded;
+ normals_device.download(downloaded);
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ Normal n = normals->points[i];
+
+ PointXYZ xyz = downloaded[i];
+ float curvature = xyz.data[3];
+
+ float abs_error = 0.01f;
+ ASSERT_NEAR(n.normal_x, xyz.x, abs_error);
+ ASSERT_NEAR(n.normal_y, xyz.y, abs_error);
+ ASSERT_NEAR(n.normal_z, xyz.z, abs_error);
+
+ float abs_error_curv = 0.01f;
+ ASSERT_NEAR(n.curvature, curvature, abs_error_curv);
+ }
+}
+
+//TEST(PCL_FeaturesGPU, DISABLED_normals_highlevel_3)
+TEST(PCL_FeaturesGPU, normals_highlevel_3)
+{
+ DataSource source;
+ cout << "Cloud size: " << source.cloud->points.size() << endl;
+ cout << "Radius: " << source.radius << endl;
+ cout << "Max_elems: " << source.max_elements << endl;
+
+ cout << "!indices, surface" << endl;
+
+ //source.runCloudViewer();
+
+ source.generateSurface();
+ //source.generateIndices();
+
+ pcl::NormalEstimation<PointXYZ, Normal> ne;
+ ne.setInputCloud (source.cloud);
+ ne.setSearchMethod (pcl::search::KdTree<PointXYZ>::Ptr (new pcl::search::KdTree<PointXYZ>));
+ //ne.setKSearch (k);
+ ne.setRadiusSearch (source.radius);
+ ne.setSearchSurface(source.surface);
+ //ne.setIndices(source.indices);
+
+ PointCloud<Normal>::Ptr normals(new PointCloud<Normal>());
+ ne.compute (*normals);
+
+ pcl::gpu::NormalEstimation::PointCloud cloud_device;
+ cloud_device.upload(source.cloud->points);
+
+ pcl::gpu::NormalEstimation::PointCloud surface_device;
+ surface_device.upload(source.surface->points);
+
+ //pcl::gpu::NormalEstimation::Indices indices_device;
+ //indices_device.upload(source.indices);
+
+ pcl::gpu::NormalEstimation ne_device;
+ ne_device.setInputCloud(cloud_device);
+ ne_device.setRadiusSearch(source.radius, source.max_elements);
+ ne_device.setSearchSurface(surface_device);
+ //ne_device.setIndices(indices_device);
+
+
+ pcl::gpu::NormalEstimation::Normals normals_device;
+ ne_device.compute(normals_device);
+
+ vector<PointXYZ> downloaded;
+ normals_device.download(downloaded);
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ Normal n = normals->points[i];
+
+ PointXYZ xyz = downloaded[i];
+ float curvature = xyz.data[3];
+
+ float abs_error = 0.01f;
+
+ if (pcl_isnan(n.normal_x) || pcl_isnan(n.normal_y) || pcl_isnan(n.normal_z))
+ continue;
+
+ ASSERT_EQ(pcl_isnan(n.normal_x), pcl_isnan(xyz.x));
+ ASSERT_EQ(pcl_isnan(n.normal_y), pcl_isnan(xyz.y));
+ ASSERT_EQ(pcl_isnan(n.normal_z), pcl_isnan(xyz.z));
+
+ ASSERT_NEAR(n.normal_x, xyz.x, abs_error);
+ ASSERT_NEAR(n.normal_y, xyz.y, abs_error);
+ ASSERT_NEAR(n.normal_z, xyz.z, abs_error);
+
+ float abs_error_curv = 0.01f;
+ ASSERT_NEAR(n.curvature, curvature, abs_error_curv);
+ }
+}
+
+
+//TEST(PCL_FeaturesGPU, DISABLED_normals_highlevel_4)
+TEST(PCL_FeaturesGPU, normals_highlevel_4)
+{
+ DataSource source;
+ cout << "Cloud size: " << source.cloud->points.size() << endl;
+ cout << "Radius: " << source.radius << endl;
+ cout << "Max_elems: " << source.max_elements << endl;
+
+ cout << "indices, surface" << endl;
+
+ //source.runCloudViewer();
+
+ source.generateSurface();
+ source.generateIndices();
+
+ pcl::NormalEstimation<PointXYZ, Normal> ne;
+ ne.setInputCloud (source.cloud);
+ ne.setSearchMethod (pcl::search::KdTree<PointXYZ>::Ptr (new pcl::search::KdTree<PointXYZ>));
+ //ne.setKSearch (k);
+ ne.setRadiusSearch (source.radius);
+ ne.setSearchSurface(source.surface);
+ ne.setIndices(source.indices);
+
+ PointCloud<Normal>::Ptr normals(new PointCloud<Normal>());
+ ne.compute (*normals);
+
+ pcl::gpu::NormalEstimation::PointCloud cloud_device;
+ cloud_device.upload(source.cloud->points);
+
+ pcl::gpu::NormalEstimation::PointCloud surface_device;
+ surface_device.upload(source.surface->points);
+
+ pcl::gpu::NormalEstimation::Indices indices_device;
+ indices_device.upload(*source.indices);
+
+ pcl::gpu::NormalEstimation ne_device;
+ ne_device.setInputCloud(cloud_device);
+ ne_device.setRadiusSearch(source.radius, source.max_elements);
+ ne_device.setSearchSurface(surface_device);
+ ne_device.setIndices(indices_device);
+
+
+ pcl::gpu::NormalEstimation::Normals normals_device;
+ ne_device.compute(normals_device);
+
+ vector<PointXYZ> downloaded;
+ normals_device.download(downloaded);
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ Normal n = normals->points[i];
+
+ PointXYZ xyz = downloaded[i];
+ float curvature = xyz.data[3];
+
+ float abs_error = 0.01f;
+
+ if (pcl_isnan(n.normal_x) || pcl_isnan(n.normal_y) || pcl_isnan(n.normal_z))
+ continue;
+
+ ASSERT_EQ(pcl_isnan(n.normal_x), pcl_isnan(xyz.x));
+ ASSERT_EQ(pcl_isnan(n.normal_y), pcl_isnan(xyz.y));
+ ASSERT_EQ(pcl_isnan(n.normal_z), pcl_isnan(xyz.z));
+
+ ASSERT_NEAR(n.normal_x, xyz.x, abs_error);
+ ASSERT_NEAR(n.normal_y, xyz.y, abs_error);
+ ASSERT_NEAR(n.normal_z, xyz.z, abs_error);
+
+ float abs_error_curv = 0.01f;
+ ASSERT_NEAR(n.curvature, curvature, abs_error_curv);
+ }
+}
+
+
+int main (int argc, char** argv)
+{
+ pcl::gpu::setDevice(0);
+ pcl::gpu::printShortCudaDeviceInfo(0);
+ testing::InitGoogleTest (&argc, argv);
+ return (RUN_ALL_TESTS ());
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#if (defined(__GNUC__) && !defined(__CUDACC__) && (GTEST_GCC_VER_ >= 40000))
+ #define GTEST_USE_OWN_TR1_TUPLE 0
+#endif
+
+#if defined(_MSC_VER) && (_MSC_VER >= 1500)
+ #define GTEST_USE_OWN_TR1_TUPLE 0
+#endif
+
+#include "gtest/gtest.h"
+
+#include <pcl/point_types.h>
+#include <pcl/features/pfh.h>
+#include <pcl/features/pfhrgb.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/common/common.h>
+#include <pcl/gpu/features/features.hpp>
+#include "data_source.hpp"
+#include <pcl/search/search.h>
+
+using namespace std;
+using namespace pcl;
+using namespace pcl::gpu;
+
+//TEST(PCL_FeaturesGPU, DISABLED_pfh_low_level)
+TEST(PCL_FeaturesGPU, pfh_low_level)
+{
+ DataSource source;
+
+ source.estimateNormals();
+ source.findRadiusNeghbors();
+ cout << "max_radius_nn_size: " << source.max_nn_size << endl;
+
+ vector<int> data;
+ source.getNeghborsArray(data);
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(source.normals->points.begin(), source.normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+ pcl::gpu::PFHEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::PFHEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ pcl::gpu::NeighborIndices indices;
+ indices.upload(data, source.sizes, source.max_nn_size);
+
+ DeviceArray2D<PFHSignature125> pfh125_features;
+
+ gpu::PFHEstimation pfh_gpu;
+ pfh_gpu.compute(cloud_gpu, normals_gpu, indices, pfh125_features);
+
+ int stub;
+ vector<PFHSignature125> downloaded;
+ pfh125_features.download(downloaded, stub);
+
+ pcl::PFHEstimation<PointXYZ, Normal, PFHSignature125> fe;
+ fe.setInputCloud (source.cloud);
+ fe.setInputNormals (source.normals);
+ fe.setSearchMethod (pcl::search::KdTree<PointXYZ>::Ptr (new pcl::search::KdTree<PointXYZ>));
+ //fe.setKSearch (k);
+ fe.setRadiusSearch (source.radius);
+
+ PointCloud<PFHSignature125> pfhs;
+ fe.compute (pfhs);
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ PFHSignature125& gpu = downloaded[i];
+ PFHSignature125& cpu = pfhs.points[i];
+
+ size_t FSize = sizeof(PFHSignature125)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+ if (norm != 0)
+ ASSERT_LE(norm_diff/norm, 0.01f/FSize);
+ //printf("norm_diff/norm = %f %f %f\n", norm_diff/norm, norm_diff, norm);
+ }
+}
+
+
+//TEST(PCL_FeaturesGPU, DISABLED_pfh_high_level1)
+TEST(PCL_FeaturesGPU, pfh_high_level1)
+{
+ DataSource source;
+ source.estimateNormals();
+
+ //source.generateSurface();
+ //source.generateIndices();
+
+ cout << "!indices, !surface" << endl;
+
+ PointCloud<Normal>::Ptr& normals = source.normals;
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(normals->points.begin(), normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+ pcl::gpu::PFHEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::PFHEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ //pcl::gpu::PFHEstimation::Indices indices_gpu;
+ //indices_gpu.upload(*source.indices);
+
+ //pcl::gpu::PFHEstimation::PointCloud surface_gpu;
+ //surface_gpu.upload(source.surface->points);
+
+ //GPU call
+ pcl::gpu::PFHEstimation fe_gpu;
+ fe_gpu.setInputCloud (cloud_gpu);
+ fe_gpu.setInputNormals (normals_gpu);
+ fe_gpu.setRadiusSearch (source.radius, source.max_elements);
+ //fe_gpu.setIndices(indices_gpu);
+ //fe_gpu.setSearchSurface(surface_gpu);
+
+ DeviceArray2D<PFHSignature125> fpfhs_gpu;
+ fe_gpu.compute(fpfhs_gpu);
+
+ // CPU call
+ pcl::PFHEstimation<PointXYZ, Normal, PFHSignature125> fe;
+ fe.setInputCloud (source.cloud);
+ fe.setInputNormals (normals);
+ fe.setSearchMethod (pcl::search::KdTree<PointXYZ>::Ptr (new pcl::search::KdTree<PointXYZ>));
+ //fe.setKSearch (k);
+ fe.setRadiusSearch (source.radius);
+ //fe.setIndices(source.indices);
+ //fe.setSearchSurface(source.surface);
+
+ PointCloud<PFHSignature125> fpfhs;
+ fe.compute (fpfhs);
+
+ int stub;
+ vector<PFHSignature125> downloaded;
+ fpfhs_gpu.download(downloaded, stub);
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ PFHSignature125& gpu = downloaded[i];
+ PFHSignature125& cpu = fpfhs.points[i];
+
+ size_t FSize = sizeof(PFHSignature125)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+ if (norm != 0)
+ ASSERT_LE(norm_diff/norm, 0.01f/FSize);
+ }
+}
+
+
+//TEST(PCL_FeaturesGPU, DISABLED_pfh_high_level2)
+TEST(PCL_FeaturesGPU, pfh_high_level2)
+{
+ DataSource source;
+ source.estimateNormals();
+
+ //source.generateSurface();
+ source.generateIndices();
+
+ cout << "indices, !surface" << endl;
+
+ PointCloud<Normal>::Ptr& normals = source.normals;
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(normals->points.begin(), normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+ pcl::gpu::PFHEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::PFHEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ pcl::gpu::PFHEstimation::Indices indices_gpu;
+ indices_gpu.upload(*source.indices);
+
+ //pcl::gpu::PFHEstimation::PointCloud surface_gpu;
+ //surface_gpu.upload(source.surface->points);
+
+ //GPU call
+ pcl::gpu::PFHEstimation fe_gpu;
+ fe_gpu.setInputCloud (cloud_gpu);
+ fe_gpu.setInputNormals (normals_gpu);
+ fe_gpu.setRadiusSearch (source.radius, source.max_elements);
+ fe_gpu.setIndices(indices_gpu);
+ //fe_gpu.setSearchSurface(surface_gpu);
+
+ DeviceArray2D<PFHSignature125> fpfhs_gpu;
+ fe_gpu.compute(fpfhs_gpu);
+
+ // CPU call
+ pcl::PFHEstimation<PointXYZ, Normal, PFHSignature125> fe;
+ fe.setInputCloud (source.cloud);
+ fe.setInputNormals (normals);
+ fe.setSearchMethod (pcl::search::KdTree<PointXYZ>::Ptr (new pcl::search::KdTree<PointXYZ>));
+ //fe.setKSearch (k);
+ fe.setRadiusSearch (source.radius);
+ fe.setIndices(source.indices);
+ //fe.setSearchSurface(source.surface);
+
+ PointCloud<PFHSignature125> fpfhs;
+ fe.compute (fpfhs);
+
+ int stub;
+ vector<PFHSignature125> downloaded;
+ fpfhs_gpu.download(downloaded, stub);
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ PFHSignature125& gpu = downloaded[i];
+ PFHSignature125& cpu = fpfhs.points[i];
+
+ size_t FSize = sizeof(PFHSignature125)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+ if (norm != 0)
+ ASSERT_LE(norm_diff/norm, 0.01f/FSize);
+ }
+}
+
+
+//TEST(PCL_FeaturesGPU, DISABLED_pfh_high_level3)
+TEST(PCL_FeaturesGPU, pfh_high_level3)
+{
+ DataSource source;
+ source.estimateNormals();
+
+ source.generateSurface();
+ //source.generateIndices();
+
+ cout << "!indices, surface" << endl;
+
+ PointCloud<Normal>::Ptr& normals = source.normals_surface;
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(normals->points.begin(), normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+ pcl::gpu::PFHEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::PFHEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ //pcl::gpu::PFHEstimation::Indices indices_gpu;
+ //indices_gpu.upload(*source.indices);
+
+ pcl::gpu::PFHEstimation::PointCloud surface_gpu;
+ surface_gpu.upload(source.surface->points);
+
+ //GPU call
+ pcl::gpu::PFHEstimation fe_gpu;
+ fe_gpu.setInputCloud (cloud_gpu);
+ fe_gpu.setInputNormals (normals_gpu);
+ fe_gpu.setRadiusSearch (source.radius, source.max_elements);
+ //fe_gpu.setIndices(indices_gpu);
+ fe_gpu.setSearchSurface(surface_gpu);
+
+ DeviceArray2D<PFHSignature125> fpfhs_gpu;
+ fe_gpu.compute(fpfhs_gpu);
+
+ // CPU call
+ pcl::PFHEstimation<PointXYZ, Normal, PFHSignature125> fe;
+ fe.setInputCloud (source.cloud);
+ fe.setInputNormals (normals);
+ fe.setSearchMethod (pcl::search::KdTree<PointXYZ>::Ptr (new pcl::search::KdTree<PointXYZ>));
+ //fe.setKSearch (k);
+ fe.setRadiusSearch (source.radius);
+ //fe.setIndices(source.indices);
+ fe.setSearchSurface(source.surface);
+
+ PointCloud<PFHSignature125> fpfhs;
+ fe.compute (fpfhs);
+
+ int stub;
+ vector<PFHSignature125> downloaded;
+ fpfhs_gpu.download(downloaded, stub);
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ PFHSignature125& gpu = downloaded[i];
+ PFHSignature125& cpu = fpfhs.points[i];
+
+ size_t FSize = sizeof(PFHSignature125)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+ if (norm != 0)
+ ASSERT_LE(norm_diff/norm, 0.01f/FSize);
+ }
+}
+
+
+//TEST(PCL_FeaturesGPU, DISABLED_pfh_high_level4)
+TEST(PCL_FeaturesGPU, pfh_high_level4)
+{
+ DataSource source;
+ source.estimateNormals();
+
+ source.generateSurface();
+ source.generateIndices();
+
+ cout << "indices, surface" << endl;
+
+ PointCloud<Normal>::Ptr& normals = source.normals_surface;
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(normals->points.begin(), normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+ pcl::gpu::PFHEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::PFHEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ pcl::gpu::PFHEstimation::Indices indices_gpu;
+ indices_gpu.upload(*source.indices);
+
+ pcl::gpu::PFHEstimation::PointCloud surface_gpu;
+ surface_gpu.upload(source.surface->points);
+
+ //GPU call
+ pcl::gpu::PFHEstimation fe_gpu;
+ fe_gpu.setInputCloud (cloud_gpu);
+ fe_gpu.setInputNormals (normals_gpu);
+ fe_gpu.setRadiusSearch (source.radius, source.max_elements);
+ fe_gpu.setIndices(indices_gpu);
+ fe_gpu.setSearchSurface(surface_gpu);
+
+ DeviceArray2D<PFHSignature125> fpfhs_gpu;
+ fe_gpu.compute(fpfhs_gpu);
+
+ // CPU call
+ pcl::PFHEstimation<PointXYZ, Normal, PFHSignature125> fe;
+ fe.setInputCloud (source.cloud);
+ fe.setInputNormals (normals);
+ fe.setSearchMethod (pcl::search::KdTree<PointXYZ>::Ptr (new pcl::search::KdTree<PointXYZ>));
+ //fe.setKSearch (k);
+ fe.setRadiusSearch (source.radius);
+ fe.setIndices(source.indices);
+ fe.setSearchSurface(source.surface);
+
+ PointCloud<PFHSignature125> fpfhs;
+ fe.compute (fpfhs);
+
+ int stub;
+ vector<PFHSignature125> downloaded;
+ fpfhs_gpu.download(downloaded, stub);
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ PFHSignature125& gpu = downloaded[i];
+ PFHSignature125& cpu = fpfhs.points[i];
+
+ size_t FSize = sizeof(PFHSignature125)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+ if (norm != 0)
+ ASSERT_LE(norm_diff/norm, 0.01f/FSize);
+ }
+}
+
+//TEST(PCL_FeaturesGPU, DISABLED_pfhrgb)
+TEST(PCL_FeaturesGPU, pfhrgb)
+{
+ DataSource source;
+ source.generateColor();
+ source.estimateNormals();
+
+ //source.generateSurface();
+ //source.generateIndices();
+
+ cout << "!indices, !surface" << endl;
+
+ PointCloud<Normal>::Ptr& normals = source.normals;
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(normals->points.begin(), normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+ pcl::gpu::PFHRGBEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::PFHRGBEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ //pcl::gpu::PFHEstimation::Indices indices_gpu;
+ //indices_gpu.upload(*source.indices);
+
+ //pcl::gpu::PFHEstimation::PointCloud surface_gpu;
+ //surface_gpu.upload(source.surface->points);
+
+ //GPU call
+ pcl::gpu::PFHRGBEstimation fe_gpu;
+ fe_gpu.setInputCloud (cloud_gpu);
+ fe_gpu.setInputNormals (normals_gpu);
+ fe_gpu.setRadiusSearch (source.radius, source.max_elements);
+ //fe_gpu.setIndices(indices_gpu);
+ //fe_gpu.setSearchSurface(surface_gpu);
+
+ DeviceArray2D<PFHRGBSignature250> fpfhs_gpu;
+ fe_gpu.compute(fpfhs_gpu);
+
+ // CPU call
+ pcl::PFHRGBEstimation<PointXYZRGB, Normal, PFHRGBSignature250> fe;
+
+ PointCloud<PointXYZRGB>::Ptr cloud_XYZRGB(new PointCloud<PointXYZRGB>());
+ cloud_XYZRGB->points.clear();
+ for(size_t i = 0; i < source.cloud->points.size(); ++i)
+ {
+ const PointXYZ& p = source.cloud->points[i];
+ PointXYZRGB o;
+
+ int color = *(int*)&p.data[3];
+ int r = color & 0xFF;
+ int g = (color >> 8) & 0xFF;
+ int b = (color >> 16) & 0xFF;
+
+ o.x = p.x; o.y = p.y; o.z = p.z;
+ o.r = r; o.g = g; o.b = b;
+
+ cloud_XYZRGB->points.push_back(o);
+ }
+ cloud_XYZRGB->width = cloud_XYZRGB->points.size();
+ cloud_XYZRGB->height = 1;
+
+ fe.setInputCloud (cloud_XYZRGB);
+ fe.setInputNormals (normals);
+ fe.setSearchMethod (pcl::search::KdTree<PointXYZRGB>::Ptr (new pcl::search::KdTree<PointXYZRGB>));
+ //fe.setKSearch (k);
+ fe.setRadiusSearch (source.radius);
+ //fe.setIndices(source.indices);
+ //fe.setSearchSurface(source.surface);
+
+ PointCloud<PFHRGBSignature250> fpfhs;
+ fe.compute (fpfhs);
+
+ int stub;
+ vector<PFHRGBSignature250> downloaded;
+ fpfhs_gpu.download(downloaded, stub);
+
+ for(size_t i = 170; i < downloaded.size(); ++i)
+ {
+ PFHRGBSignature250& gpu = downloaded[i];
+ PFHRGBSignature250& cpu = fpfhs.points[i];
+
+ size_t FSize = sizeof(PFHRGBSignature250)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+ if (norm != 0)
+ ASSERT_LE(norm_diff/norm, 0.01f);
+ }
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#if (defined(__GNUC__) && !defined(__CUDACC__) && (GTEST_GCC_VER_ >= 40000))
+ #define GTEST_USE_OWN_TR1_TUPLE 0
+#endif
+
+#if defined(_MSC_VER) && (_MSC_VER >= 1500)
+ #define GTEST_USE_OWN_TR1_TUPLE 0
+#endif
+
+#include "gtest/gtest.h"
+
+#include <pcl/point_types.h>
+#include <pcl/features/ppf.h>
+#include <pcl/features/impl/ppf.hpp>
+#include <pcl/features/ppfrgb.h>
+#include <pcl/features/impl/ppfrgb.hpp>
+#include <pcl/gpu/features/features.hpp>
+
+#include "data_source.hpp"
+
+using namespace std;
+using namespace pcl;
+using namespace pcl::gpu;
+
+//TEST(PCL_FeaturesGPU, DISABLED_ppf)
+TEST(PCL_FeaturesGPU, ppf)
+{
+ DataSource source;
+
+ source.generateIndices();
+ source.estimateNormals();
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(source.normals->points.begin(), source.normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+
+ pcl::gpu::PPFEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::PPFEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ pcl::gpu::PPFEstimation::Indices indices_gpu;
+ indices_gpu.upload(*source.indices);
+
+ DeviceArray<PPFSignature> ppf_features;
+
+ gpu::PPFEstimation pph_gpu;
+ pph_gpu.setInputCloud(cloud_gpu);
+ pph_gpu.setInputNormals(normals_gpu);
+ pph_gpu.setIndices(indices_gpu);
+ pph_gpu.compute(ppf_features);
+
+
+ vector<PPFSignature> downloaded;
+ ppf_features.download(downloaded);
+
+ pcl::PPFEstimation<PointXYZ, Normal, PPFSignature> fe;
+ fe.setInputCloud (source.cloud);
+ fe.setInputNormals (source.normals);
+ fe.setIndices(source.indices);
+
+ PointCloud<PPFSignature> ppfs;
+ fe.compute (ppfs);
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ PPFSignature& gpu = downloaded[i];
+ PPFSignature& cpu = ppfs.points[i];
+
+ ASSERT_NEAR(gpu.f1, cpu.f1, 0.01f);
+ ASSERT_NEAR(gpu.f2, cpu.f2, 0.01f);
+ ASSERT_NEAR(gpu.f3, cpu.f3, 0.01f);
+ ASSERT_NEAR(gpu.f4, cpu.f4, 0.01f);
+ ASSERT_NEAR(gpu.alpha_m, cpu.alpha_m, 0.01f);
+ }
+}
+
+
+//TEST(PCL_FeaturesGPU, DISABLED_ppfrgb)
+TEST(PCL_FeaturesGPU, ppfrgb)
+{
+ DataSource source;
+ source.generateColor();
+
+ source.generateIndices();
+ source.estimateNormals();
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(source.normals->points.begin(), source.normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+
+ pcl::gpu::PPFRGBEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::PPFRGBEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ pcl::gpu::PPFRGBEstimation::Indices indices_gpu;
+ indices_gpu.upload(*source.indices);
+
+ DeviceArray<PPFRGBSignature> ppf_features;
+
+ gpu::PPFRGBEstimation pph_gpu;
+ pph_gpu.setInputCloud(cloud_gpu);
+ pph_gpu.setInputNormals(normals_gpu);
+ pph_gpu.setIndices(indices_gpu);
+ pph_gpu.compute(ppf_features);
+
+ vector<PPFRGBSignature> downloaded;
+ ppf_features.download(downloaded);
+
+ pcl::PPFRGBEstimation<PointXYZRGB, Normal, PPFRGBSignature> fe;
+
+ PointCloud<PointXYZRGB>::Ptr cloud_XYZRGB(new PointCloud<PointXYZRGB>());
+ cloud_XYZRGB->points.clear();
+ for(size_t i = 0; i < source.cloud->points.size(); ++i)
+ {
+ const PointXYZ& p = source.cloud->points[i];
+ int color = *(int*)&p.data[3];
+ int r = color & 0xFF;
+ int g = (color >> 8) & 0xFF;
+ int b = (color >> 16) & 0xFF;
+
+ PointXYZRGB o;
+ o.x = p.x; o.y = p.y; o.z = p.z; o.r = r; o.g = g; o.b = b;
+ cloud_XYZRGB->points.push_back(o);
+ }
+ cloud_XYZRGB->width = cloud_XYZRGB->points.size();
+ cloud_XYZRGB->height = 1;
+
+
+ fe.setInputCloud (cloud_XYZRGB);
+ fe.setInputNormals (source.normals);
+ fe.setIndices(source.indices);
+
+ PointCloud<PPFRGBSignature> ppfs;
+ fe.compute (ppfs);
+
+ for(size_t i = 207025; i < downloaded.size(); ++i)
+ {
+ PPFRGBSignature& gpu = downloaded[i];
+ PPFRGBSignature& cpu = ppfs.points[i];
+
+ ASSERT_NEAR(gpu.f1, cpu.f1, 0.01f);
+ ASSERT_NEAR(gpu.f2, cpu.f2, 0.01f);
+ ASSERT_NEAR(gpu.f3, cpu.f3, 0.01f);
+ ASSERT_NEAR(gpu.f4, cpu.f4, 0.01f);
+ ASSERT_NEAR(gpu.alpha_m, cpu.alpha_m, 0.01f);
+
+ if (pcl_isnan(gpu.r_ratio) || pcl_isnan(gpu.g_ratio) || pcl_isnan(gpu.b_ratio) ||
+ pcl_isnan(cpu.r_ratio) || pcl_isnan(cpu.g_ratio) || pcl_isnan(cpu.b_ratio))
+ continue;
+
+ ASSERT_NEAR(gpu.r_ratio, cpu.r_ratio, 0.01f);
+ ASSERT_NEAR(gpu.g_ratio, cpu.g_ratio, 0.01f);
+ ASSERT_NEAR(gpu.b_ratio, cpu.b_ratio, 0.01f);
+ }
+}
+
+
+//TEST(PCL_FeaturesGPU, DISABLED_ppfrgb_region)
+TEST(PCL_FeaturesGPU, ppfrgb_region)
+{
+ DataSource source;
+
+ source.generateColor();
+
+ source.generateIndices();
+ source.radius/=2.f;
+
+ source.estimateNormals();
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(source.normals->points.begin(), source.normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+ //////////////////////////////////////////////////////////////////////////////////////////////
+
+ pcl::gpu::PPFRGBRegionEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::PPFRGBRegionEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ pcl::gpu::PPFRGBRegionEstimation::Indices indices_gpu;
+ indices_gpu.upload(*source.indices);
+
+ DeviceArray<PPFRGBSignature> ppf_features;
+
+ gpu::PPFRGBRegionEstimation pph_gpu;
+ pph_gpu.setInputCloud(cloud_gpu);
+ pph_gpu.setInputNormals(normals_gpu);
+ pph_gpu.setIndices(indices_gpu);
+
+ pph_gpu.setRadiusSearch(source.radius, source.max_elements);
+
+ pph_gpu.compute(ppf_features);
+
+ vector<PPFRGBSignature> downloaded;
+ ppf_features.download(downloaded);
+
+ pcl::PPFRGBRegionEstimation<PointXYZRGB, Normal, PPFRGBSignature> fe;
+
+ PointCloud<PointXYZRGB>::Ptr cloud_XYZRGB(new PointCloud<PointXYZRGB>());
+ cloud_XYZRGB->points.clear();
+ for(size_t i = 0; i < source.cloud->points.size(); ++i)
+ {
+ const PointXYZ& p = source.cloud->points[i];
+ int color = *(int*)&p.data[3];
+ int r = color & 0xFF;
+ int g = (color >> 8) & 0xFF;
+ int b = (color >> 16) & 0xFF;
+
+ PointXYZRGB o;
+ o.x = p.x; o.y = p.y; o.z = p.z; o.r = r; o.g = g; o.b = b;
+ cloud_XYZRGB->points.push_back(o);
+ }
+ cloud_XYZRGB->width = cloud_XYZRGB->points.size();
+ cloud_XYZRGB->height = 1;
+
+
+ fe.setInputCloud (cloud_XYZRGB);
+ fe.setInputNormals (source.normals);
+ fe.setIndices(source.indices);
+ fe.setRadiusSearch(source.radius);
+
+ PointCloud<PPFRGBSignature> ppfs;
+ fe.compute (ppfs);
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ PPFRGBSignature& gpu = downloaded[i];
+ PPFRGBSignature& cpu = ppfs.points[i];
+
+ ASSERT_NEAR(gpu.f1, cpu.f1, 0.01f);
+ ASSERT_NEAR(gpu.f2, cpu.f2, 0.01f);
+ ASSERT_NEAR(gpu.f3, cpu.f3, 0.01f);
+ ASSERT_NEAR(gpu.f4, cpu.f4, 0.01f);
+ ASSERT_NEAR(gpu.alpha_m, cpu.alpha_m, 0.01f);
+
+ if (pcl_isnan(gpu.r_ratio) || pcl_isnan(gpu.g_ratio) || pcl_isnan(gpu.b_ratio) ||
+ pcl_isnan(cpu.r_ratio) || pcl_isnan(cpu.g_ratio) || pcl_isnan(cpu.b_ratio))
+ continue;
+
+ ASSERT_NEAR(gpu.r_ratio, cpu.r_ratio, 0.01f);
+ ASSERT_NEAR(gpu.g_ratio, cpu.g_ratio, 0.01f);
+ ASSERT_NEAR(gpu.b_ratio, cpu.b_ratio, 0.01f);
+ }
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#if (defined(__GNUC__) && !defined(__CUDACC__) && (GTEST_GCC_VER_ >= 40000))
+ #define GTEST_USE_OWN_TR1_TUPLE 0
+#endif
+
+#if defined(_MSC_VER) && (_MSC_VER >= 1500)
+ #define GTEST_USE_OWN_TR1_TUPLE 0
+#endif
+
+#include "gtest/gtest.h"
+
+#include <pcl/point_types.h>
+#include <pcl/features/principal_curvatures.h>
+#include <pcl/gpu/features/features.hpp>
+
+#include "data_source.hpp"
+
+using namespace std;
+using namespace pcl;
+using namespace pcl::gpu;
+
+//TEST(PCL_FeaturesGPU, DISABLED_PrincipalCurvatures)
+TEST(PCL_FeaturesGPU, PrincipalCurvatures)
+{
+ DataSource source;
+
+ source.estimateNormals();
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(source.normals->points.begin(), source.normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+
+ pcl::gpu::PrincipalCurvaturesEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::PrincipalCurvaturesEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ DeviceArray<PrincipalCurvatures> pc_features;
+
+ gpu::PrincipalCurvaturesEstimation pc_gpu;
+ pc_gpu.setInputCloud(cloud_gpu);
+ pc_gpu.setInputNormals(normals_gpu);
+ pc_gpu.setRadiusSearch(source.radius, source.max_elements);
+ pc_gpu.compute(pc_features);
+
+ vector<PrincipalCurvatures> downloaded;
+ pc_features.download(downloaded);
+
+ pcl::PrincipalCurvaturesEstimation<PointXYZ, Normal, PrincipalCurvatures> fe;
+ fe.setInputCloud (source.cloud);
+ fe.setInputNormals (source.normals);
+ fe.setRadiusSearch(source.radius);
+
+ PointCloud<PrincipalCurvatures> pc;
+ fe.compute (pc);
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ PrincipalCurvatures& gpu = downloaded[i];
+ PrincipalCurvatures& cpu = pc.points[i];
+
+ ASSERT_NEAR(gpu.principal_curvature_x, cpu.principal_curvature_x, 0.01f);
+ ASSERT_NEAR(gpu.principal_curvature_y, cpu.principal_curvature_y, 0.01f);
+ ASSERT_NEAR(gpu.principal_curvature_z, cpu.principal_curvature_z, 0.01f);
+ ASSERT_NEAR(gpu.pc1, cpu.pc1, 0.01f);
+ ASSERT_NEAR(gpu.pc2, cpu.pc2, 0.01f);
+ }
+}
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#if (defined(__GNUC__) && !defined(__CUDACC__) && (GTEST_GCC_VER_ >= 40000))
+ #define GTEST_USE_OWN_TR1_TUPLE 0
+#endif
+
+#if defined(_MSC_VER) && (_MSC_VER >= 1500)
+ #define GTEST_USE_OWN_TR1_TUPLE 0
+#endif
+
+#include "gtest/gtest.h"
+
+#include <pcl/point_types.h>
+#include <pcl/features/spin_image.h>
+#include <pcl/gpu/features/features.hpp>
+#include <pcl/common/time.h>
+
+#include "data_source.hpp"
+
+using namespace std;
+using namespace pcl;
+using namespace pcl::gpu;
+
+typedef pcl::Histogram<153> SpinImage;
+
+//TEST(PCL_FeaturesGPU, DISABLED_spinImages_rectangular)
+TEST(PCL_FeaturesGPU, spinImages_rectangular)
+{
+ DataSource source;
+
+ source.estimateNormals();
+ source.generateIndices(5);
+ //source.indices->resize(1);
+ source.radius *= 2;
+
+ const int min_beighbours = 15;
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(source.normals->points.begin(), source.normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+ pcl::gpu::VFHEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::VFHEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ pcl::gpu::VFHEstimation::Indices indices_gpu;
+ indices_gpu.upload(*source.indices);
+
+ //////////// GPU ////////////
+ gpu::SpinImageEstimation se_gpu;
+ se_gpu.setInputWithNormals(cloud_gpu, normals_gpu);
+ se_gpu.setIndices(indices_gpu);
+ se_gpu.setRadiusSearch(source.radius, source.max_elements*10);
+ se_gpu.setMinPointCountInNeighbourhood(min_beighbours);
+ //se_gpu.setRadialStructure();
+ //se_gpu.setAngularDomain();
+
+ DeviceArray2D<SpinImage> spin_images_device;
+ DeviceArray<unsigned char> mask_device;
+
+ {
+ ScopeTime up("gpu");
+ se_gpu.compute(spin_images_device, mask_device);
+ }
+
+ int c;
+ vector<SpinImage> downloaded;
+ vector<unsigned char> downloaded_mask;
+ spin_images_device.download(downloaded, c);
+ mask_device.download(downloaded_mask);
+
+ //////////// CPU ////////////
+ pcl::SpinImageEstimation<PointXYZ, Normal, SpinImage> se(8, 0.0, 16);
+ se.setInputCloud (source.cloud);
+ se.setInputNormals (source.normals);
+ se.setIndices (source.indices);
+ se.setRadiusSearch (source.radius);
+ se.setMinPointCountInNeighbourhood(min_beighbours);
+ //se.setRadialStructure();
+ //se.setAngularDomain();
+
+ PointCloud<SpinImage>::Ptr spin_images (new PointCloud<SpinImage> ());
+ {
+ ScopeTime up("cpu");
+ se.compute (*spin_images);
+ }
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ if(!downloaded_mask[i]) // less than min neighbours, so spinimage wasn't computed
+ continue;
+
+ SpinImage& gpu = downloaded[i];
+ SpinImage& cpu = spin_images->points[i];
+
+ size_t FSize = sizeof(SpinImage)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+
+ if (norm != 0)
+ ASSERT_LE(norm_diff/norm, 0.01f/FSize);
+ }
+}
+
+
+//TEST(PCL_FeaturesGPU, DISABLED_spinImages_radial)
+TEST(PCL_FeaturesGPU, spinImages_radial)
+{
+ DataSource source;
+
+ source.estimateNormals();
+ source.generateIndices(5);
+ //source.indices->resize(1);
+ source.radius *= 2;
+
+ const int min_beighbours = 15;
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(source.normals->points.begin(), source.normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+
+ //uploading data to GPU
+ pcl::gpu::VFHEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::VFHEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ pcl::gpu::VFHEstimation::Indices indices_gpu;
+ indices_gpu.upload(*source.indices);
+
+ //////////// GPU ////////////
+ gpu::SpinImageEstimation se_gpu;
+ se_gpu.setInputWithNormals(cloud_gpu, normals_gpu);
+ se_gpu.setIndices(indices_gpu);
+ se_gpu.setRadiusSearch(source.radius, source.max_elements*10);
+ se_gpu.setMinPointCountInNeighbourhood(min_beighbours);
+ se_gpu.setRadialStructure();
+ //se_gpu.setAngularDomain();
+
+ DeviceArray2D<SpinImage> spin_images_device;
+ DeviceArray<unsigned char> mask_device;
+
+ {
+ ScopeTime up("gpu");
+ se_gpu.compute(spin_images_device, mask_device);
+ }
+
+ int c;
+ vector<SpinImage> downloaded;
+ vector<unsigned char> downloaded_mask;
+ spin_images_device.download(downloaded, c);
+ mask_device.download(downloaded_mask);
+
+ //////////// CPU ////////////
+ pcl::SpinImageEstimation<PointXYZ, Normal, SpinImage> se(8, 0.0, 16);
+ se.setInputCloud (source.cloud);
+ se.setInputNormals (source.normals);
+ se.setIndices (source.indices);
+ se.setRadiusSearch (source.radius);
+ se.setMinPointCountInNeighbourhood(min_beighbours);
+ se.setRadialStructure();
+ //se.setAngularDomain();
+
+ PointCloud<SpinImage>::Ptr spin_images (new PointCloud<SpinImage> ());
+ {
+ ScopeTime up("cpu");
+ se.compute (*spin_images);
+ }
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ if(!downloaded_mask[i]) // less than min neighbours, so spinimage wasn't computed
+ continue;
+
+ SpinImage& gpu = downloaded[i];
+ SpinImage& cpu = spin_images->points[i];
+
+ size_t FSize = sizeof(SpinImage)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+
+ if (norm != 0)
+ ASSERT_LE(norm_diff/norm, 0.01f/FSize);
+ }
+}
+
+
+//TEST(PCL_FeaturesGPU, DISABLED_spinImages_rectangular_angular)
+TEST(PCL_FeaturesGPU, spinImages_rectangular_angular)
+{
+ DataSource source;
+
+ source.estimateNormals();
+ source.generateIndices(5);
+ //source.indices->resize(1);
+ source.radius *= 2;
+
+ const int min_beighbours = 15;
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(source.normals->points.begin(), source.normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+
+ //uploading data to GPU
+ pcl::gpu::VFHEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::VFHEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ pcl::gpu::VFHEstimation::Indices indices_gpu;
+ indices_gpu.upload(*source.indices);
+
+ //////////// GPU ////////////
+ gpu::SpinImageEstimation se_gpu;
+ se_gpu.setInputWithNormals(cloud_gpu, normals_gpu);
+ se_gpu.setIndices(indices_gpu);
+ se_gpu.setRadiusSearch(source.radius, source.max_elements*10);
+ se_gpu.setMinPointCountInNeighbourhood(min_beighbours);
+ //se_gpu.setRadialStructure();
+ se_gpu.setAngularDomain();
+
+ DeviceArray2D<SpinImage> spin_images_device;
+ DeviceArray<unsigned char> mask_device;
+
+ {
+ ScopeTime up("gpu");
+ se_gpu.compute(spin_images_device, mask_device);
+ }
+
+ int c;
+ vector<SpinImage> downloaded;
+ vector<unsigned char> downloaded_mask;
+ spin_images_device.download(downloaded, c);
+ mask_device.download(downloaded_mask);
+
+ //////////// CPU ////////////
+ pcl::SpinImageEstimation<PointXYZ, Normal, SpinImage> se(8, 0.0, 16);
+ se.setInputCloud (source.cloud);
+ se.setInputNormals (source.normals);
+ se.setIndices (source.indices);
+ se.setRadiusSearch (source.radius);
+ se.setMinPointCountInNeighbourhood(min_beighbours);
+ //se.setRadialStructure();
+ se.setAngularDomain();
+
+ PointCloud<SpinImage>::Ptr spin_images (new PointCloud<SpinImage> ());
+ {
+ ScopeTime up("cpu");
+ se.compute (*spin_images);
+ }
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ if(!downloaded_mask[i]) // less than min neighbours, so spinimage wasn't computed
+ continue;
+
+ SpinImage& gpu = downloaded[i];
+ SpinImage& cpu = spin_images->points[i];
+
+ size_t FSize = sizeof(SpinImage)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+
+ if (norm != 0)
+ ASSERT_LE(norm_diff/norm, 0.02f/FSize);
+ }
+}
+
+
+//TEST(PCL_FeaturesGPU, DISABLED_spinImages_radial_angular)
+TEST(PCL_FeaturesGPU, spinImages_radial_angular)
+{
+ DataSource source;
+
+ source.estimateNormals();
+ source.generateIndices(5);
+ //source.indices->resize(1);
+ source.radius *= 2;
+
+ const int min_beighbours = 15;
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(source.normals->points.begin(), source.normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+
+ //uploading data to GPU
+ pcl::gpu::VFHEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::VFHEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ pcl::gpu::VFHEstimation::Indices indices_gpu;
+ indices_gpu.upload(*source.indices);
+
+ //////////// GPU ////////////
+ gpu::SpinImageEstimation se_gpu;
+ se_gpu.setInputWithNormals(cloud_gpu, normals_gpu);
+ se_gpu.setIndices(indices_gpu);
+ se_gpu.setRadiusSearch(source.radius, source.max_elements*10);
+ se_gpu.setMinPointCountInNeighbourhood(min_beighbours);
+ se_gpu.setRadialStructure();
+ se_gpu.setAngularDomain();
+
+ DeviceArray2D<SpinImage> spin_images_device;
+ DeviceArray<unsigned char> mask_device;
+
+ {
+ ScopeTime up("gpu");
+ se_gpu.compute(spin_images_device, mask_device);
+ }
+
+ int c;
+ vector<SpinImage> downloaded;
+ vector<unsigned char> downloaded_mask;
+ spin_images_device.download(downloaded, c);
+ mask_device.download(downloaded_mask);
+
+ //////////// CPU ////////////
+ pcl::SpinImageEstimation<PointXYZ, Normal, SpinImage> se(8, 0.0, 16);
+ se.setInputCloud (source.cloud);
+ se.setInputNormals (source.normals);
+ se.setIndices (source.indices);
+ se.setRadiusSearch (source.radius);
+ se.setMinPointCountInNeighbourhood(min_beighbours);
+ se.setRadialStructure();
+ se.setAngularDomain();
+
+ PointCloud<SpinImage>::Ptr spin_images (new PointCloud<SpinImage> ());
+ {
+ ScopeTime up("cpu");
+ se.compute (*spin_images);
+ }
+
+ for(size_t i = 0; i < downloaded.size(); ++i)
+ {
+ if(!downloaded_mask[i]) // less than min neighbours, so spinimage wasn't computed
+ continue;
+
+ SpinImage& gpu = downloaded[i];
+ SpinImage& cpu = spin_images->points[i];
+
+ size_t FSize = sizeof(SpinImage)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+
+ if (norm != 0)
+ ASSERT_LE(norm_diff/norm, 0.01f/FSize);
+ }
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+
+#if (defined(__GNUC__) && !defined(__CUDACC__) && (GTEST_GCC_VER_ >= 40000))
+ #define GTEST_USE_OWN_TR1_TUPLE 0
+#endif
+
+#if defined(_MSC_VER) && (_MSC_VER >= 1500)
+ #define GTEST_USE_OWN_TR1_TUPLE 0
+#endif
+
+
+#include "gtest/gtest.h"
+
+#include <pcl/point_types.h>
+#include <pcl/features/vfh.h>
+#include <pcl/gpu/features/features.hpp>
+#include <pcl/common/time.h>
+
+#include "data_source.hpp"
+
+using namespace std;
+using namespace pcl;
+using namespace pcl::gpu;
+
+//TEST(PCL_FeaturesGPU, DISABLED_vfh1)
+TEST(PCL_FeaturesGPU, vfh1)
+{
+ DataSource source;
+
+ source.estimateNormals();
+ source.generateIndices(3);
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(source.normals->points.begin(), source.normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+
+ pcl::gpu::VFHEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::VFHEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ pcl::gpu::VFHEstimation::Indices indices_gpu;
+ indices_gpu.upload(*source.indices);
+
+ gpu::VFHEstimation pc_gpu;
+ pc_gpu.setSearchSurface(cloud_gpu);
+ pc_gpu.setInputNormals(normals_gpu);
+ pc_gpu.setIndices(indices_gpu);
+ pc_gpu.setRadiusSearch(source.radius, source.max_elements);
+
+ DeviceArray<VFHSignature308> vfh_features;
+
+ {
+ //ScopeTime up("gpu");
+ pc_gpu.compute(vfh_features);
+ }
+
+ vector<VFHSignature308> downloaded;
+ vfh_features.download(downloaded);
+
+ pcl::VFHEstimation<PointXYZ, Normal, VFHSignature308> fe;
+ fe.setInputCloud (source.cloud);
+ fe.setInputNormals (source.normals);
+ fe.setIndices(source.indices);
+ fe.setRadiusSearch(source.radius);
+ fe.setKSearch(0);
+
+ PointCloud<VFHSignature308> vfh;
+ {
+ //ScopeTime up("cpu");
+ fe.compute (vfh);
+ }
+
+ VFHSignature308& gpu = downloaded[0];
+ VFHSignature308& cpu = vfh.points[0];
+
+ size_t FSize = sizeof(VFHSignature308)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+ if (norm != 0)
+ ASSERT_LE(norm_diff/norm, 0.01f/FSize);
+ else
+ FAIL();
+}
+
+//TEST(PCL_FeaturesGPU, DISABLED_vfh_norm_bins_false)
+TEST(PCL_FeaturesGPU, vfh_norm_bins_false)
+{
+ DataSource source;
+
+ source.estimateNormals();
+ source.generateIndices(3);
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(source.normals->points.begin(), source.normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+
+ pcl::gpu::VFHEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::VFHEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ pcl::gpu::VFHEstimation::Indices indices_gpu;
+ indices_gpu.upload(*source.indices);
+
+ gpu::VFHEstimation pc_gpu;
+ pc_gpu.setNormalizeBins(false);
+ pc_gpu.setSearchSurface(cloud_gpu);
+ pc_gpu.setInputNormals(normals_gpu);
+ pc_gpu.setIndices(indices_gpu);
+ pc_gpu.setRadiusSearch(source.radius, source.max_elements);
+
+ DeviceArray<VFHSignature308> vfh_features;
+ pc_gpu.compute(vfh_features);
+
+ vector<VFHSignature308> downloaded;
+ vfh_features.download(downloaded);
+
+ pcl::VFHEstimation<PointXYZ, Normal, VFHSignature308> fe;
+ fe.setNormalizeBins(false);
+ fe.setInputCloud (source.cloud);
+ fe.setInputNormals (source.normals);
+ fe.setIndices(source.indices);
+ fe.setRadiusSearch(source.radius);
+ fe.setKSearch(0);
+
+ PointCloud<VFHSignature308> vfh;
+ fe.compute (vfh);
+
+ VFHSignature308& gpu = downloaded[0];
+ VFHSignature308& cpu = vfh.points[0];
+
+ size_t FSize = sizeof(VFHSignature308)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+ if (norm != 0)
+ ASSERT_LE(norm_diff/norm, 0.01f/FSize);
+ else
+ FAIL();
+}
+
+//TEST(PCL_FeaturesGPU, DISABLED_vfh_norm_distance_true)
+TEST(PCL_FeaturesGPU, vfh_norm_distance_true)
+{
+ DataSource source;
+
+ source.estimateNormals();
+ source.generateIndices(3);
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(source.normals->points.begin(), source.normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+
+ pcl::gpu::VFHEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::VFHEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ pcl::gpu::VFHEstimation::Indices indices_gpu;
+ indices_gpu.upload(*source.indices);
+
+ gpu::VFHEstimation pc_gpu;
+ pc_gpu.setNormalizeDistance(true);
+ pc_gpu.setSearchSurface(cloud_gpu);
+ pc_gpu.setInputNormals(normals_gpu);
+ pc_gpu.setIndices(indices_gpu);
+ pc_gpu.setRadiusSearch(source.radius, source.max_elements);
+
+ DeviceArray<VFHSignature308> vfh_features;
+ pc_gpu.compute(vfh_features);
+
+ vector<VFHSignature308> downloaded;
+ vfh_features.download(downloaded);
+
+ pcl::VFHEstimation<PointXYZ, Normal, VFHSignature308> fe;
+ fe.setNormalizeDistance(true);
+ fe.setInputCloud (source.cloud);
+ fe.setInputNormals (source.normals);
+ fe.setIndices(source.indices);
+ fe.setRadiusSearch(source.radius);
+ fe.setKSearch(0);
+
+ PointCloud<VFHSignature308> vfh;
+ fe.compute (vfh);
+
+ VFHSignature308& gpu = downloaded[0];
+ VFHSignature308& cpu = vfh.points[0];
+
+ size_t FSize = sizeof(VFHSignature308)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+ if (norm != 0)
+ ASSERT_LE(norm_diff/norm, 0.01f/FSize);
+ else
+ FAIL();
+}
+
+
+//TEST(PCL_FeaturesGPU, DISABLED_vfh_fill_size_component_true)
+TEST(PCL_FeaturesGPU, vfh_fill_size_component_true)
+{
+ DataSource source;
+
+ source.estimateNormals();
+ source.generateIndices(3);
+
+ vector<PointXYZ> normals_for_gpu(source.normals->points.size());
+ std::transform(source.normals->points.begin(), source.normals->points.end(), normals_for_gpu.begin(), DataSource::Normal2PointXYZ());
+
+ //uploading data to GPU
+
+ //////////////////////////////////////////////////////////////////////////////////////////////
+
+ pcl::gpu::VFHEstimation::PointCloud cloud_gpu;
+ cloud_gpu.upload(source.cloud->points);
+
+ pcl::gpu::VFHEstimation::Normals normals_gpu;
+ normals_gpu.upload(normals_for_gpu);
+
+ pcl::gpu::VFHEstimation::Indices indices_gpu;
+ indices_gpu.upload(*source.indices);
+
+ gpu::VFHEstimation pc_gpu;
+ pc_gpu.setFillSizeComponent(true);
+ pc_gpu.setSearchSurface(cloud_gpu);
+ pc_gpu.setInputNormals(normals_gpu);
+ pc_gpu.setIndices(indices_gpu);
+ pc_gpu.setRadiusSearch(source.radius, source.max_elements);
+
+ DeviceArray<VFHSignature308> vfh_features;
+ pc_gpu.compute(vfh_features);
+
+ vector<VFHSignature308> downloaded;
+ vfh_features.download(downloaded);
+
+ pcl::VFHEstimation<PointXYZ, Normal, VFHSignature308> fe;
+ fe.setFillSizeComponent(true);
+ fe.setInputCloud (source.cloud);
+ fe.setInputNormals (source.normals);
+ fe.setIndices(source.indices);
+ fe.setRadiusSearch(source.radius);
+ fe.setKSearch(0);
+
+ PointCloud<VFHSignature308> vfh;
+ fe.compute (vfh);
+
+ VFHSignature308& gpu = downloaded[0];
+ VFHSignature308& cpu = vfh.points[0];
+
+ size_t FSize = sizeof(VFHSignature308)/sizeof(gpu.histogram[0]);
+
+ float norm = 0, norm_diff = 0;
+ for(size_t j = 0; j < FSize; ++j)
+ {
+ norm_diff += (gpu.histogram[j] - cpu.histogram[j]) * (gpu.histogram[j] - cpu.histogram[j]);
+ norm += cpu.histogram[j] * cpu.histogram[j];
+
+ //ASSERT_NEAR(gpu.histogram[j], cpu.histogram[j], 0.03f);
+ }
+ if (norm != 0)
+ ASSERT_LE(norm_diff/norm, 0.01f/FSize);
+ else
+ FAIL();
+}
\ No newline at end of file
--- /dev/null
+set(SUBSYS_NAME gpu_kinfu)
+set(SUBSYS_PATH gpu/kinfu)
+set(SUBSYS_DESC "Kinect Fusion implementation")
+set(SUBSYS_DEPS common io gpu_containers geometry search visualization)
+
+set(build FALSE)
+
+# OpenNI found?
+if(NOT WITH_OPENNI)
+ set(DEFAULT FALSE)
+ set(REASON "OpenNI was not found or was disabled by the user.")
+else()
+ set(DEFAULT TRUE)
+ set(REASON)
+endif()
+
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}")
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS})
+PCL_SET_SUBSYS_INCLUDE_DIR("${SUBSYS_NAME}" "${SUBSYS_PATH}")
+mark_as_advanced("BUILD_${SUBSYS_NAME}")
+
+if (build)
+ REMOVE_VTK_DEFINITIONS()
+ FILE(GLOB incs include/pcl/gpu/kinfu/*.h*)
+ FILE(GLOB srcs src/*.cpp src/*.h*)
+ FILE(GLOB cuda src/cuda/*.cu src/cuda/*.h*)
+ #FILE(GLOB tsdf src/cuda/tsdf_volume.cu)
+ #FILE(GLOB est src/cuda/estimate_tranform.cu)
+
+ source_group("Source Files\\cuda" FILES ${cuda} )
+ source_group("Source Files" FILES ${srcs} )
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include" "${CMAKE_CURRENT_SOURCE_DIR}/src" ${CUDA_INCLUDE_DIRS})
+
+ if (UNIX OR APPLE)
+ set (CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "-Xcompiler;-fPIC;")
+ endif()
+
+ if(NOT UNIX OR APPLE)
+ add_definitions(-DPCLAPI_EXPORTS)
+ endif()
+
+ #set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "-gencode;arch=compute_11,code=compute_11;-gencode;arch=compute_12,code=compute_12")
+
+ if(TRUE)
+ #list(REMOVE_ITEM cuda ${est})
+ #CUDA_COMPILE(est_objs ${est})
+
+ set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "--ftz=true;--prec-div=false;--prec-sqrt=false")
+ CUDA_COMPILE(cuda_objs ${cuda})
+
+ #LIST(APPEND cuda ${est})
+ #LIST(APPEND cuda_objs ${est_objs})
+
+ else()
+ list(REMOVE_ITEM cuda ${tsdf})
+ CUDA_COMPILE(cuda_objs ${cuda})
+ set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "--ftz=true;--prec-div=false;--prec-sqrt=false")
+ CUDA_COMPILE(tsdf_obj ${tsdf})
+
+ LIST(APPEND cuda ${tsdf})
+ LIST(APPEND cuda_objs ${tsdf_obj})
+ endif()
+
+ PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${cuda} ${cuda_objs})
+ target_link_libraries("${LIB_NAME}" pcl_gpu_containers)
+
+ set(EXT_DEPS "")
+ #set(EXT_DEPS CUDA)
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "")
+
+ # Install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}" ${incs})
+
+ if (BUILD_visualization)
+ add_subdirectory(test)
+ add_subdirectory(tools)
+ endif()
+endif()
+
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_KINFU_COLOR_VOLUME_H_
+#define PCL_KINFU_COLOR_VOLUME_H_
+
+#include <pcl/pcl_macros.h>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <Eigen/Core>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ class TsdfVolume;
+
+ /** \brief ColorVolume class
+ * \author Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+ class PCL_EXPORTS ColorVolume
+ {
+ public:
+ typedef PointXYZ PointType;
+ typedef boost::shared_ptr<ColorVolume> Ptr;
+
+ /** \brief Constructor
+ * \param[in] tsdf tsdf volume to get parameters from
+ * \param[in] max_weight max weight for running average. Can be less than 255. Negative means default.
+ */
+ ColorVolume(const TsdfVolume& tsdf, int max_weight = -1);
+
+ /** \brief Desctructor */
+ ~ColorVolume();
+
+ /** \brief Resets color volume to uninitialized state */
+ void
+ reset();
+
+ /** \brief Returns running average length */
+ int
+ getMaxWeight() const;
+
+ /** \brief Returns container with color volume in GPU memory */
+ DeviceArray2D<int>
+ data() const;
+
+ /** \brief Computes colors from color volume
+ * \param[in] cloud Points for which colors are to be computed.
+ * \param[out] colors output array for colors
+ */
+ void
+ fetchColors (const DeviceArray<PointType>& cloud, DeviceArray<RGB>& colors) const;
+
+ private:
+ /** \brief Volume resolution */
+ Eigen::Vector3i resolution_;
+
+ /** \brief Volume size in meters */
+ Eigen::Vector3f volume_size_;
+
+ /** \brief Length of running average */
+ int max_weight_;
+
+ /** \brief color volume data */
+ DeviceArray2D<int> color_volume_;
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ };
+ }
+}
+
+#endif /* PCL_KINFU_COLOR_VOLUME_H_ */
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_KINFU_KINFUTRACKER_HPP_
+#define PCL_KINFU_KINFUTRACKER_HPP_
+
+#include <pcl/pcl_macros.h>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/kinfu/pixel_rgb.h>
+#include <pcl/gpu/kinfu/tsdf_volume.h>
+#include <pcl/gpu/kinfu/color_volume.h>
+#include <pcl/gpu/kinfu/raycaster.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <Eigen/Core>
+#include <vector>
+
+// Focal lengths of RGB camera
+#define KINFU_DEFAULT_RGB_FOCAL_X 525.f
+#define KINFU_DEFAULT_RGB_FOCAL_Y 525.f
+
+// Focal lengths of depth (i.e. NIR) camera
+#define KINFU_DEFAULT_DEPTH_FOCAL_X 585.f
+#define KINFU_DEFAULT_DEPTH_FOCAL_Y 585.f
+
+namespace pcl
+{
+ namespace gpu
+ {
+ /** \brief KinfuTracker class encapsulates implementation of Microsoft Kinect Fusion algorithm
+ * \author Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+ class PCL_EXPORTS KinfuTracker
+ {
+ public:
+ /** \brief Pixel type for rendered image. */
+ typedef pcl::gpu::PixelRGB PixelRGB;
+
+ typedef DeviceArray2D<PixelRGB> View;
+ typedef DeviceArray2D<unsigned short> DepthMap;
+
+ typedef pcl::PointXYZ PointType;
+ typedef pcl::Normal NormalType;
+
+ /** \brief Constructor
+ * \param[in] rows height of depth image
+ * \param[in] cols width of depth image
+ */
+ KinfuTracker (int rows = 480, int cols = 640);
+
+ /** \brief Sets Depth camera intrinsics
+ * \param[in] fx focal length x
+ * \param[in] fy focal length y
+ * \param[in] cx principal point x
+ * \param[in] cy principal point y
+ */
+ void
+ setDepthIntrinsics (float fx, float fy, float cx = -1, float cy = -1);
+
+ /** \brief Get Depth camera intrinsics
+ * \param[out] fx focal length x
+ * \param[out] fy focal length y
+ * \param[out] cx principal point x
+ * \param[out] cy principal point y
+ */
+ void
+ getDepthIntrinsics (float& fx, float& fy, float& cx, float& cy);
+
+
+ /** \brief Sets initial camera pose relative to volume coordiante space
+ * \param[in] pose Initial camera pose
+ */
+ void
+ setInitalCameraPose (const Eigen::Affine3f& pose);
+
+ /** \brief Sets truncation threshold for depth image for ICP step only! This helps
+ * to filter measurements that are outside tsdf volume. Pass zero to disable the truncation.
+ * \param[in] max_icp_distance Maximal distance, higher values are reset to zero (means no measurement).
+ */
+ void
+ setDepthTruncationForICP (float max_icp_distance = 0.f);
+
+ /** \brief Sets ICP filtering parameters.
+ * \param[in] distThreshold distance.
+ * \param[in] sineOfAngle sine of angle between normals.
+ */
+ void
+ setIcpCorespFilteringParams (float distThreshold, float sineOfAngle);
+
+ /** \brief Sets integration threshold. TSDF volume is integrated iff a camera movement metric exceedes the threshold value.
+ * The metric represents the following: M = (rodrigues(Rotation).norm() + alpha*translation.norm())/2, where alpha = 1.f (hardcoded constant)
+ * \param[in] threshold a value to compare with the metric. Suitable values are ~0.001
+ */
+ void
+ setCameraMovementThreshold(float threshold = 0.001f);
+
+ /** \brief Performs initialization for color integration. Must be called before calling color integration.
+ * \param[in] max_weight max weighe for color integration. -1 means default weight.
+ */
+ void
+ initColorIntegration(int max_weight = -1);
+
+ /** \brief Returns cols passed to ctor */
+ int
+ cols ();
+
+ /** \brief Returns rows passed to ctor */
+ int
+ rows ();
+
+ /** \brief Processes next frame.
+ * \param[in] depth next frame with values in millimeters
+ * \param hint
+ * \return true if can render 3D view.
+ */
+ bool operator() (const DepthMap& depth, Eigen::Affine3f* hint=NULL);
+
+ /** \brief Processes next frame (both depth and color integration). Please call initColorIntegration before invpoking this.
+ * \param[in] depth next depth frame with values in millimeters
+ * \param[in] colors next RGB frame
+ * \return true if can render 3D view.
+ */
+ bool operator() (const DepthMap& depth, const View& colors);
+
+ /** \brief Returns camera pose at given time, default the last pose
+ * \param[in] time Index of frame for which camera pose is returned.
+ * \return camera pose
+ */
+ Eigen::Affine3f
+ getCameraPose (int time = -1) const;
+
+ /** \brief Returns number of poses including initial */
+ size_t
+ getNumberOfPoses () const;
+
+ /** \brief Returns TSDF volume storage */
+ const TsdfVolume& volume() const;
+
+ /** \brief Returns TSDF volume storage */
+ TsdfVolume& volume();
+
+ /** \brief Returns color volume storage */
+ const ColorVolume& colorVolume() const;
+
+ /** \brief Returns color volume storage */
+ ColorVolume& colorVolume();
+
+ /** \brief Renders 3D scene to display to human
+ * \param[out] view output array with image
+ */
+ void
+ getImage (View& view) const;
+
+ /** \brief Returns point cloud abserved from last camera pose
+ * \param[out] cloud output array for points
+ */
+ void
+ getLastFrameCloud (DeviceArray2D<PointType>& cloud) const;
+
+ /** \brief Returns point cloud abserved from last camera pose
+ * \param[out] normals output array for normals
+ */
+ void
+ getLastFrameNormals (DeviceArray2D<NormalType>& normals) const;
+
+ /** \brief Disables ICP forever */
+ void disableIcp();
+
+ private:
+
+ /** \brief Number of pyramid levels */
+ enum { LEVELS = 3 };
+
+ /** \brief ICP Correspondences map type */
+ typedef DeviceArray2D<int> CorespMap;
+
+ /** \brief Vertex or Normal Map type */
+ typedef DeviceArray2D<float> MapArr;
+
+ typedef Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Matrix3frm;
+ typedef Eigen::Vector3f Vector3f;
+
+ /** \brief Height of input depth image. */
+ int rows_;
+ /** \brief Width of input depth image. */
+ int cols_;
+ /** \brief Frame counter */
+ int global_time_;
+
+ /** \brief Truncation threshold for depth image for ICP step */
+ float max_icp_distance_;
+
+ /** \brief Intrinsic parameters of depth camera. */
+ float fx_, fy_, cx_, cy_;
+
+ /** \brief Tsdf volume container. */
+ TsdfVolume::Ptr tsdf_volume_;
+ ColorVolume::Ptr color_volume_;
+
+ /** \brief Initial camera rotation in volume coo space. */
+ Matrix3frm init_Rcam_;
+
+ /** \brief Initial camera position in volume coo space. */
+ Vector3f init_tcam_;
+
+ /** \brief array with IPC iteration numbers for each pyramid level */
+ int icp_iterations_[LEVELS];
+ /** \brief distance threshold in correspondences filtering */
+ float distThres_;
+ /** \brief angle threshold in correspondences filtering. Represents max sine of angle between normals. */
+ float angleThres_;
+
+ /** \brief Depth pyramid. */
+ std::vector<DepthMap> depths_curr_;
+ /** \brief Vertex maps pyramid for current frame in global coordinate space. */
+ std::vector<MapArr> vmaps_g_curr_;
+ /** \brief Normal maps pyramid for current frame in global coordinate space. */
+ std::vector<MapArr> nmaps_g_curr_;
+
+ /** \brief Vertex maps pyramid for previous frame in global coordinate space. */
+ std::vector<MapArr> vmaps_g_prev_;
+ /** \brief Normal maps pyramid for previous frame in global coordinate space. */
+ std::vector<MapArr> nmaps_g_prev_;
+
+ /** \brief Vertex maps pyramid for current frame in current coordinate space. */
+ std::vector<MapArr> vmaps_curr_;
+ /** \brief Normal maps pyramid for current frame in current coordinate space. */
+ std::vector<MapArr> nmaps_curr_;
+
+ /** \brief Array of buffers with ICP correspondences for each pyramid level. */
+ std::vector<CorespMap> coresps_;
+
+ /** \brief Buffer for storing scaled depth image */
+ DeviceArray2D<float> depthRawScaled_;
+
+ /** \brief Temporary buffer for ICP */
+ DeviceArray2D<double> gbuf_;
+ /** \brief Buffer to store MLS matrix. */
+ DeviceArray<double> sumbuf_;
+
+ /** \brief Array of camera rotation matrices for each moment of time. */
+ std::vector<Matrix3frm> rmats_;
+
+ /** \brief Array of camera translations for each moment of time. */
+ std::vector<Vector3f> tvecs_;
+
+ /** \brief Camera movement threshold. TSDF is integrated iff a camera movement metric exceedes some value. */
+ float integration_metric_threshold_;
+
+ /** \brief ICP step is completelly disabled. Inly integratio now */
+ bool disable_icp_;
+
+ /** \brief Allocates all GPU internal buffers.
+ * \param[in] rows_arg
+ * \param[in] cols_arg
+ */
+ void
+ allocateBufffers (int rows_arg, int cols_arg);
+
+ /** \brief Performs the tracker reset to initial state. It's used if case of camera tracking fail.
+ */
+ void
+ reset ();
+
+public:
+EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ };
+ }
+};
+
+#endif /* PCL_KINFU_KINFUTRACKER_HPP_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_KINFU_TSDF_MARCHING_CUBES_H_
+#define PCL_KINFU_TSDF_MARCHING_CUBES_H_
+
+#include <pcl/pcl_macros.h>
+#include <pcl/gpu/containers/device_array.h>
+#include <Eigen/Core>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ class TsdfVolume;
+
+ /** \brief MarchingCubes implements MarchingCubes functionality for TSDF volume on GPU
+ * \author Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+ class PCL_EXPORTS MarchingCubes
+ {
+ public:
+
+ /** \brief Default size for triangles buffer */
+ enum
+ {
+ POINTS_PER_TRIANGLE = 3,
+ DEFAULT_TRIANGLES_BUFFER_SIZE = 2 * 1000 * 1000 * POINTS_PER_TRIANGLE
+ };
+
+ /** \brief Point type. */
+ typedef pcl::PointXYZ PointType;
+
+ /** \brief Smart pointer. */
+ typedef boost::shared_ptr<MarchingCubes> Ptr;
+
+ /** \brief Default constructor */
+ MarchingCubes();
+
+ /** \brief Destructor */
+ ~MarchingCubes();
+
+ /** \brief Runs marching cubes triangulation.
+ * \param[in] tsdf
+ * \param[in] triangles_buffer Buffer for triangles. Its size determines max extracted triangles. If empty, it will be allocated with default size will be used.
+ * \return Array with triangles. Each 3 consequent poits belond to a single triangle. The returned array points to 'triangles_buffer' data.
+ */
+ DeviceArray<PointType>
+ run(const TsdfVolume& tsdf, DeviceArray<PointType>& triangles_buffer);
+
+ private:
+ /** \brief Edge table for marching cubes */
+ DeviceArray<int> edgeTable_;
+
+ /** \brief Number of vertextes table for marching cubes */
+ DeviceArray<int> numVertsTable_;
+
+ /** \brief Triangles table for marching cubes */
+ DeviceArray<int> triTable_;
+
+ /** \brief Temporary buffer used by marching cubes (first row stores occuped voxes id, second number of vetexes, third poits offsets */
+ DeviceArray2D<int> occupied_voxels_buffer_;
+ };
+ }
+}
+
+#endif /* PCL_KINFU_MARCHING_CUBES_H_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_KINFU_PIXEL_RGB_HPP_
+#define PCL_KINFU_PIXEL_RGB_HPP_
+
+namespace pcl
+{
+ namespace gpu
+ {
+ /** \brief Input/output pixel format for KinfuTracker */
+
+ struct PixelRGB
+ {
+ unsigned char r, g, b;
+ };
+ }
+}
+
+#endif /* PCL_KINFU_PIXEL_RGB_HPP_ */
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+
+#ifndef PCL_KINFU_TSDF_RAYCASTER_H_
+#define PCL_KINFU_TSDF_RAYCASTER_H_
+
+
+#include <pcl/pcl_macros.h>
+#include <pcl/point_types.h>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/kinfu/pixel_rgb.h>
+#include <boost/shared_ptr.hpp>
+#include <Eigen/Geometry>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ class TsdfVolume;
+
+ /** \brief Class that performs raycasting for TSDF volume
+ * \author Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+ struct PCL_EXPORTS RayCaster
+ {
+ public:
+ typedef boost::shared_ptr<RayCaster> Ptr;
+ typedef DeviceArray2D<float> MapArr;
+ typedef DeviceArray2D<PixelRGB> View;
+ typedef DeviceArray2D<unsigned short> Depth;
+
+ /** \brief Image with height */
+ const int cols, rows;
+
+ /** \brief Constructor
+ * \param[in] rows image rows
+ * \param[in] cols image cols
+ * \param[in] fx focal x
+ * \param[in] fy focal y
+ * \param[in] cx principal point x
+ * \param[in] cy principal point y
+ */
+ RayCaster(int rows = 480, int cols = 640, float fx = 525.f, float fy = 525.f, float cx = -1, float cy = -1);
+ ~RayCaster();
+
+ /** \brief Sets camera intrinsics */
+ void
+ setIntrinsics(float fx = 525.f, float fy = 525.f, float cx = -1, float cy = -1);
+
+ /** \brief Runs raycasting algorithm from given camera pose. It writes results to internal fiels.
+ * \param[in] volume tsdf volume container
+ * \param[in] camera_pose camera pose
+ */
+ void
+ run(const TsdfVolume& volume, const Eigen::Affine3f& camera_pose);
+
+ /** \brief Generates scene view using data raycasted by run method. So call it before.
+ * \param[out] view output array for RGB image
+ */
+ void
+ generateSceneView(View& view) const;
+
+ /** \brief Generates scene view using data raycasted by run method. So call it before.
+ * \param[out] view output array for RGB image
+ * \param[in] light_source_pose pose of light source
+ */
+ void
+ generateSceneView(View& view, const Eigen::Vector3f& light_source_pose) const;
+
+ /** \brief Generates depth image using data raycasted by run method. So call it before.
+ * \param[out] depth output array for depth image
+ */
+ void
+ generateDepthImage(Depth& depth) const;
+
+ /** \brief Returns raycasterd vertex map. */
+ MapArr
+ getVertexMap() const;
+
+ /** \brief Returns raycasterd normal map. */
+ MapArr
+ getNormalMap() const;
+
+ private:
+ /** \brief Camera intrinsics. */
+ float fx_, fy_, cx_, cy_;
+
+ /* Vertext/normal map internal representation example for rows=2 and cols=4
+ * X X X X
+ * X X X X
+ * Y Y Y Y
+ * Y Y Y Y
+ * Z Z Z Z
+ * Z Z Z Z
+ */
+
+ /** \brief vertex map of 3D points*/
+ MapArr vertex_map_;
+
+ /** \brief normal map of 3D points*/
+ MapArr normal_map_;
+
+ /** \brief camera pose from which raycasting was done */
+ Eigen::Affine3f camera_pose_;
+
+ /** \brief Last passed volume size */
+ Eigen::Vector3f volume_size_;
+
+public:
+EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+
+ /** \brief Converts from map representation to organized not-dence point cloud. */
+ template<typename PointType>
+ void convertMapToOranizedCloud(const RayCaster::MapArr& map, DeviceArray2D<PointType>& cloud);
+ }
+}
+
+#endif /* PCL_KINFU_TSDF_RAYCASTER_H_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_KINFU_TSDF_VOLUME_H_
+#define PCL_KINFU_TSDF_VOLUME_H_
+
+#include <pcl/pcl_macros.h>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <Eigen/Core>
+#include <vector>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ /** \brief TsdfVolume class
+ * \author Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+ class PCL_EXPORTS TsdfVolume
+ {
+ public:
+ typedef boost::shared_ptr<TsdfVolume> Ptr;
+
+ /** \brief Supported Point Types */
+ typedef PointXYZ PointType;
+ typedef Normal NormalType;
+
+ /** \brief Default buffer size for fetching cloud. It limits max number of points that can be extracted */
+ enum { DEFAULT_CLOUD_BUFFER_SIZE = 10 * 1000 * 1000 };
+
+ /** \brief Constructor
+ * \param[in] resolution volume resolution
+ */
+ TsdfVolume(const Eigen::Vector3i& resolution);
+
+ /** \brief Sets Tsdf volume size for each dimention
+ * \param[in] size size of tsdf volume in meters
+ */
+ void
+ setSize(const Eigen::Vector3f& size);
+
+ /** \brief Sets Tsdf truncation distance. Must be greater than 2 * volume_voxel_size
+ * \param[in] distance TSDF truncation distance
+ */
+ void
+ setTsdfTruncDist (float distance);
+
+ /** \brief Returns tsdf volume container that point to data in GPU memroy */
+ DeviceArray2D<int>
+ data() const;
+
+ /** \brief Returns volume size in meters */
+ const Eigen::Vector3f&
+ getSize() const;
+
+ /** \brief Returns volume resolution */
+ const Eigen::Vector3i&
+ getResolution() const;
+
+ /** \brief Returns volume voxel size in meters */
+ const Eigen::Vector3f
+ getVoxelSize() const;
+
+ /** \brief Returns tsdf truncation distance in meters */
+ float
+ getTsdfTruncDist () const;
+
+ /** \brief Resets tsdf volume data to uninitialized state */
+ void
+ reset();
+
+ /** \brief Generates cloud using CPU (downloads volumetric representation to CPU memory)
+ * \param[out] cloud output array for cloud
+ * \param[in] connected26 If false point cloud is extracted using 6 neighbor, otherwise 26.
+ */
+ void
+ fetchCloudHost (PointCloud<PointType>& cloud, bool connected26 = false) const;
+
+ /** \brief Generates cloud using GPU in connected6 mode only
+ * \param[out] cloud_buffer buffer to store point cloud
+ * \return DeviceArray with disabled reference counting that points to filled part of cloud_buffer.
+ */
+ DeviceArray<PointType>
+ fetchCloud (DeviceArray<PointType>& cloud_buffer) const;
+
+ /** \brief Computes normals as gradient of tsdf for given points
+ * \param[in] cloud Points where normals are computed.
+ * \param[out] normals array for normals
+ */
+ void
+ fetchNormals (const DeviceArray<PointType>& cloud, DeviceArray<PointType>& normals) const;
+
+ /** \brief Computes normals as gradient of tsdf for given points
+ * \param[in] cloud Points where normals are computed.
+ * \param[out] normals array for normals
+ */
+ void
+ fetchNormals(const DeviceArray<PointType>& cloud, DeviceArray<NormalType>& normals) const;
+
+ /** \brief Downloads tsdf volume from GPU memory.
+ * \param[out] tsdf Array with tsdf values. if volume resolution is 512x512x512, so for voxel (x,y,z) tsdf value can be retrieved as volume[512*512*z + 512*y + x];
+ */
+ void
+ downloadTsdf (std::vector<float>& tsdf) const;
+
+ /** \brief Downloads TSDF volume and according voxel weights from GPU memory
+ * \param[out] tsdf Array with tsdf values. if volume resolution is 512x512x512, so for voxel (x,y,z) tsdf value can be retrieved as volume[512*512*z + 512*y + x];
+ * \param[out] weights Array with tsdf voxel weights. Same size and access index as for tsdf. A weight of 0 indicates the voxel was never used.
+ */
+ void
+ downloadTsdfAndWeighs(std::vector<float>& tsdf, std::vector<short>& weights) const;
+
+ private:
+ /** \brief tsdf volume size in meters */
+ Eigen::Vector3f size_;
+
+ /** \brief tsdf volume resolution */
+ Eigen::Vector3i resolution_;
+
+ /** \brief tsdf volume data container */
+ DeviceArray2D<int> volume_;
+
+ /** \brief tsdf truncation distance */
+ float tranc_dist_;
+
+public:
+EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ };
+ }
+}
+
+#endif /* PCL_KINFU_TSDF_VOLUME_H_ */
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/gpu/kinfu/color_volume.h>
+#include <pcl/gpu/kinfu/tsdf_volume.h>
+#include "internal.h"
+#include <algorithm>
+#include <Eigen/Core>
+
+using namespace pcl;
+using namespace pcl::gpu;
+using namespace Eigen;
+using pcl::device::device_cast;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+pcl::gpu::ColorVolume::ColorVolume(const TsdfVolume& tsdf, int max_weight) : resolution_(tsdf.getResolution()), volume_size_(tsdf.getSize()), max_weight_(1)
+{
+ max_weight_ = max_weight < 0 ? max_weight_ : max_weight;
+ max_weight_ = max_weight_ > 255 ? 255 : max_weight_;
+
+ int volume_x = resolution_(0);
+ int volume_y = resolution_(1);
+ int volume_z = resolution_(2);
+
+ color_volume_.create (volume_y * volume_z, volume_x);
+ reset();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+pcl::gpu::ColorVolume::~ColorVolume()
+{
+
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::ColorVolume::reset()
+{
+ device::initColorVolume(color_volume_);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+int
+pcl::gpu::ColorVolume::getMaxWeight() const
+{
+ return max_weight_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+DeviceArray2D<int>
+pcl::gpu::ColorVolume::data() const
+{
+ return color_volume_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::ColorVolume::fetchColors (const DeviceArray<PointType>& cloud, DeviceArray<RGB>& colors) const
+{
+ colors.create(cloud.size());
+ device::exctractColors(color_volume_, device_cast<const float3> (volume_size_), cloud, (uchar4*)colors.ptr()/*bgra*/);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+
+namespace pcl
+{
+ namespace device
+ {
+ const float sigma_color = 30; //in mm
+ const float sigma_space = 4.5; // in pixels
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ __global__ void
+ bilateralKernel (const PtrStepSz<ushort> src,
+ PtrStep<ushort> dst,
+ float sigma_space2_inv_half, float sigma_color2_inv_half)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x >= src.cols || y >= src.rows)
+ return;
+
+ const int R = 6; //static_cast<int>(sigma_space * 1.5);
+ const int D = R * 2 + 1;
+
+ int value = src.ptr (y)[x];
+
+ int tx = min (x - D / 2 + D, src.cols - 1);
+ int ty = min (y - D / 2 + D, src.rows - 1);
+
+ float sum1 = 0;
+ float sum2 = 0;
+
+ for (int cy = max (y - D / 2, 0); cy < ty; ++cy)
+ {
+ for (int cx = max (x - D / 2, 0); cx < tx; ++cx)
+ {
+ int tmp = src.ptr (cy)[cx];
+
+ float space2 = (x - cx) * (x - cx) + (y - cy) * (y - cy);
+ float color2 = (value - tmp) * (value - tmp);
+
+ float weight = __expf (-(space2 * sigma_space2_inv_half + color2 * sigma_color2_inv_half));
+
+ sum1 += tmp * weight;
+ sum2 += weight;
+ }
+ }
+
+ int res = __float2int_rn (sum1 / sum2);
+ dst.ptr (y)[x] = max (0, min (res, numeric_limits<short>::max ()));
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ __global__ void
+ pyrDownGaussKernel (const PtrStepSz<ushort> src, PtrStepSz<ushort> dst, float sigma_color)
+ {
+ int x = blockIdx.x * blockDim.x + threadIdx.x;
+ int y = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if (x >= dst.cols || y >= dst.rows)
+ return;
+
+ const int D = 5;
+
+ int center = src.ptr (2 * y)[2 * x];
+
+ int x_mi = max(0, 2*x - D/2) - 2*x;
+ int y_mi = max(0, 2*y - D/2) - 2*y;
+
+ int x_ma = min(src.cols, 2*x -D/2+D) - 2*x;
+ int y_ma = min(src.rows, 2*y -D/2+D) - 2*y;
+
+ float sum = 0;
+ float wall = 0;
+
+ float weights[] = {0.375f, 0.25f, 0.0625f} ;
+
+ for(int yi = y_mi; yi < y_ma; ++yi)
+ for(int xi = x_mi; xi < x_ma; ++xi)
+ {
+ int val = src.ptr (2*y + yi)[2*x + xi];
+
+ if (abs (val - center) < 3 * sigma_color)
+ {
+ sum += val * weights[abs(xi)] * weights[abs(yi)];
+ wall += weights[abs(xi)] * weights[abs(yi)];
+ }
+ }
+
+
+ dst.ptr (y)[x] = static_cast<int>(sum /wall);
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ __global__ void
+ pyrDownKernel (const PtrStepSz<ushort> src, PtrStepSz<ushort> dst, float sigma_color)
+ {
+ int x = blockIdx.x * blockDim.x + threadIdx.x;
+ int y = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if (x >= dst.cols || y >= dst.rows)
+ return;
+
+ const int D = 5;
+
+ int center = src.ptr (2 * y)[2 * x];
+
+ int tx = min (2 * x - D / 2 + D, src.cols - 1);
+ int ty = min (2 * y - D / 2 + D, src.rows - 1);
+ int cy = max (0, 2 * y - D / 2);
+
+ int sum = 0;
+ int count = 0;
+
+ for (; cy < ty; ++cy)
+ for (int cx = max (0, 2 * x - D / 2); cx < tx; ++cx)
+ {
+ int val = src.ptr (cy)[cx];
+ if (abs (val - center) < 3 * sigma_color)
+ {
+ sum += val;
+ ++count;
+ }
+ }
+ dst.ptr (y)[x] = sum / count;
+ }
+
+ __global__ void
+ truncateDepthKernel(PtrStepSz<ushort> depth, ushort max_distance_mm)
+ {
+ int x = blockIdx.x * blockDim.x + threadIdx.x;
+ int y = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if (x < depth.cols && y < depth.rows)
+ if(depth.ptr(y)[x] > max_distance_mm)
+ depth.ptr(y)[x] = 0;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::device::bilateralFilter (const DepthMap& src, DepthMap& dst)
+{
+ dim3 block (32, 8);
+ dim3 grid (divUp (src.cols (), block.x), divUp (src.rows (), block.y));
+
+ cudaFuncSetCacheConfig (bilateralKernel, cudaFuncCachePreferL1);
+ bilateralKernel<<<grid, block>>>(src, dst, 0.5f / (sigma_space * sigma_space), 0.5f / (sigma_color * sigma_color));
+
+ cudaSafeCall ( cudaGetLastError () );
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::device::pyrDown (const DepthMap& src, DepthMap& dst)
+{
+ dst.create (src.rows () / 2, src.cols () / 2);
+
+ dim3 block (32, 8);
+ dim3 grid (divUp (dst.cols (), block.x), divUp (dst.rows (), block.y));
+
+ //pyrDownGaussKernel<<<grid, block>>>(src, dst, sigma_color);
+ pyrDownKernel<<<grid, block>>>(src, dst, sigma_color);
+ cudaSafeCall ( cudaGetLastError () );
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::device::truncateDepth(DepthMap& depth, float max_distance)
+{
+ dim3 block (32, 8);
+ dim3 grid (divUp (depth.cols (), block.x), divUp (depth.rows (), block.y));
+
+ truncateDepthKernel<<<grid, block>>>(depth, static_cast<ushort>(max_distance * 1000.f));
+
+ cudaSafeCall ( cudaGetLastError () );
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+
+//#include "pcl/gpu/utils/device/vector_math.hpp"
+
+namespace pcl
+{
+ namespace device
+ {
+ __global__ void
+ initColorVolumeKernel (PtrStep<uchar4> volume)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x < VOLUME_X && y < VOLUME_Y)
+ {
+ uchar4 *pos = volume.ptr (y) + x;
+ int z_step = VOLUME_Y * volume.step / sizeof(*pos);
+
+#pragma unroll
+ for (int z = 0; z < VOLUME_Z; ++z, pos += z_step)
+ *pos = make_uchar4 (0, 0, 0, 0);
+ }
+ }
+ }
+}
+
+void
+pcl::device::initColorVolume (PtrStep<uchar4> color_volume)
+{
+ dim3 block (32, 16);
+ dim3 grid (1, 1, 1);
+ grid.x = divUp (VOLUME_X, block.x);
+ grid.y = divUp (VOLUME_Y, block.y);
+
+ initColorVolumeKernel<<<grid, block>>>(color_volume);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+}
+
+namespace pcl
+{
+ namespace device
+ {
+ struct ColorVolumeImpl
+ {
+ enum
+ {
+ CTA_SIZE_X = 32,
+ CTA_SIZE_Y = 8,
+
+ ONE_VOXEL = 0
+ };
+
+ Intr intr;
+
+ PtrStep<float> vmap;
+ PtrStepSz<uchar3> colors;
+
+ Mat33 R_inv;
+ float3 t;
+
+ float3 cell_size;
+ float tranc_dist;
+
+ int max_weight;
+
+ mutable PtrStep<uchar4> color_volume;
+
+ __device__ __forceinline__ int3
+ getVoxel (float3 point) const
+ {
+ int vx = __float2int_rd (point.x / cell_size.x); // round to negative infinity
+ int vy = __float2int_rd (point.y / cell_size.y);
+ int vz = __float2int_rd (point.z / cell_size.z);
+
+ return make_int3 (vx, vy, vz);
+ }
+
+ __device__ __forceinline__ float3
+ getVoxelGCoo (int x, int y, int z) const
+ {
+ float3 coo = make_float3 (x, y, z);
+ coo += 0.5f; //shift to cell center;
+
+ coo.x *= cell_size.x;
+ coo.y *= cell_size.y;
+ coo.z *= cell_size.z;
+
+ return coo;
+ }
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x >= VOLUME_X || y >= VOLUME_Y)
+ return;
+
+ for (int z = 0; z < VOLUME_Z; ++z)
+ {
+ float3 v_g = getVoxelGCoo (x, y, z);
+
+ float3 v = R_inv * (v_g - t);
+
+ if (v.z <= 0)
+ continue;
+
+ int2 coo; //project to current cam
+ coo.x = __float2int_rn (v.x * intr.fx / v.z + intr.cx);
+ coo.y = __float2int_rn (v.y * intr.fy / v.z + intr.cy);
+
+ if (coo.x >= 0 && coo.y >= 0 && coo.x < colors.cols && coo.y < colors.rows)
+ {
+ float3 p;
+ p.x = vmap.ptr (coo.y)[coo.x];
+
+ if (isnan (p.x))
+ continue;
+
+ p.y = vmap.ptr (coo.y + colors.rows )[coo.x];
+ p.z = vmap.ptr (coo.y + colors.rows * 2)[coo.x];
+
+ bool update = false;
+ if (ONE_VOXEL)
+ {
+ int3 vp = getVoxel (p);
+ update = vp.x == x && vp.y == y && vp.z == z;
+ }
+ else
+ {
+ float dist = norm (p - v_g);
+ update = dist < tranc_dist;
+ }
+
+ if (update)
+ {
+ uchar4 *ptr = color_volume.ptr (VOLUME_Y * z + y) + x;
+ uchar3 rgb = colors.ptr (coo.y)[coo.x];
+ uchar4 volume_rgbw = *ptr;
+
+ int weight_prev = volume_rgbw.w;
+
+ const float Wrk = 1.f;
+ float new_x = (volume_rgbw.x * weight_prev + Wrk * rgb.x) / (weight_prev + Wrk);
+ float new_y = (volume_rgbw.y * weight_prev + Wrk * rgb.y) / (weight_prev + Wrk);
+ float new_z = (volume_rgbw.z * weight_prev + Wrk * rgb.z) / (weight_prev + Wrk);
+
+ int weight_new = weight_prev + 1;
+
+ uchar4 volume_rgbw_new;
+ volume_rgbw_new.x = min (255, max (0, __float2int_rn (new_x)));
+ volume_rgbw_new.y = min (255, max (0, __float2int_rn (new_y)));
+ volume_rgbw_new.z = min (255, max (0, __float2int_rn (new_z)));
+ volume_rgbw_new.w = min (max_weight, weight_new);
+
+ *ptr = volume_rgbw_new;
+ }
+ } /* in camera image range */
+ } /* for(int z = 0; z < VOLUME_X; ++z) */
+ } /* void operator() */
+ };
+
+ __global__ void
+ updateColorVolumeKernel (const ColorVolumeImpl cvi) {
+ cvi ();
+ }
+ }
+}
+
+void
+pcl::device::updateColorVolume (const Intr& intr, float tranc_dist, const Mat33& R_inv, const float3& t,
+ const MapArr& vmap, const PtrStepSz<uchar3>& colors, const float3& volume_size, PtrStep<uchar4> color_volume, int max_weight)
+{
+ ColorVolumeImpl cvi;
+ cvi.vmap = vmap;
+ cvi.colors = colors;
+ cvi.color_volume = color_volume;
+
+ cvi.R_inv = R_inv;
+ cvi.t = t;
+ cvi.intr = intr;
+ cvi.tranc_dist = tranc_dist;
+ cvi.max_weight = min (max (0, max_weight), 255);
+
+ cvi.cell_size.x = volume_size.x / VOLUME_X;
+ cvi.cell_size.y = volume_size.y / VOLUME_Y;
+ cvi.cell_size.z = volume_size.z / VOLUME_Z;
+
+ dim3 block (ColorVolumeImpl::CTA_SIZE_X, ColorVolumeImpl::CTA_SIZE_Y);
+ dim3 grid (divUp (VOLUME_X, block.x), divUp (VOLUME_Y, block.y));
+
+ updateColorVolumeKernel<<<grid, block>>>(cvi);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+}
+
+namespace pcl
+{
+ namespace device
+ {
+ __global__ void
+ extractColorsKernel (const float3 cell_size, const PtrStep<uchar4> color_volume, const PtrSz<PointType> points, uchar4 *colors)
+ {
+ int idx = threadIdx.x + blockIdx.x * blockDim.x;
+
+ if (idx < points.size)
+ {
+ int3 v;
+ float3 p = *(const float3*)(points.data + idx);
+ v.x = __float2int_rd (p.x / cell_size.x); // round to negative infinity
+ v.y = __float2int_rd (p.y / cell_size.y);
+ v.z = __float2int_rd (p.z / cell_size.z);
+
+ uchar4 rgbw = color_volume.ptr (VOLUME_Y * v.z + v.y)[v.x];
+ colors[idx] = make_uchar4 (rgbw.z, rgbw.y, rgbw.x, 0); //bgra
+ }
+ }
+ }
+}
+
+void
+pcl::device::exctractColors (const PtrStep<uchar4>& color_volume, const float3& volume_size, const PtrSz<PointType>& points, uchar4* colors)
+{
+ const int block = 256;
+ float3 cell_size = make_float3 (volume_size.x / VOLUME_X, volume_size.y / VOLUME_Y, volume_size.z / VOLUME_Z);
+ extractColorsKernel<<<divUp (points.size, block), block>>>(cell_size, color_volume, points, colors);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+};
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+//#include <pcl/gpu/utils/device/block.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+ __device__ unsigned int count = 0;
+
+ struct CorespSearch
+ {
+ enum { CTA_SIZE_X = 32, CTA_SIZE_Y = 8, CTA_SIZE = CTA_SIZE_X * CTA_SIZE_Y };
+
+ struct plus
+ {
+ __forceinline__ __device__ int
+ operator () (const int &lhs, const volatile int& rhs) const {
+ return lhs + rhs;
+ }
+ };
+
+ PtrStep<float> vmap_g_curr;
+ PtrStep<float> nmap_g_curr;
+
+ Mat33 Rprev_inv;
+ float3 tprev;
+
+ Intr intr;
+
+ PtrStep<float> vmap_g_prev;
+ PtrStep<float> nmap_g_prev;
+
+ float distThres;
+ float angleThres;
+
+ mutable PtrStepSz<short2> coresp;
+
+ mutable int* gbuf;
+
+ __device__ __forceinline__ int
+ search () const
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x >= coresp.cols || y >= coresp.rows)
+ return 0;
+
+ coresp.ptr (y)[x] = make_short2 (-1, -1);
+
+ float3 ncurr_g;
+ ncurr_g.x = nmap_g_curr.ptr (y)[x];
+
+ if (isnan (ncurr_g.x))
+ return 0;
+
+ float3 vcurr_g;
+ vcurr_g.x = vmap_g_curr.ptr (y )[x];
+ vcurr_g.y = vmap_g_curr.ptr (y + coresp.rows)[x];
+ vcurr_g.z = vmap_g_curr.ptr (y + 2 * coresp.rows)[x];
+
+ float3 vcurr_cp = Rprev_inv * (vcurr_g - tprev); // prev camera coo space
+
+ int2 ukr; //projection
+ ukr.x = __float2int_rn (vcurr_cp.x * intr.fx / vcurr_cp.z + intr.cx); //4
+ ukr.y = __float2int_rn (vcurr_cp.y * intr.fy / vcurr_cp.z + intr.cy); //4
+
+ if (ukr.x < 0 || ukr.y < 0 || ukr.x >= coresp.cols || ukr.y >= coresp.rows)
+ return 0;
+
+ float3 nprev_g;
+ nprev_g.x = nmap_g_prev.ptr (ukr.y)[ukr.x];
+
+ if (isnan (nprev_g.x))
+ return 0;
+
+ float3 vprev_g;
+ vprev_g.x = vmap_g_prev.ptr (ukr.y )[ukr.x];
+ vprev_g.y = vmap_g_prev.ptr (ukr.y + coresp.rows)[ukr.x];
+ vprev_g.z = vmap_g_prev.ptr (ukr.y + 2 * coresp.rows)[ukr.x];
+
+ float dist = norm (vcurr_g - vprev_g);
+ if (dist > distThres)
+ return 0;
+
+ ncurr_g.y = nmap_g_curr.ptr (y + coresp.rows)[x];
+ ncurr_g.z = nmap_g_curr.ptr (y + 2 * coresp.rows)[x];
+
+ nprev_g.y = nmap_g_prev.ptr (ukr.y + coresp.rows)[ukr.x];
+ nprev_g.z = nmap_g_prev.ptr (ukr.y + 2 * coresp.rows)[ukr.x];
+
+ float sine = norm (cross (ncurr_g, nprev_g));
+
+ /*if (sine >= 1 || asinf(sine) >= angleThres)
+ return 0;*/
+
+ if (/*sine >= 1 || */ sine >= angleThres)
+ return 0;
+
+ coresp.ptr (y)[x] = make_short2 (ukr.x, ukr.y);
+ return 1;
+ }
+
+ __device__ __forceinline__ void
+ reduce (int i) const
+ {
+ __shared__ volatile int smem[CTA_SIZE];
+
+ int tid = Block::flattenedThreadId ();
+
+ smem[tid] = i;
+ __syncthreads ();
+
+ Block::reduce<CTA_SIZE>(smem, plus ());
+
+ __shared__ bool isLastBlockDone;
+
+ if (tid == 0)
+ {
+ gbuf[blockIdx.x + gridDim.x * blockIdx.y] = smem[0];
+ __threadfence ();
+
+ unsigned int value = atomicInc (&count, gridDim.x * gridDim.y);
+
+ isLastBlockDone = (value == (gridDim.x * gridDim.y - 1));
+ }
+ __syncthreads ();
+
+ if (isLastBlockDone)
+ {
+ int sum = 0;
+ int stride = Block::stride ();
+ for (int pos = tid; pos < gridDim.x * gridDim.y; pos += stride)
+ sum += gbuf[pos];
+
+ smem[tid] = sum;
+ __syncthreads ();
+ Block::reduce<CTA_SIZE>(smem, plus ());
+
+ if (tid == 0)
+ {
+ gbuf[0] = smem[0];
+ count = 0;
+ }
+ }
+ }
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int mask = search ();
+ //reduce(mask); if uncomment -> need to allocate and set gbuf
+ }
+ };
+
+ __global__ void
+ corespKernel (const CorespSearch cs) {
+ cs ();
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::device::findCoresp (const MapArr& vmap_g_curr, const MapArr& nmap_g_curr,
+ const Mat33& Rprev_inv, const float3& tprev, const Intr& intr,
+ const MapArr& vmap_g_prev, const MapArr& nmap_g_prev,
+ float distThres, float angleThres, PtrStepSz<short2> coresp)
+{
+ CorespSearch cs;
+
+ cs.vmap_g_curr = vmap_g_curr;
+ cs.nmap_g_curr = nmap_g_curr;
+
+ cs.Rprev_inv = Rprev_inv;
+ cs.tprev = tprev;
+
+ cs.intr = intr;
+
+ cs.vmap_g_prev = vmap_g_prev;
+ cs.nmap_g_prev = nmap_g_prev;
+
+ cs.distThres = distThres;
+ cs.angleThres = angleThres;
+
+ cs.coresp = coresp;
+
+ dim3 block (CorespSearch::CTA_SIZE_X, CorespSearch::CTA_SIZE_Y);
+ dim3 grid (divUp (coresp.cols, block.x), divUp (coresp.rows, block.y));
+
+ corespKernel<<<grid, block>>>(cs);
+
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_GPU_KINFU_DEVICE_HPP_
+#define PCL_GPU_KINFU_DEVICE_HPP_
+
+//#include <pcl/gpu/utils/device/limits.hpp>
+//#include <pcl/gpu/utils/device/vector_math.hpp>
+#include "utils.hpp" //temporary reimplementing to release kinfu without pcl_gpu_utils
+
+#include "internal.h"
+
+namespace pcl
+{
+ namespace device
+ {
+ #define INV_DIV 3.051850947599719e-5f
+
+ __device__ __forceinline__ void
+ pack_tsdf (float tsdf, int weight, short2& value)
+ {
+ int fixedp = max (-DIVISOR, min (DIVISOR, __float2int_rz (tsdf * DIVISOR)));
+ //int fixedp = __float2int_rz(tsdf * DIVISOR);
+ value = make_short2 (fixedp, weight);
+ }
+
+ __device__ __forceinline__ void
+ unpack_tsdf (short2 value, float& tsdf, int& weight)
+ {
+ weight = value.y;
+ tsdf = __int2float_rn (value.x) / DIVISOR; //*/ * INV_DIV;
+ }
+
+ __device__ __forceinline__ float
+ unpack_tsdf (short2 value)
+ {
+ return static_cast<float>(value.x) / DIVISOR; //*/ * INV_DIV;
+ }
+
+
+ __device__ __forceinline__ float3
+ operator* (const Mat33& m, const float3& vec)
+ {
+ return make_float3 (dot (m.data[0], vec), dot (m.data[1], vec), dot (m.data[2], vec));
+ }
+
+
+ ////////////////////////////////////////////////////////////////////////////////////////
+ ///// Prefix Scan utility
+
+ enum ScanKind { exclusive, inclusive };
+
+ template<ScanKind Kind, class T>
+ __device__ __forceinline__ T
+ scan_warp ( volatile T *ptr, const unsigned int idx = threadIdx.x )
+ {
+ const unsigned int lane = idx & 31; // index of thread in warp (0..31)
+
+ if (lane >= 1) ptr[idx] = ptr[idx - 1] + ptr[idx];
+ if (lane >= 2) ptr[idx] = ptr[idx - 2] + ptr[idx];
+ if (lane >= 4) ptr[idx] = ptr[idx - 4] + ptr[idx];
+ if (lane >= 8) ptr[idx] = ptr[idx - 8] + ptr[idx];
+ if (lane >= 16) ptr[idx] = ptr[idx - 16] + ptr[idx];
+
+ if (Kind == inclusive)
+ return ptr[idx];
+ else
+ return (lane > 0) ? ptr[idx - 1] : 0;
+ }
+ }
+}
+
+#endif /* PCL_GPU_KINFU_DEVICE_HPP_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+//#include <pcl/gpu/utils/device/block.hpp>
+//#include <pcl/gpu/utils/device/funcattrib.hpp>
+#include "device.hpp"
+
+namespace pcl
+{
+ namespace device
+ {
+ typedef double float_type;
+
+ template<int CTA_SIZE_, typename T>
+ static __device__ __forceinline__ void reduce(volatile T* buffer)
+ {
+ int tid = Block::flattenedThreadId();
+ T val = buffer[tid];
+
+ if (CTA_SIZE_ >= 1024) { if (tid < 512) buffer[tid] = val = val + buffer[tid + 512]; __syncthreads(); }
+ if (CTA_SIZE_ >= 512) { if (tid < 256) buffer[tid] = val = val + buffer[tid + 256]; __syncthreads(); }
+ if (CTA_SIZE_ >= 256) { if (tid < 128) buffer[tid] = val = val + buffer[tid + 128]; __syncthreads(); }
+ if (CTA_SIZE_ >= 128) { if (tid < 64) buffer[tid] = val = val + buffer[tid + 64]; __syncthreads(); }
+
+ if (tid < 32)
+ {
+ if (CTA_SIZE_ >= 64) { buffer[tid] = val = val + buffer[tid + 32]; }
+ if (CTA_SIZE_ >= 32) { buffer[tid] = val = val + buffer[tid + 16]; }
+ if (CTA_SIZE_ >= 16) { buffer[tid] = val = val + buffer[tid + 8]; }
+ if (CTA_SIZE_ >= 8) { buffer[tid] = val = val + buffer[tid + 4]; }
+ if (CTA_SIZE_ >= 4) { buffer[tid] = val = val + buffer[tid + 2]; }
+ if (CTA_SIZE_ >= 2) { buffer[tid] = val = val + buffer[tid + 1]; }
+ }
+ }
+
+ struct Combined
+ {
+ enum
+ {
+ CTA_SIZE_X = 32,
+ CTA_SIZE_Y = 8,
+ CTA_SIZE = CTA_SIZE_X * CTA_SIZE_Y
+ };
+
+
+ Mat33 Rcurr;
+ float3 tcurr;
+
+ PtrStep<float> vmap_curr;
+ PtrStep<float> nmap_curr;
+
+ Mat33 Rprev_inv;
+ float3 tprev;
+
+ Intr intr;
+
+ PtrStep<float> vmap_g_prev;
+ PtrStep<float> nmap_g_prev;
+
+ float distThres;
+ float angleThres;
+
+ int cols;
+ int rows;
+
+ mutable PtrStep<float_type> gbuf;
+
+ __device__ __forceinline__ bool
+ search (int x, int y, float3& n, float3& d, float3& s) const
+ {
+ float3 ncurr;
+ ncurr.x = nmap_curr.ptr (y)[x];
+
+ if (isnan (ncurr.x))
+ return (false);
+
+ float3 vcurr;
+ vcurr.x = vmap_curr.ptr (y )[x];
+ vcurr.y = vmap_curr.ptr (y + rows)[x];
+ vcurr.z = vmap_curr.ptr (y + 2 * rows)[x];
+
+ float3 vcurr_g = Rcurr * vcurr + tcurr;
+
+ float3 vcurr_cp = Rprev_inv * (vcurr_g - tprev); // prev camera coo space
+
+ int2 ukr; //projection
+ ukr.x = __float2int_rn (vcurr_cp.x * intr.fx / vcurr_cp.z + intr.cx); //4
+ ukr.y = __float2int_rn (vcurr_cp.y * intr.fy / vcurr_cp.z + intr.cy); //4
+
+ if (ukr.x < 0 || ukr.y < 0 || ukr.x >= cols || ukr.y >= rows || vcurr_cp.z < 0)
+ return (false);
+
+ float3 nprev_g;
+ nprev_g.x = nmap_g_prev.ptr (ukr.y)[ukr.x];
+
+ if (isnan (nprev_g.x))
+ return (false);
+
+ float3 vprev_g;
+ vprev_g.x = vmap_g_prev.ptr (ukr.y )[ukr.x];
+ vprev_g.y = vmap_g_prev.ptr (ukr.y + rows)[ukr.x];
+ vprev_g.z = vmap_g_prev.ptr (ukr.y + 2 * rows)[ukr.x];
+
+ float dist = norm (vprev_g - vcurr_g);
+ if (dist > distThres)
+ return (false);
+
+ ncurr.y = nmap_curr.ptr (y + rows)[x];
+ ncurr.z = nmap_curr.ptr (y + 2 * rows)[x];
+
+ float3 ncurr_g = Rcurr * ncurr;
+
+ nprev_g.y = nmap_g_prev.ptr (ukr.y + rows)[ukr.x];
+ nprev_g.z = nmap_g_prev.ptr (ukr.y + 2 * rows)[ukr.x];
+
+ float sine = norm (cross (ncurr_g, nprev_g));
+
+ if (sine >= angleThres)
+ return (false);
+ n = nprev_g;
+ d = vprev_g;
+ s = vcurr_g;
+ return (true);
+ }
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int x = threadIdx.x + blockIdx.x * CTA_SIZE_X;
+ int y = threadIdx.y + blockIdx.y * CTA_SIZE_Y;
+
+ float3 n, d, s;
+ bool found_coresp = false;
+
+ if (x < cols && y < rows)
+ found_coresp = search (x, y, n, d, s);
+
+ float row[7];
+
+ if (found_coresp)
+ {
+ *(float3*)&row[0] = cross (s, n);
+ *(float3*)&row[3] = n;
+ row[6] = dot (n, d - s);
+ }
+ else
+ row[0] = row[1] = row[2] = row[3] = row[4] = row[5] = row[6] = 0.f;
+
+ __shared__ float_type smem[CTA_SIZE];
+ int tid = Block::flattenedThreadId ();
+
+ int shift = 0;
+ for (int i = 0; i < 6; ++i) //rows
+ {
+ #pragma unroll
+ for (int j = i; j < 7; ++j) // cols + b
+ {
+ __syncthreads ();
+ smem[tid] = row[i] * row[j];
+ __syncthreads ();
+
+ reduce<CTA_SIZE>(smem);
+
+ if (tid == 0)
+ gbuf.ptr (shift++)[blockIdx.x + gridDim.x * blockIdx.y] = smem[0];
+ }
+ }
+ }
+ };
+
+ __global__ void
+ combinedKernel (const Combined cs)
+ {
+ cs ();
+ }
+
+ struct TranformReduction
+ {
+ enum
+ {
+ CTA_SIZE = 512,
+ STRIDE = CTA_SIZE,
+
+ B = 6, COLS = 6, ROWS = 6, DIAG = 6,
+ UPPER_DIAG_MAT = (COLS * ROWS - DIAG) / 2 + DIAG,
+ TOTAL = UPPER_DIAG_MAT + B,
+
+ GRID_X = TOTAL
+ };
+
+ PtrStep<float_type> gbuf;
+ int length;
+ mutable float_type* output;
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ const float_type *beg = gbuf.ptr (blockIdx.x);
+ const float_type *end = beg + length;
+
+ int tid = threadIdx.x;
+
+ float_type sum = 0.f;
+ for (const float_type *t = beg + tid; t < end; t += STRIDE)
+ sum += *t;
+
+ __shared__ float_type smem[CTA_SIZE];
+
+ smem[tid] = sum;
+ __syncthreads ();
+
+ reduce<CTA_SIZE>(smem);
+
+ if (tid == 0)
+ output[blockIdx.x] = smem[0];
+ }
+ };
+
+ __global__ void
+ TransformEstimatorKernel2 (const TranformReduction tr)
+ {
+ tr ();
+ }
+ }
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::device::estimateCombined (const Mat33& Rcurr, const float3& tcurr,
+ const MapArr& vmap_curr, const MapArr& nmap_curr,
+ const Mat33& Rprev_inv, const float3& tprev, const Intr& intr,
+ const MapArr& vmap_g_prev, const MapArr& nmap_g_prev,
+ float distThres, float angleThres,
+ DeviceArray2D<float_type>& gbuf, DeviceArray<float_type>& mbuf,
+ float_type* matrixA_host, float_type* vectorB_host)
+{
+ int cols = vmap_curr.cols ();
+ int rows = vmap_curr.rows () / 3;
+
+ Combined cs;
+
+ cs.Rcurr = Rcurr;
+ cs.tcurr = tcurr;
+
+ cs.vmap_curr = vmap_curr;
+ cs.nmap_curr = nmap_curr;
+
+ cs.Rprev_inv = Rprev_inv;
+ cs.tprev = tprev;
+
+ cs.intr = intr;
+
+ cs.vmap_g_prev = vmap_g_prev;
+ cs.nmap_g_prev = nmap_g_prev;
+
+ cs.distThres = distThres;
+ cs.angleThres = angleThres;
+
+ cs.cols = cols;
+ cs.rows = rows;
+
+//////////////////////////////
+
+ dim3 block (Combined::CTA_SIZE_X, Combined::CTA_SIZE_Y);
+ dim3 grid (1, 1, 1);
+ grid.x = divUp (cols, block.x);
+ grid.y = divUp (rows, block.y);
+
+ mbuf.create (TranformReduction::TOTAL);
+ if (gbuf.rows () != TranformReduction::TOTAL || gbuf.cols () < (int)(grid.x * grid.y))
+ gbuf.create (TranformReduction::TOTAL, grid.x * grid.y);
+
+ cs.gbuf = gbuf;
+
+ combinedKernel<<<grid, block>>>(cs);
+ cudaSafeCall ( cudaGetLastError () );
+ //cudaSafeCall(cudaDeviceSynchronize());
+
+ //printFuncAttrib(combinedKernel);
+
+ TranformReduction tr;
+ tr.gbuf = gbuf;
+ tr.length = grid.x * grid.y;
+ tr.output = mbuf;
+
+ TransformEstimatorKernel2<<<TranformReduction::TOTAL, TranformReduction::CTA_SIZE>>>(tr);
+ cudaSafeCall (cudaGetLastError ());
+ cudaSafeCall (cudaDeviceSynchronize ());
+
+ float_type host_data[TranformReduction::TOTAL];
+ mbuf.download (host_data);
+
+ int shift = 0;
+ for (int i = 0; i < 6; ++i) //rows
+ for (int j = i; j < 7; ++j) // cols + b
+ {
+ float_type value = host_data[shift++];
+ if (j == 6) // vector b
+ vectorB_host[i] = value;
+ else
+ matrixA_host[j * 6 + i] = matrixA_host[i * 6 + j] = value;
+ }
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+//#include <pcl/gpu/utils/device/block.hpp>
+//#include <pcl/gpu/utils/device/funcattrib.hpp>
+//#include <pcl/gpu/utils/timers_cuda.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+ template<typename T>
+ struct TransformEstimator
+ {
+ enum
+ {
+ CTA_SIZE_X = 32,
+ CTA_SIZE_Y = 8,
+ CTA_SIZE = CTA_SIZE_X * CTA_SIZE_Y
+ };
+
+ struct plus
+ {
+ __forceinline__ __device__ T
+ operator () (const T &lhs, const volatile T &rhs) const {
+ return lhs + rhs;
+ }
+ };
+
+ PtrStep<float> v_dst;
+ PtrStep<float> n_dst;
+ PtrStep<float> v_src;
+ PtrStepSz<short2> coresp;
+
+ mutable PtrStep<T> gbuf;
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ float row[7];
+ row[0] = row[1] = row[2] = row[3] = row[4] = row[5] = row[6] = 0.f;
+
+ if (x < coresp.cols || y < coresp.rows)
+ {
+ short2 ukr = coresp.ptr (y)[x];
+
+ if (ukr.x != -1)
+ {
+ float3 n;
+ n.x = n_dst.ptr (ukr.y )[ukr.x];
+ n.y = n_dst.ptr (ukr.y + coresp.rows)[ukr.x];
+ n.z = n_dst.ptr (ukr.y + 2 * coresp.rows)[ukr.x];
+
+ float3 d;
+ d.x = v_dst.ptr (ukr.y )[ukr.x];
+ d.y = v_dst.ptr (ukr.y + coresp.rows)[ukr.x];
+ d.z = v_dst.ptr (ukr.y + 2 * coresp.rows)[ukr.x];
+
+ float3 s;
+ s.x = v_src.ptr (y )[x];
+ s.y = v_src.ptr (y + coresp.rows)[x];
+ s.z = v_src.ptr (y + 2 * coresp.rows)[x];
+
+ float b = dot (n, d - s);
+
+ *(float3*)&row[0] = cross (s, n);
+ *(float3*)&row[3] = n;
+ row[6] = b;
+ }
+ }
+
+ __shared__ T smem[CTA_SIZE];
+ int tid = Block::flattenedThreadId ();
+
+ int shift = 0;
+ for (int i = 0; i < 6; ++i) //rows
+ {
+ #pragma unroll
+ for (int j = i; j < 7; ++j) // cols + b
+ {
+ __syncthreads ();
+ smem[tid] = row[i] * row[j];
+ __syncthreads ();
+
+ Block::reduce<CTA_SIZE>(smem, plus ());
+
+ if (tid == 0)
+ gbuf.ptr (shift++)[blockIdx.x + gridDim.x * blockIdx.y] = smem[0];
+ }
+ }
+ }
+ };
+
+ template<typename T>
+ struct TranformReduction
+ {
+ enum
+ {
+ CTA_SIZE = 512,
+ STRIDE = CTA_SIZE,
+
+ B = 6, COLS = 6, ROWS = 6, DIAG = 6,
+ UPPER_DIAG_MAT = (COLS * ROWS - DIAG) / 2 + DIAG,
+ TOTAL = UPPER_DIAG_MAT + B,
+
+ GRID_X = TOTAL
+ };
+
+ PtrStep<T> gbuf;
+ int length;
+ mutable T* output;
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ const T *beg = gbuf.ptr (blockIdx.x);
+ const T *end = beg + length;
+
+ int tid = threadIdx.x;
+
+ T sum = 0.f;
+ for (const T *t = beg + tid; t < end; t += STRIDE)
+ sum += *t;
+
+ __shared__ T smem[CTA_SIZE];
+
+ smem[tid] = sum;
+ __syncthreads ();
+
+ Block::reduce<CTA_SIZE>(smem, TransformEstimator<T>::plus ());
+
+ if (tid == 0)
+ output[blockIdx.x] = smem[0];
+ }
+ };
+
+ __global__ void
+ TransformEstimatorKernel1 (const TransformEstimator<float> te) {
+ te ();
+ }
+ __global__ void
+ TransformEstimatorKernel2 (const TranformReduction<float> tr) {
+ tr ();
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::device::estimateTransform (const MapArr& v_dst, const MapArr& n_dst,
+ const MapArr& v_src, const PtrStepSz<short2>& coresp,
+ DeviceArray2D<float>& gbuf, DeviceArray<float>& mbuf,
+ float* matrixA_host, float* vectorB_host)
+{
+ typedef TransformEstimator<float> TEst;
+ typedef TranformReduction<float> TRed;
+
+ dim3 block (TEst::CTA_SIZE_X, TEst::CTA_SIZE_Y);
+ dim3 grid (1, 1, 1);
+ grid.x = divUp (coresp.cols, block.x);
+ grid.y = divUp (coresp.rows, block.y);
+
+ mbuf.create (TRed::TOTAL);
+ if (gbuf.rows () != TRed::TOTAL || gbuf.cols () < (int)(grid.x * grid.y))
+ gbuf.create (TRed::TOTAL, grid.x * grid.y);
+
+ TEst te;
+ te.n_dst = n_dst;
+ te.v_dst = v_dst;
+ te.v_src = v_src;
+ te.coresp = coresp;
+ te.gbuf = gbuf;
+
+ TransformEstimatorKernel1<<<grid, block>>>(te);
+ cudaSafeCall ( cudaGetLastError () );
+ //cudaSafeCall(cudaDeviceSynchronize());
+
+ TRed tr;
+ tr.gbuf = gbuf;
+ tr.length = grid.x * grid.y;
+ tr.output = mbuf;
+
+ TransformEstimatorKernel2<<<TRed::TOTAL, TRed::CTA_SIZE>>>(tr);
+
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+
+ float host_data[TRed::TOTAL];
+ mbuf.download (host_data);
+
+ int shift = 0;
+ for (int i = 0; i < 6; ++i) //rows
+ for (int j = i; j < 7; ++j) // cols + b
+ {
+ float value = host_data[shift++];
+ if (j == 6) // vector b
+ vectorB_host[i] = value;
+ else
+ matrixA_host[j * 6 + i] = matrixA_host[i * 6 + j] = value;
+ }
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+//#include <pcl/gpu/utils/device/funcattrib.hpp>
+//include <pcl/gpu/utils/device/block.hpp>
+//#include <pcl/gpu/utils/device/warp.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+
+ ////////////////////////////////////////////////////////////////////////////////////////
+ ///// Full Volume Scan6
+
+ enum
+ {
+ CTA_SIZE_X = 32,
+ CTA_SIZE_Y = 6,
+ CTA_SIZE = CTA_SIZE_X * CTA_SIZE_Y,
+
+ MAX_LOCAL_POINTS = 3
+ };
+
+ __device__ int global_count = 0;
+ __device__ int output_count;
+ __device__ unsigned int blocks_done = 0;
+
+ __shared__ float storage_X[CTA_SIZE * MAX_LOCAL_POINTS];
+ __shared__ float storage_Y[CTA_SIZE * MAX_LOCAL_POINTS];
+ __shared__ float storage_Z[CTA_SIZE * MAX_LOCAL_POINTS];
+
+ struct FullScan6
+ {
+ PtrStep<short2> volume;
+ float3 cell_size;
+
+ mutable PtrSz<PointType> output;
+
+ __device__ __forceinline__ float
+ fetch (int x, int y, int z, int& weight) const
+ {
+ float tsdf;
+ unpack_tsdf (volume.ptr (VOLUME_Y * z + y)[x], tsdf, weight);
+ return tsdf;
+ }
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int x = threadIdx.x + blockIdx.x * CTA_SIZE_X;
+ int y = threadIdx.y + blockIdx.y * CTA_SIZE_Y;
+
+#if __CUDA_ARCH__ < 200
+ __shared__ int cta_buffer[CTA_SIZE];
+#endif
+
+#if __CUDA_ARCH__ >= 120
+ if (__all (x >= VOLUME_X) || __all (y >= VOLUME_Y))
+ return;
+#else
+ if (Emulation::All(x >= VOLUME_X, cta_buffer) ||
+ Emulation::All(y >= VOLUME_Y, cta_buffer))
+ return;
+#endif
+
+ float3 V;
+ V.x = (x + 0.5f) * cell_size.x;
+ V.y = (y + 0.5f) * cell_size.y;
+
+ int ftid = Block::flattenedThreadId ();
+
+ for (int z = 0; z < VOLUME_Z - 1; ++z)
+ {
+ float3 points[MAX_LOCAL_POINTS];
+ int local_count = 0;
+
+ if (x < VOLUME_X && y < VOLUME_Y)
+ {
+ int W;
+ float F = fetch (x, y, z, W);
+
+ if (W != 0 && F != 1.f)
+ {
+ V.z = (z + 0.5f) * cell_size.z;
+
+ //process dx
+ if (x + 1 < VOLUME_X)
+ {
+ int Wn;
+ float Fn = fetch (x + 1, y, z, Wn);
+
+ if (Wn != 0 && Fn != 1.f)
+ if ((F > 0 && Fn < 0) || (F < 0 && Fn > 0))
+ {
+ float3 p;
+ p.y = V.y;
+ p.z = V.z;
+
+ float Vnx = V.x + cell_size.x;
+
+ float d_inv = 1.f / (fabs (F) + fabs (Fn));
+ p.x = (V.x * fabs (Fn) + Vnx * fabs (F)) * d_inv;
+
+ points[local_count++] = p;
+ }
+ } /* if (x + 1 < VOLUME_X) */
+
+ //process dy
+ if (y + 1 < VOLUME_Y)
+ {
+ int Wn;
+ float Fn = fetch (x, y + 1, z, Wn);
+
+ if (Wn != 0 && Fn != 1.f)
+ if ((F > 0 && Fn < 0) || (F < 0 && Fn > 0))
+ {
+ float3 p;
+ p.x = V.x;
+ p.z = V.z;
+
+ float Vny = V.y + cell_size.y;
+
+ float d_inv = 1.f / (fabs (F) + fabs (Fn));
+ p.y = (V.y * fabs (Fn) + Vny * fabs (F)) * d_inv;
+
+ points[local_count++] = p;
+ }
+ } /* if (y + 1 < VOLUME_Y) */
+
+ //process dz
+ //if (z + 1 < VOLUME_Z) // guaranteed by loop
+ {
+ int Wn;
+ float Fn = fetch (x, y, z + 1, Wn);
+
+ if (Wn != 0 && Fn != 1.f)
+ if ((F > 0 && Fn < 0) || (F < 0 && Fn > 0))
+ {
+ float3 p;
+ p.x = V.x;
+ p.y = V.y;
+
+ float Vnz = V.z + cell_size.z;
+
+ float d_inv = 1.f / (fabs (F) + fabs (Fn));
+ p.z = (V.z * fabs (Fn) + Vnz * fabs (F)) * d_inv;
+
+ points[local_count++] = p;
+ }
+ } /* if (z + 1 < VOLUME_Z) */
+ } /* if (W != 0 && F != 1.f) */
+ } /* if (x < VOLUME_X && y < VOLUME_Y) */
+
+
+#if __CUDA_ARCH__ >= 200
+ ///not we fulfilled points array at current iteration
+ int total_warp = __popc (__ballot (local_count > 0)) + __popc (__ballot (local_count > 1)) + __popc (__ballot (local_count > 2));
+#else
+ int tid = Block::flattenedThreadId();
+ cta_buffer[tid] = local_count;
+ int total_warp = Emulation::warp_reduce(cta_buffer, tid);
+#endif
+
+ if (total_warp > 0)
+ {
+ int lane = Warp::laneId ();
+ int storage_index = (ftid >> Warp::LOG_WARP_SIZE) * Warp::WARP_SIZE * MAX_LOCAL_POINTS;
+
+ volatile int* cta_buffer = (int*)(storage_X + storage_index);
+
+ cta_buffer[lane] = local_count;
+ int offset = scan_warp<exclusive>(cta_buffer, lane);
+
+ if (lane == 0)
+ {
+ int old_global_count = atomicAdd (&global_count, total_warp);
+ cta_buffer[0] = old_global_count;
+ }
+ int old_global_count = cta_buffer[0];
+
+ for (int l = 0; l < local_count; ++l)
+ {
+ storage_X[storage_index + offset + l] = points[l].x;
+ storage_Y[storage_index + offset + l] = points[l].y;
+ storage_Z[storage_index + offset + l] = points[l].z;
+ }
+
+ PointType *pos = output.data + old_global_count + lane;
+ for (int idx = lane; idx < total_warp; idx += Warp::STRIDE, pos += Warp::STRIDE)
+ {
+ float x = storage_X[storage_index + idx];
+ float y = storage_Y[storage_index + idx];
+ float z = storage_Z[storage_index + idx];
+ store_point_type (x, y, z, pos);
+ }
+
+ bool full = (old_global_count + total_warp) >= output.size;
+
+ if (full)
+ break;
+ }
+
+ } /* for(int z = 0; z < VOLUME_Z - 1; ++z) */
+
+
+ ///////////////////////////
+ // prepare for future scans
+ if (ftid == 0)
+ {
+ unsigned int total_blocks = gridDim.x * gridDim.y * gridDim.z;
+ unsigned int value = atomicInc (&blocks_done, total_blocks);
+
+ //last block
+ if (value == total_blocks - 1)
+ {
+ output_count = min ((int)output.size, global_count);
+ blocks_done = 0;
+ global_count = 0;
+ }
+ }
+ } /* operator() */
+
+ __device__ __forceinline__ void
+ store_point_type (float x, float y, float z, float4* ptr) const {
+ *ptr = make_float4 (x, y, z, 0);
+ }
+ __device__ __forceinline__ void
+ store_point_type (float x, float y, float z, float3* ptr) const {
+ *ptr = make_float3 (x, y, z);
+ }
+ };
+
+ __global__ void
+ extractKernel (const FullScan6 fs) {
+ fs ();
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+size_t
+pcl::device::extractCloud (const PtrStep<short2>& volume, const float3& volume_size,
+ PtrSz<PointType> output)
+{
+ FullScan6 fs;
+ fs.volume = volume;
+ fs.cell_size.x = volume_size.x / VOLUME_X;
+ fs.cell_size.y = volume_size.y / VOLUME_Y;
+ fs.cell_size.z = volume_size.z / VOLUME_Z;
+ fs.output = output;
+
+ dim3 block (CTA_SIZE_X, CTA_SIZE_Y);
+ dim3 grid (divUp (VOLUME_X, block.x), divUp (VOLUME_Y, block.y));
+
+ //cudaFuncSetCacheConfig(extractKernel, cudaFuncCachePreferL1);
+ //printFuncAttrib(extractKernel);
+
+ extractKernel<<<grid, block>>>(fs);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+
+ int size;
+ cudaSafeCall ( cudaMemcpyFromSymbol (&size, output_count, sizeof(size)) );
+ return (size_t)size;
+}
+
+
+namespace pcl
+{
+ namespace device
+ {
+ template<typename NormalType>
+ struct ExtractNormals
+ {
+ float3 cell_size;
+ PtrStep<short2> volume;
+ PtrSz<PointType> points;
+
+ mutable NormalType* output;
+
+ __device__ __forceinline__ float
+ readTsdf (int x, int y, int z) const
+ {
+ return unpack_tsdf (volume.ptr (VOLUME_Y * z + y)[x]);
+ }
+
+ __device__ __forceinline__ float3
+ fetchPoint (int idx) const
+ {
+ PointType p = points.data[idx];
+ return make_float3 (p.x, p.y, p.z);
+ }
+ __device__ __forceinline__ void
+ storeNormal (int idx, float3 normal) const
+ {
+ NormalType n;
+ n.x = normal.x; n.y = normal.y; n.z = normal.z;
+ output[idx] = n;
+ }
+
+ __device__ __forceinline__ int3
+ getVoxel (const float3& point) const
+ {
+ int vx = __float2int_rd (point.x / cell_size.x); // round to negative infinity
+ int vy = __float2int_rd (point.y / cell_size.y);
+ int vz = __float2int_rd (point.z / cell_size.z);
+
+ return make_int3 (vx, vy, vz);
+ }
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int idx = threadIdx.x + blockIdx.x * blockDim.x;
+
+ if (idx >= points.size)
+ return;
+ const float qnan = numeric_limits<float>::quiet_NaN ();
+ float3 n = make_float3 (qnan, qnan, qnan);
+
+ float3 point = fetchPoint (idx);
+ int3 g = getVoxel (point);
+
+ if (g.x > 1 && g.y > 1 && g.z > 1 && g.x < VOLUME_X - 2 && g.y < VOLUME_Y - 2 && g.z < VOLUME_Z - 2)
+ {
+ float3 t;
+
+ t = point;
+ t.x += cell_size.x;
+ float Fx1 = interpolateTrilineary (t);
+
+ t = point;
+ t.x -= cell_size.x;
+ float Fx2 = interpolateTrilineary (t);
+
+ n.x = (Fx1 - Fx2);
+
+ t = point;
+ t.y += cell_size.y;
+ float Fy1 = interpolateTrilineary (t);
+
+ t = point;
+ t.y -= cell_size.y;
+ float Fy2 = interpolateTrilineary (t);
+
+ n.y = (Fy1 - Fy2);
+
+ t = point;
+ t.z += cell_size.z;
+ float Fz1 = interpolateTrilineary (t);
+
+ t = point;
+ t.z -= cell_size.z;
+ float Fz2 = interpolateTrilineary (t);
+
+ n.z = (Fz1 - Fz2);
+
+ n = normalized (n);
+ }
+ storeNormal (idx, n);
+ }
+
+ __device__ __forceinline__ float
+ interpolateTrilineary (const float3& point) const
+ {
+ int3 g = getVoxel (point);
+
+ float vx = (g.x + 0.5f) * cell_size.x;
+ float vy = (g.y + 0.5f) * cell_size.y;
+ float vz = (g.z + 0.5f) * cell_size.z;
+
+ g.x = (point.x < vx) ? (g.x - 1) : g.x;
+ g.y = (point.y < vy) ? (g.y - 1) : g.y;
+ g.z = (point.z < vz) ? (g.z - 1) : g.z;
+
+ float a = (point.x - (g.x + 0.5f) * cell_size.x) / cell_size.x;
+ float b = (point.y - (g.y + 0.5f) * cell_size.y) / cell_size.y;
+ float c = (point.z - (g.z + 0.5f) * cell_size.z) / cell_size.z;
+
+ float res = readTsdf (g.x + 0, g.y + 0, g.z + 0) * (1 - a) * (1 - b) * (1 - c) +
+ readTsdf (g.x + 0, g.y + 0, g.z + 1) * (1 - a) * (1 - b) * c +
+ readTsdf (g.x + 0, g.y + 1, g.z + 0) * (1 - a) * b * (1 - c) +
+ readTsdf (g.x + 0, g.y + 1, g.z + 1) * (1 - a) * b * c +
+ readTsdf (g.x + 1, g.y + 0, g.z + 0) * a * (1 - b) * (1 - c) +
+ readTsdf (g.x + 1, g.y + 0, g.z + 1) * a * (1 - b) * c +
+ readTsdf (g.x + 1, g.y + 1, g.z + 0) * a * b * (1 - c) +
+ readTsdf (g.x + 1, g.y + 1, g.z + 1) * a * b * c;
+ return res;
+ }
+ };
+
+ template<typename NormalType>
+ __global__ void
+ extractNormalsKernel (const ExtractNormals<NormalType> en) {
+ en ();
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename NormalType> void
+pcl::device::extractNormals (const PtrStep<short2>& volume, const float3& volume_size,
+ const PtrSz<PointType>& points, NormalType* output)
+{
+ ExtractNormals<NormalType> en;
+ en.volume = volume;
+ en.cell_size.x = volume_size.x / VOLUME_X;
+ en.cell_size.y = volume_size.y / VOLUME_Y;
+ en.cell_size.z = volume_size.z / VOLUME_Z;
+ en.points = points;
+ en.output = output;
+
+ dim3 block (256);
+ dim3 grid (divUp (points.size, block.x));
+
+ extractNormalsKernel<<<grid, block>>>(en);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+}
+
+using namespace pcl::device;
+
+template void pcl::device::extractNormals<PointType>(const PtrStep<short2>&volume, const float3 &volume_size, const PtrSz<PointType>&input, PointType * output);
+template void pcl::device::extractNormals<float8>(const PtrStep<short2>&volume, const float3 &volume_size, const PtrSz<PointType>&input, float8 * output);
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+
+using namespace pcl::device;
+
+namespace pcl
+{
+ namespace device
+ {
+ struct ImageGenerator
+ {
+ enum
+ {
+ CTA_SIZE_X = 32, CTA_SIZE_Y = 8
+ };
+
+ PtrStep<float> vmap;
+ PtrStep<float> nmap;
+
+ LightSource light;
+
+ mutable PtrStepSz<uchar3> dst;
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int x = threadIdx.x + blockIdx.x * CTA_SIZE_X;
+ int y = threadIdx.y + blockIdx.y * CTA_SIZE_Y;
+
+ if (x >= dst.cols || y >= dst.rows)
+ return;
+
+ float3 v, n;
+ v.x = vmap.ptr (y)[x];
+ n.x = nmap.ptr (y)[x];
+
+ uchar3 color = make_uchar3 (0, 0, 0);
+
+ if (!isnan (v.x) && !isnan (n.x))
+ {
+ v.y = vmap.ptr (y + dst.rows)[x];
+ v.z = vmap.ptr (y + 2 * dst.rows)[x];
+
+ n.y = nmap.ptr (y + dst.rows)[x];
+ n.z = nmap.ptr (y + 2 * dst.rows)[x];
+
+ float weight = 1.f;
+
+ for (int i = 0; i < light.number; ++i)
+ {
+ float3 vec = normalized (light.pos[i] - v);
+
+ weight *= fabs (dot (vec, n));
+ }
+
+ int br = (int)(205 * weight) + 50;
+ br = max (0, min (255, br));
+ color = make_uchar3 (br, br, br);
+ }
+ dst.ptr (y)[x] = color;
+ }
+ };
+
+ __global__ void
+ generateImageKernel (const ImageGenerator ig) {
+ ig ();
+ }
+ }
+}
+
+
+void
+pcl::device::generateImage (const MapArr& vmap, const MapArr& nmap, const LightSource& light,
+ PtrStepSz<uchar3> dst)
+{
+ ImageGenerator ig;
+ ig.vmap = vmap;
+ ig.nmap = nmap;
+ ig.light = light;
+ ig.dst = dst;
+
+ dim3 block (ImageGenerator::CTA_SIZE_X, ImageGenerator::CTA_SIZE_Y);
+ dim3 grid (divUp (dst.cols, block.x), divUp (dst.rows, block.y));
+
+ generateImageKernel<<<grid, block>>>(ig);
+ cudaSafeCall (cudaGetLastError ());
+ cudaSafeCall (cudaDeviceSynchronize ());
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+ namespace device
+ {
+ __global__ void generateDepthKernel(const float3 R_inv_row3, const float3 t, const PtrStep<float> vmap, PtrStepSz<unsigned short> depth)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x < depth.cols && y < depth.rows)
+ {
+ unsigned short result = 0;
+
+ float3 v_g;
+ v_g.x = vmap.ptr (y)[x];
+ if (!isnan (v_g.x))
+ {
+ v_g.y = vmap.ptr (y + depth.rows)[x];
+ v_g.z = vmap.ptr (y + 2 * depth.rows)[x];
+
+ float v_z = dot(R_inv_row3, v_g - t);
+
+ result = static_cast<unsigned short>(v_z * 1000);
+ }
+ depth.ptr(y)[x] = result;
+ }
+ }
+ }
+}
+
+void
+pcl::device::generateDepth (const Mat33& R_inv, const float3& t, const MapArr& vmap, DepthMap& dst)
+{
+ dim3 block(32, 8);
+ dim3 grid(divUp(dst.cols(), block.x), divUp(dst.rows(), block.y));
+
+ generateDepthKernel<<<grid, block>>>(R_inv.data[2], t, vmap, dst);
+ cudaSafeCall (cudaGetLastError ());
+ cudaSafeCall (cudaDeviceSynchronize ());
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+ namespace device
+ {
+ __global__ void
+ paint3DViewKernel(const PtrStep<uchar3> colors, PtrStepSz<uchar3> dst, float colors_weight)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x < dst.cols && y < dst.rows)
+ {
+ uchar3 value = dst.ptr(y)[x];
+ uchar3 color = colors.ptr(y)[x];
+
+ if (value.x != 0 || value.y != 0 || value.z != 0)
+ {
+ float cx = value.x * (1.f - colors_weight) + color.x * colors_weight;
+ float cy = value.y * (1.f - colors_weight) + color.y * colors_weight;
+ float cz = value.z * (1.f - colors_weight) + color.z * colors_weight;
+
+ value.x = min(255, max(0, __float2int_rn(cx)));
+ value.y = min(255, max(0, __float2int_rn(cy)));
+ value.z = min(255, max(0, __float2int_rn(cz)));
+ }
+
+ dst.ptr(y)[x] = value;
+ }
+ }
+ }
+}
+
+void
+pcl::device::paint3DView(const PtrStep<uchar3>& colors, PtrStepSz<uchar3> dst, float colors_weight)
+{
+ dim3 block(32, 8);
+ dim3 grid(divUp(dst.cols, block.x), divUp(dst.rows, block.y));
+
+ colors_weight = min(1.f, max(0.f, colors_weight));
+
+ paint3DViewKernel<<<grid, block>>>(colors, dst, colors_weight);
+ cudaSafeCall (cudaGetLastError ());
+ cudaSafeCall (cudaDeviceSynchronize ());
+}
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+
+using namespace pcl::device;
+using namespace pcl::gpu;
+
+namespace pcl
+{
+ namespace device
+ {
+ __global__ void
+ computeVmapKernel (const PtrStepSz<unsigned short> depth, PtrStep<float> vmap, float fx_inv, float fy_inv, float cx, float cy)
+ {
+ int u = threadIdx.x + blockIdx.x * blockDim.x;
+ int v = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (u < depth.cols && v < depth.rows)
+ {
+ float z = depth.ptr (v)[u] / 1000.f; // load and convert: mm -> meters
+
+ if (z != 0)
+ {
+ float vx = z * (u - cx) * fx_inv;
+ float vy = z * (v - cy) * fy_inv;
+ float vz = z;
+
+ vmap.ptr (v )[u] = vx;
+ vmap.ptr (v + depth.rows )[u] = vy;
+ vmap.ptr (v + depth.rows * 2)[u] = vz;
+ }
+ else
+ vmap.ptr (v)[u] = numeric_limits<float>::quiet_NaN ();
+
+ }
+ }
+
+ __global__ void
+ computeNmapKernel (int rows, int cols, const PtrStep<float> vmap, PtrStep<float> nmap)
+ {
+ int u = threadIdx.x + blockIdx.x * blockDim.x;
+ int v = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (u >= cols || v >= rows)
+ return;
+
+ if (u == cols - 1 || v == rows - 1)
+ {
+ nmap.ptr (v)[u] = numeric_limits<float>::quiet_NaN ();
+ return;
+ }
+
+ float3 v00, v01, v10;
+ v00.x = vmap.ptr (v )[u];
+ v01.x = vmap.ptr (v )[u + 1];
+ v10.x = vmap.ptr (v + 1)[u];
+
+ if (!isnan (v00.x) && !isnan (v01.x) && !isnan (v10.x))
+ {
+ v00.y = vmap.ptr (v + rows)[u];
+ v01.y = vmap.ptr (v + rows)[u + 1];
+ v10.y = vmap.ptr (v + 1 + rows)[u];
+
+ v00.z = vmap.ptr (v + 2 * rows)[u];
+ v01.z = vmap.ptr (v + 2 * rows)[u + 1];
+ v10.z = vmap.ptr (v + 1 + 2 * rows)[u];
+
+ float3 r = normalized (cross (v01 - v00, v10 - v00));
+
+ nmap.ptr (v )[u] = r.x;
+ nmap.ptr (v + rows)[u] = r.y;
+ nmap.ptr (v + 2 * rows)[u] = r.z;
+ }
+ else
+ nmap.ptr (v)[u] = numeric_limits<float>::quiet_NaN ();
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::device::createVMap (const Intr& intr, const DepthMap& depth, MapArr& vmap)
+{
+ vmap.create (depth.rows () * 3, depth.cols ());
+
+ dim3 block (32, 8);
+ dim3 grid (1, 1, 1);
+ grid.x = divUp (depth.cols (), block.x);
+ grid.y = divUp (depth.rows (), block.y);
+
+ float fx = intr.fx, cx = intr.cx;
+ float fy = intr.fy, cy = intr.cy;
+
+ computeVmapKernel<<<grid, block>>>(depth, vmap, 1.f / fx, 1.f / fy, cx, cy);
+ cudaSafeCall (cudaGetLastError ());
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::device::createNMap (const MapArr& vmap, MapArr& nmap)
+{
+ nmap.create (vmap.rows (), vmap.cols ());
+
+ int rows = vmap.rows () / 3;
+ int cols = vmap.cols ();
+
+ dim3 block (32, 8);
+ dim3 grid (1, 1, 1);
+ grid.x = divUp (cols, block.x);
+ grid.y = divUp (rows, block.y);
+
+ computeNmapKernel<<<grid, block>>>(rows, cols, vmap, nmap);
+ cudaSafeCall (cudaGetLastError ());
+}
+
+namespace pcl
+{
+ namespace device
+ {
+ __global__ void
+ tranformMapsKernel (int rows, int cols, const PtrStep<float> vmap_src, const PtrStep<float> nmap_src,
+ const Mat33 Rmat, const float3 tvec, PtrStepSz<float> vmap_dst, PtrStep<float> nmap_dst)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ const float qnan = pcl::device::numeric_limits<float>::quiet_NaN ();
+
+ if (x < cols && y < rows)
+ {
+ //vetexes
+ float3 vsrc, vdst = make_float3 (qnan, qnan, qnan);
+ vsrc.x = vmap_src.ptr (y)[x];
+
+ if (!isnan (vsrc.x))
+ {
+ vsrc.y = vmap_src.ptr (y + rows)[x];
+ vsrc.z = vmap_src.ptr (y + 2 * rows)[x];
+
+ vdst = Rmat * vsrc + tvec;
+
+ vmap_dst.ptr (y + rows)[x] = vdst.y;
+ vmap_dst.ptr (y + 2 * rows)[x] = vdst.z;
+ }
+
+ vmap_dst.ptr (y)[x] = vdst.x;
+
+ //normals
+ float3 nsrc, ndst = make_float3 (qnan, qnan, qnan);
+ nsrc.x = nmap_src.ptr (y)[x];
+
+ if (!isnan (nsrc.x))
+ {
+ nsrc.y = nmap_src.ptr (y + rows)[x];
+ nsrc.z = nmap_src.ptr (y + 2 * rows)[x];
+
+ ndst = Rmat * nsrc;
+
+ nmap_dst.ptr (y + rows)[x] = ndst.y;
+ nmap_dst.ptr (y + 2 * rows)[x] = ndst.z;
+ }
+
+ nmap_dst.ptr (y)[x] = ndst.x;
+ }
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::device::tranformMaps (const MapArr& vmap_src, const MapArr& nmap_src,
+ const Mat33& Rmat, const float3& tvec,
+ MapArr& vmap_dst, MapArr& nmap_dst)
+{
+ int cols = vmap_src.cols ();
+ int rows = vmap_src.rows () / 3;
+
+ vmap_dst.create (rows * 3, cols);
+ nmap_dst.create (rows * 3, cols);
+
+ dim3 block (32, 8);
+ dim3 grid (1, 1, 1);
+ grid.x = divUp (cols, block.x);
+ grid.y = divUp (rows, block.y);
+
+ tranformMapsKernel<<<grid, block>>>(rows, cols, vmap_src, nmap_src, Rmat, tvec, vmap_dst, nmap_dst);
+ cudaSafeCall (cudaGetLastError ());
+
+ cudaSafeCall (cudaDeviceSynchronize ());
+}
+
+namespace pcl
+{
+ namespace device
+ {
+ template<bool normalize>
+ __global__ void
+ resizeMapKernel (int drows, int dcols, int srows, const PtrStep<float> input, PtrStep<float> output)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x >= dcols || y >= drows)
+ return;
+
+ const float qnan = numeric_limits<float>::quiet_NaN ();
+
+ int xs = x * 2;
+ int ys = y * 2;
+
+ float x00 = input.ptr (ys + 0)[xs + 0];
+ float x01 = input.ptr (ys + 0)[xs + 1];
+ float x10 = input.ptr (ys + 1)[xs + 0];
+ float x11 = input.ptr (ys + 1)[xs + 1];
+
+ if (isnan (x00) || isnan (x01) || isnan (x10) || isnan (x11))
+ {
+ output.ptr (y)[x] = qnan;
+ return;
+ }
+ else
+ {
+ float3 n;
+
+ n.x = (x00 + x01 + x10 + x11) / 4;
+
+ float y00 = input.ptr (ys + srows + 0)[xs + 0];
+ float y01 = input.ptr (ys + srows + 0)[xs + 1];
+ float y10 = input.ptr (ys + srows + 1)[xs + 0];
+ float y11 = input.ptr (ys + srows + 1)[xs + 1];
+
+ n.y = (y00 + y01 + y10 + y11) / 4;
+
+ float z00 = input.ptr (ys + 2 * srows + 0)[xs + 0];
+ float z01 = input.ptr (ys + 2 * srows + 0)[xs + 1];
+ float z10 = input.ptr (ys + 2 * srows + 1)[xs + 0];
+ float z11 = input.ptr (ys + 2 * srows + 1)[xs + 1];
+
+ n.z = (z00 + z01 + z10 + z11) / 4;
+
+ if (normalize)
+ n = normalized (n);
+
+ output.ptr (y )[x] = n.x;
+ output.ptr (y + drows)[x] = n.y;
+ output.ptr (y + 2 * drows)[x] = n.z;
+ }
+ }
+
+ template<bool normalize>
+ void
+ resizeMap (const MapArr& input, MapArr& output)
+ {
+ int in_cols = input.cols ();
+ int in_rows = input.rows () / 3;
+
+ int out_cols = in_cols / 2;
+ int out_rows = in_rows / 2;
+
+ output.create (out_rows * 3, out_cols);
+
+ dim3 block (32, 8);
+ dim3 grid (divUp (out_cols, block.x), divUp (out_rows, block.y));
+ resizeMapKernel<normalize><< < grid, block>>>(out_rows, out_cols, in_rows, input, output);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::device::resizeVMap (const MapArr& input, MapArr& output)
+{
+ resizeMap<false>(input, output);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::device::resizeNMap (const MapArr& input, MapArr& output)
+{
+ resizeMap<true>(input, output);
+}
+
+namespace pcl
+{
+ namespace device
+ {
+
+ template<typename T>
+ __global__ void
+ convertMapKernel (int rows, int cols, const PtrStep<float> map, PtrStep<T> output)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x >= cols || y >= rows)
+ return;
+
+ const float qnan = numeric_limits<float>::quiet_NaN ();
+
+ T t;
+ t.x = map.ptr (y)[x];
+ if (!isnan (t.x))
+ {
+ t.y = map.ptr (y + rows)[x];
+ t.z = map.ptr (y + 2 * rows)[x];
+ }
+ else
+ t.y = t.z = qnan;
+
+ output.ptr (y)[x] = t;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename T> void
+pcl::device::convert (const MapArr& vmap, DeviceArray2D<T>& output)
+{
+ int cols = vmap.cols ();
+ int rows = vmap.rows () / 3;
+
+ output.create (rows, cols);
+
+ dim3 block (32, 8);
+ dim3 grid (divUp (cols, block.x), divUp (rows, block.y));
+
+ convertMapKernel<T><< < grid, block>>>(rows, cols, vmap, output);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+}
+
+template void pcl::device::convert (const MapArr& vmap, DeviceArray2D<float4>& output);
+template void pcl::device::convert (const MapArr& vmap, DeviceArray2D<float8>& output);
+
+namespace pcl
+{
+ namespace device
+ {
+ __global__ void
+ mergePointNormalKernel (const float4* cloud, const float8* normals, PtrSz<float12> output)
+ {
+ int idx = threadIdx.x + blockIdx.x * blockDim.x;
+
+ if (idx < output.size)
+ {
+ float4 p = cloud[idx];
+ float8 n = normals[idx];
+
+ float12 o;
+ o.x = p.x;
+ o.y = p.y;
+ o.z = p.z;
+
+ o.normal_x = n.x;
+ o.normal_y = n.y;
+ o.normal_z = n.z;
+
+ output.data[idx] = o;
+ }
+ }
+ }
+}
+
+void
+pcl::device::mergePointNormal (const DeviceArray<float4>& cloud, const DeviceArray<float8>& normals, const DeviceArray<float12>& output)
+{
+ const int block = 256;
+ int total = (int)output.size ();
+
+ mergePointNormalKernel<<<divUp (total, block), block>>>(cloud, normals, output);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+//#include "pcl/gpu/utils/device/block.hpp"
+//#include "pcl/gpu/utils/device/warp.hpp"
+//#include "pcl/gpu/utils/device/vector_math.hpp"
+
+#include "thrust/device_ptr.h"
+#include "thrust/scan.h"
+
+namespace pcl
+{
+ namespace device
+ {
+ //texture<int, 1, cudaReadModeElementType> edgeTex;
+ texture<int, 1, cudaReadModeElementType> triTex;
+ texture<int, 1, cudaReadModeElementType> numVertsTex;
+ }
+}
+
+void
+pcl::device::bindTextures (const int */*edgeBuf*/, const int *triBuf, const int *numVertsBuf)
+{
+ cudaChannelFormatDesc desc = cudaCreateChannelDesc<int>();
+ //cudaSafeCall(cudaBindTexture(0, edgeTex, edgeBuf, desc) );
+ cudaSafeCall (cudaBindTexture (0, triTex, triBuf, desc) );
+ cudaSafeCall (cudaBindTexture (0, numVertsTex, numVertsBuf, desc) );
+}
+void
+pcl::device::unbindTextures ()
+{
+ //cudaSafeCall( cudaUnbindTexture(edgeTex) );
+ cudaSafeCall ( cudaUnbindTexture (numVertsTex) );
+ cudaSafeCall ( cudaUnbindTexture (triTex) );
+}
+
+namespace pcl
+{
+ namespace device
+ {
+ __device__ int global_count = 0;
+ __device__ int output_count;
+ __device__ unsigned int blocks_done = 0;
+
+ struct CubeIndexEstimator
+ {
+ PtrStep<short2> volume;
+
+ static __device__ __forceinline__ float isoValue() { return 0.f; }
+
+ __device__ __forceinline__ void
+ readTsdf (int x, int y, int z, float& tsdf, int& weight) const
+ {
+ unpack_tsdf (volume.ptr (VOLUME_Y * z + y)[x], tsdf, weight);
+ }
+
+ __device__ __forceinline__ int
+ computeCubeIndex (int x, int y, int z, float f[8]) const
+ {
+ int weight;
+ readTsdf (x, y, z, f[0], weight); if (weight == 0) return 0;
+ readTsdf (x + 1, y, z, f[1], weight); if (weight == 0) return 0;
+ readTsdf (x + 1, y + 1, z, f[2], weight); if (weight == 0) return 0;
+ readTsdf (x, y + 1, z, f[3], weight); if (weight == 0) return 0;
+ readTsdf (x, y, z + 1, f[4], weight); if (weight == 0) return 0;
+ readTsdf (x + 1, y, z + 1, f[5], weight); if (weight == 0) return 0;
+ readTsdf (x + 1, y + 1, z + 1, f[6], weight); if (weight == 0) return 0;
+ readTsdf (x, y + 1, z + 1, f[7], weight); if (weight == 0) return 0;
+
+ // calculate flag indicating if each vertex is inside or outside isosurface
+ int cubeindex;
+ cubeindex = int(f[0] < isoValue());
+ cubeindex += int(f[1] < isoValue()) * 2;
+ cubeindex += int(f[2] < isoValue()) * 4;
+ cubeindex += int(f[3] < isoValue()) * 8;
+ cubeindex += int(f[4] < isoValue()) * 16;
+ cubeindex += int(f[5] < isoValue()) * 32;
+ cubeindex += int(f[6] < isoValue()) * 64;
+ cubeindex += int(f[7] < isoValue()) * 128;
+
+ return cubeindex;
+ }
+ };
+
+ struct OccupiedVoxels : public CubeIndexEstimator
+ {
+ enum
+ {
+ CTA_SIZE_X = 32,
+ CTA_SIZE_Y = 8,
+ CTA_SIZE = CTA_SIZE_X * CTA_SIZE_Y,
+
+ WARPS_COUNT = CTA_SIZE / Warp::WARP_SIZE
+ };
+
+ mutable int* voxels_indeces;
+ mutable int* vetexes_number;
+ int max_size;
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int x = threadIdx.x + blockIdx.x * CTA_SIZE_X;
+ int y = threadIdx.y + blockIdx.y * CTA_SIZE_Y;
+
+#if __CUDA_ARCH__ < 200
+ __shared__ int cta_buffer[CTA_SIZE];
+#endif
+
+
+#if __CUDA_ARCH__ >= 120
+ if (__all (x >= VOLUME_X) || __all (y >= VOLUME_Y))
+ return;
+#else
+ if (Emulation::All(x >= VOLUME_X, cta_buffer) ||
+ Emulation::All(y >= VOLUME_Y, cta_buffer))
+ return;
+#endif
+
+ int ftid = Block::flattenedThreadId ();
+ int warp_id = Warp::id();
+ int lane_id = Warp::laneId();
+
+ volatile __shared__ int warps_buffer[WARPS_COUNT];
+
+ for (int z = 0; z < VOLUME_Z - 1; z++)
+ {
+ int numVerts = 0;;
+ if (x + 1 < VOLUME_X && y + 1 < VOLUME_Y)
+ {
+ float field[8];
+ int cubeindex = computeCubeIndex (x, y, z, field);
+
+ // read number of vertices from texture
+ numVerts = (cubeindex == 0 || cubeindex == 255) ? 0 : tex1Dfetch (numVertsTex, cubeindex);
+ }
+#if __CUDA_ARCH__ >= 200
+ int total = __popc (__ballot (numVerts > 0));
+#else
+ int total = __popc (Emulation::Ballot(numVerts > 0, cta_buffer));
+#endif
+ if (total == 0)
+ continue;
+
+ if (lane_id == 0)
+ {
+ int old = atomicAdd (&global_count, total);
+ warps_buffer[warp_id] = old;
+ }
+ int old_global_voxels_count = warps_buffer[warp_id];
+
+#if __CUDA_ARCH__ >= 200
+ int offs = Warp::binaryExclScan (__ballot (numVerts > 0));
+#else
+ int offs = Warp::binaryExclScan(Emulation::Ballot(numVerts > 0, cta_buffer));
+#endif
+
+ if (old_global_voxels_count + offs < max_size && numVerts > 0)
+ {
+ voxels_indeces[old_global_voxels_count + offs] = VOLUME_Y * VOLUME_X * z + VOLUME_X * y + x;
+ vetexes_number[old_global_voxels_count + offs] = numVerts;
+ }
+
+ bool full = old_global_voxels_count + total >= max_size;
+
+ if (full)
+ break;
+
+ } /* for(int z = 0; z < VOLUME_Z - 1; z++) */
+
+
+ /////////////////////////
+ // prepare for future scans
+ if (ftid == 0)
+ {
+ unsigned int total_blocks = gridDim.x * gridDim.y * gridDim.z;
+ unsigned int value = atomicInc (&blocks_done, total_blocks);
+
+ //last block
+ if (value == total_blocks - 1)
+ {
+ output_count = min (max_size, global_count);
+ blocks_done = 0;
+ global_count = 0;
+ }
+ }
+ } /* operator () */
+ };
+ __global__ void getOccupiedVoxelsKernel (const OccupiedVoxels ov) { ov (); }
+ }
+}
+
+int
+pcl::device::getOccupiedVoxels (const PtrStep<short2>& volume, DeviceArray2D<int>& occupied_voxels)
+{
+ OccupiedVoxels ov;
+ ov.volume = volume;
+
+ ov.voxels_indeces = occupied_voxels.ptr (0);
+ ov.vetexes_number = occupied_voxels.ptr (1);
+ ov.max_size = occupied_voxels.cols ();
+
+ dim3 block (OccupiedVoxels::CTA_SIZE_X, OccupiedVoxels::CTA_SIZE_Y);
+ dim3 grid (divUp (VOLUME_X, block.x), divUp (VOLUME_Y, block.y));
+
+ //cudaFuncSetCacheConfig(getOccupiedVoxelsKernel, cudaFuncCachePreferL1);
+ //printFuncAttrib(getOccupiedVoxelsKernel);
+
+ getOccupiedVoxelsKernel<<<grid, block>>>(ov);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+
+ int size;
+ cudaSafeCall ( cudaMemcpyFromSymbol (&size, output_count, sizeof(size)) );
+ return size;
+}
+
+int
+pcl::device::computeOffsetsAndTotalVertexes (DeviceArray2D<int>& occupied_voxels)
+{
+ thrust::device_ptr<int> beg (occupied_voxels.ptr (1));
+ thrust::device_ptr<int> end = beg + occupied_voxels.cols ();
+
+ thrust::device_ptr<int> out (occupied_voxels.ptr (2));
+ thrust::exclusive_scan (beg, end, out);
+
+ int lastElement, lastScanElement;
+
+ DeviceArray<int> last_elem (occupied_voxels.ptr(1) + occupied_voxels.cols () - 1, 1);
+ DeviceArray<int> last_scan (occupied_voxels.ptr(2) + occupied_voxels.cols () - 1, 1);
+
+ last_elem.download (&lastElement);
+ last_scan.download (&lastScanElement);
+
+ return lastElement + lastScanElement;
+}
+
+
+namespace pcl
+{
+ namespace device
+ {
+ struct TrianglesGenerator : public CubeIndexEstimator
+ {
+#if __CUDA_ARCH__ >= 200
+ enum { CTA_SIZE = 256, MAX_GRID_SIZE_X = 65536 };
+#else
+ enum { CTA_SIZE = 96, MAX_GRID_SIZE_X = 65536 };
+#endif
+
+ const int* occupied_voxels;
+ const int* vertex_ofssets;
+ int voxels_count;
+ float3 cell_size;
+
+ mutable PointType *output;
+
+ __device__ __forceinline__ float3
+ getNodeCoo (int x, int y, int z) const
+ {
+ float3 coo = make_float3 (x, y, z);
+ coo += 0.5f; //shift to volume cell center;
+
+ coo.x *= cell_size.x;
+ coo.y *= cell_size.y;
+ coo.z *= cell_size.z;
+
+ return coo;
+ }
+
+ __device__ __forceinline__ float3
+ vertex_interp (float3 p0, float3 p1, float f0, float f1) const
+ {
+ float t = (isoValue() - f0) / (f1 - f0 + 1e-15f);
+ float x = p0.x + t * (p1.x - p0.x);
+ float y = p0.y + t * (p1.y - p0.y);
+ float z = p0.z + t * (p1.z - p0.z);
+ return make_float3 (x, y, z);
+ }
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int tid = threadIdx.x;
+ int idx = (blockIdx.y * MAX_GRID_SIZE_X + blockIdx.x) * CTA_SIZE + tid;
+
+
+ if (idx >= voxels_count)
+ return;
+
+ int voxel = occupied_voxels[idx];
+
+ int z = voxel / (VOLUME_X * VOLUME_Y);
+ int y = (voxel - z * VOLUME_X * VOLUME_Y) / VOLUME_X;
+ int x = (voxel - z * VOLUME_X * VOLUME_Y) - y * VOLUME_X;
+
+ float f[8];
+ int cubeindex = computeCubeIndex (x, y, z, f);
+
+ // calculate cell vertex positions
+ float3 v[8];
+ v[0] = getNodeCoo (x, y, z);
+ v[1] = getNodeCoo (x + 1, y, z);
+ v[2] = getNodeCoo (x + 1, y + 1, z);
+ v[3] = getNodeCoo (x, y + 1, z);
+ v[4] = getNodeCoo (x, y, z + 1);
+ v[5] = getNodeCoo (x + 1, y, z + 1);
+ v[6] = getNodeCoo (x + 1, y + 1, z + 1);
+ v[7] = getNodeCoo (x, y + 1, z + 1);
+
+ // find the vertices where the surface intersects the cube
+ // use shared memory to avoid using local
+ __shared__ float3 vertlist[12][CTA_SIZE];
+
+ vertlist[0][tid] = vertex_interp (v[0], v[1], f[0], f[1]);
+ vertlist[1][tid] = vertex_interp (v[1], v[2], f[1], f[2]);
+ vertlist[2][tid] = vertex_interp (v[2], v[3], f[2], f[3]);
+ vertlist[3][tid] = vertex_interp (v[3], v[0], f[3], f[0]);
+ vertlist[4][tid] = vertex_interp (v[4], v[5], f[4], f[5]);
+ vertlist[5][tid] = vertex_interp (v[5], v[6], f[5], f[6]);
+ vertlist[6][tid] = vertex_interp (v[6], v[7], f[6], f[7]);
+ vertlist[7][tid] = vertex_interp (v[7], v[4], f[7], f[4]);
+ vertlist[8][tid] = vertex_interp (v[0], v[4], f[0], f[4]);
+ vertlist[9][tid] = vertex_interp (v[1], v[5], f[1], f[5]);
+ vertlist[10][tid] = vertex_interp (v[2], v[6], f[2], f[6]);
+ vertlist[11][tid] = vertex_interp (v[3], v[7], f[3], f[7]);
+ __syncthreads ();
+
+ // output triangle vertices
+ int numVerts = tex1Dfetch (numVertsTex, cubeindex);
+
+ for (int i = 0; i < numVerts; i += 3)
+ {
+ int index = vertex_ofssets[idx] + i;
+
+ int v1 = tex1Dfetch (triTex, (cubeindex * 16) + i + 0);
+ int v2 = tex1Dfetch (triTex, (cubeindex * 16) + i + 1);
+ int v3 = tex1Dfetch (triTex, (cubeindex * 16) + i + 2);
+
+ store_point (output, index + 0, vertlist[v1][tid]);
+ store_point (output, index + 1, vertlist[v2][tid]);
+ store_point (output, index + 2, vertlist[v3][tid]);
+ }
+ }
+
+ __device__ __forceinline__ void
+ store_point (float4 *ptr, int index, const float3& point) const {
+ ptr[index] = make_float4 (point.x, point.y, point.z, 1.0f);
+ }
+ };
+ __global__ void
+ trianglesGeneratorKernel (const TrianglesGenerator tg) {tg (); }
+ }
+}
+
+
+void
+pcl::device::generateTriangles (const PtrStep<short2>& volume, const DeviceArray2D<int>& occupied_voxels, const float3& volume_size, DeviceArray<PointType>& output)
+{
+ int device;
+ cudaSafeCall( cudaGetDevice(&device) );
+
+ cudaDeviceProp prop;
+ cudaSafeCall( cudaGetDeviceProperties(&prop, device) );
+
+ int block_size = prop.major < 2 ? 96 : 256; // please see TrianglesGenerator::CTA_SIZE
+
+ typedef TrianglesGenerator Tg;
+ Tg tg;
+
+ tg.volume = volume;
+ tg.occupied_voxels = occupied_voxels.ptr (0);
+ tg.vertex_ofssets = occupied_voxels.ptr (2);
+ tg.voxels_count = occupied_voxels.cols ();
+ tg.cell_size.x = volume_size.x / VOLUME_X;
+ tg.cell_size.y = volume_size.y / VOLUME_Y;
+ tg.cell_size.z = volume_size.z / VOLUME_Z;
+ tg.output = output;
+
+ int blocks_num = divUp (tg.voxels_count, block_size);
+
+ dim3 block (block_size);
+ dim3 grid(min(blocks_num, Tg::MAX_GRID_SIZE_X), divUp(blocks_num, Tg::MAX_GRID_SIZE_X));
+
+ trianglesGeneratorKernel<<<grid, block>>>(tg);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+}
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+//#include <pcl/gpu/features/device/eigen.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+ enum
+ {
+ kx = 7,
+ ky = 7,
+ STEP = 1
+ };
+
+ __global__ void
+ computeNmapKernelEigen (int rows, int cols, const PtrStep<float> vmap, PtrStep<float> nmap)
+ {
+ int u = threadIdx.x + blockIdx.x * blockDim.x;
+ int v = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (u >= cols || v >= rows)
+ return;
+
+ nmap.ptr (v)[u] = numeric_limits<float>::quiet_NaN ();
+
+ if (isnan (vmap.ptr (v)[u]))
+ return;
+
+ int ty = min (v - ky / 2 + ky, rows - 1);
+ int tx = min (u - kx / 2 + kx, cols - 1);
+
+ float3 centroid = make_float3 (0.f, 0.f, 0.f);
+ int counter = 0;
+ for (int cy = max (v - ky / 2, 0); cy < ty; cy += STEP)
+ for (int cx = max (u - kx / 2, 0); cx < tx; cx += STEP)
+ {
+ float v_x = vmap.ptr (cy)[cx];
+ if (!isnan (v_x))
+ {
+ centroid.x += v_x;
+ centroid.y += vmap.ptr (cy + rows)[cx];
+ centroid.z += vmap.ptr (cy + 2 * rows)[cx];
+ ++counter;
+ }
+ }
+
+ if (counter < kx * ky / 2)
+ return;
+
+ centroid *= 1.f / counter;
+
+ float cov[] = {0, 0, 0, 0, 0, 0};
+
+ for (int cy = max (v - ky / 2, 0); cy < ty; cy += STEP)
+ for (int cx = max (u - kx / 2, 0); cx < tx; cx += STEP)
+ {
+ float3 v;
+ v.x = vmap.ptr (cy)[cx];
+ if (isnan (v.x))
+ continue;
+
+ v.y = vmap.ptr (cy + rows)[cx];
+ v.z = vmap.ptr (cy + 2 * rows)[cx];
+
+ float3 d = v - centroid;
+
+ cov[0] += d.x * d.x; //cov (0, 0)
+ cov[1] += d.x * d.y; //cov (0, 1)
+ cov[2] += d.x * d.z; //cov (0, 2)
+ cov[3] += d.y * d.y; //cov (1, 1)
+ cov[4] += d.y * d.z; //cov (1, 2)
+ cov[5] += d.z * d.z; //cov (2, 2)
+ }
+
+ typedef Eigen33::Mat33 Mat33;
+ Eigen33 eigen33 (cov);
+
+ Mat33 tmp;
+ Mat33 vec_tmp;
+ Mat33 evecs;
+ float3 evals;
+ eigen33.compute (tmp, vec_tmp, evecs, evals);
+
+ float3 n = normalized (evecs[0]);
+
+ u = threadIdx.x + blockIdx.x * blockDim.x;
+ v = threadIdx.y + blockIdx.y * blockDim.y;
+
+ nmap.ptr (v )[u] = n.x;
+ nmap.ptr (v + rows)[u] = n.y;
+ nmap.ptr (v + 2 * rows)[u] = n.z;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::device::computeNormalsEigen (const MapArr& vmap, MapArr& nmap)
+{
+ int cols = vmap.cols ();
+ int rows = vmap.rows () / 3;
+
+ nmap.create (vmap.rows (), vmap.cols ());
+
+ dim3 block (32, 8);
+ dim3 grid (1, 1, 1);
+ grid.x = divUp (cols, block.x);
+ grid.y = divUp (rows, block.y);
+
+ computeNmapKernelEigen<<<grid, block>>>(rows, cols, vmap, nmap);
+ cudaSafeCall (cudaGetLastError ());
+ cudaSafeCall (cudaDeviceSynchronize ());
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+#include "device.hpp"
+
+namespace pcl
+{
+ namespace device
+ {
+ __device__ __forceinline__ float
+ getMinTime (const float3& volume_max, const float3& origin, const float3& dir)
+ {
+ float txmin = ( (dir.x > 0 ? 0.f : volume_max.x) - origin.x) / dir.x;
+ float tymin = ( (dir.y > 0 ? 0.f : volume_max.y) - origin.y) / dir.y;
+ float tzmin = ( (dir.z > 0 ? 0.f : volume_max.z) - origin.z) / dir.z;
+
+ return fmax ( fmax (txmin, tymin), tzmin);
+ }
+
+ __device__ __forceinline__ float
+ getMaxTime (const float3& volume_max, const float3& origin, const float3& dir)
+ {
+ float txmax = ( (dir.x > 0 ? volume_max.x : 0.f) - origin.x) / dir.x;
+ float tymax = ( (dir.y > 0 ? volume_max.y : 0.f) - origin.y) / dir.y;
+ float tzmax = ( (dir.z > 0 ? volume_max.z : 0.f) - origin.z) / dir.z;
+
+ return fmin (fmin (txmax, tymax), tzmax);
+ }
+
+ struct RayCaster
+ {
+ enum { CTA_SIZE_X = 32, CTA_SIZE_Y = 8 };
+
+ Mat33 Rcurr;
+ float3 tcurr;
+
+ float time_step;
+ float3 volume_size;
+
+ float3 cell_size;
+ int cols, rows;
+
+ PtrStep<short2> volume;
+
+ Intr intr;
+
+ mutable PtrStep<float> nmap;
+ mutable PtrStep<float> vmap;
+
+ __device__ __forceinline__ float3
+ get_ray_next (int x, int y) const
+ {
+ float3 ray_next;
+ ray_next.x = (x - intr.cx) / intr.fx;
+ ray_next.y = (y - intr.cy) / intr.fy;
+ ray_next.z = 1;
+ return ray_next;
+ }
+
+ __device__ __forceinline__ bool
+ checkInds (const int3& g) const
+ {
+ return (g.x >= 0 && g.y >= 0 && g.z >= 0 && g.x < VOLUME_X && g.y < VOLUME_Y && g.z < VOLUME_Z);
+ }
+
+ __device__ __forceinline__ float
+ readTsdf (int x, int y, int z) const
+ {
+ return unpack_tsdf (volume.ptr (VOLUME_Y * z + y)[x]);
+ }
+
+ __device__ __forceinline__ int3
+ getVoxel (float3 point) const
+ {
+ int vx = __float2int_rd (point.x / cell_size.x); // round to negative infinity
+ int vy = __float2int_rd (point.y / cell_size.y);
+ int vz = __float2int_rd (point.z / cell_size.z);
+
+ return make_int3 (vx, vy, vz);
+ }
+
+ __device__ __forceinline__ float
+ interpolateTrilineary (const float3& origin, const float3& dir, float time) const
+ {
+ return interpolateTrilineary (origin + dir * time);
+ }
+
+ __device__ __forceinline__ float
+ interpolateTrilineary (const float3& point) const
+ {
+ int3 g = getVoxel (point);
+
+ if (g.x <= 0 || g.x >= VOLUME_X - 1)
+ return numeric_limits<float>::quiet_NaN ();
+
+ if (g.y <= 0 || g.y >= VOLUME_Y - 1)
+ return numeric_limits<float>::quiet_NaN ();
+
+ if (g.z <= 0 || g.z >= VOLUME_Z - 1)
+ return numeric_limits<float>::quiet_NaN ();
+
+ float vx = (g.x + 0.5f) * cell_size.x;
+ float vy = (g.y + 0.5f) * cell_size.y;
+ float vz = (g.z + 0.5f) * cell_size.z;
+
+ g.x = (point.x < vx) ? (g.x - 1) : g.x;
+ g.y = (point.y < vy) ? (g.y - 1) : g.y;
+ g.z = (point.z < vz) ? (g.z - 1) : g.z;
+
+ float a = (point.x - (g.x + 0.5f) * cell_size.x) / cell_size.x;
+ float b = (point.y - (g.y + 0.5f) * cell_size.y) / cell_size.y;
+ float c = (point.z - (g.z + 0.5f) * cell_size.z) / cell_size.z;
+
+ float res = readTsdf (g.x + 0, g.y + 0, g.z + 0) * (1 - a) * (1 - b) * (1 - c) +
+ readTsdf (g.x + 0, g.y + 0, g.z + 1) * (1 - a) * (1 - b) * c +
+ readTsdf (g.x + 0, g.y + 1, g.z + 0) * (1 - a) * b * (1 - c) +
+ readTsdf (g.x + 0, g.y + 1, g.z + 1) * (1 - a) * b * c +
+ readTsdf (g.x + 1, g.y + 0, g.z + 0) * a * (1 - b) * (1 - c) +
+ readTsdf (g.x + 1, g.y + 0, g.z + 1) * a * (1 - b) * c +
+ readTsdf (g.x + 1, g.y + 1, g.z + 0) * a * b * (1 - c) +
+ readTsdf (g.x + 1, g.y + 1, g.z + 1) * a * b * c;
+ return res;
+ }
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int x = threadIdx.x + blockIdx.x * CTA_SIZE_X;
+ int y = threadIdx.y + blockIdx.y * CTA_SIZE_Y;
+
+ if (x >= cols || y >= rows)
+ return;
+
+ vmap.ptr (y)[x] = numeric_limits<float>::quiet_NaN ();
+ nmap.ptr (y)[x] = numeric_limits<float>::quiet_NaN ();
+
+ float3 ray_start = tcurr;
+ float3 ray_next = Rcurr * get_ray_next (x, y) + tcurr;
+
+ float3 ray_dir = normalized (ray_next - ray_start);
+
+ //ensure that it isn't a degenerate case
+ ray_dir.x = (ray_dir.x == 0.f) ? 1e-15 : ray_dir.x;
+ ray_dir.y = (ray_dir.y == 0.f) ? 1e-15 : ray_dir.y;
+ ray_dir.z = (ray_dir.z == 0.f) ? 1e-15 : ray_dir.z;
+
+ // computer time when entry and exit volume
+ float time_start_volume = getMinTime (volume_size, ray_start, ray_dir);
+ float time_exit_volume = getMaxTime (volume_size, ray_start, ray_dir);
+
+ const float min_dist = 0.f; //in meters
+ time_start_volume = fmax (time_start_volume, min_dist);
+ if (time_start_volume >= time_exit_volume)
+ return;
+
+ float time_curr = time_start_volume;
+ int3 g = getVoxel (ray_start + ray_dir * time_curr);
+ g.x = max (0, min (g.x, VOLUME_X - 1));
+ g.y = max (0, min (g.y, VOLUME_Y - 1));
+ g.z = max (0, min (g.z, VOLUME_Z - 1));
+
+ float tsdf = readTsdf (g.x, g.y, g.z);
+
+ //infinite loop guard
+ const float max_time = 3 * (volume_size.x + volume_size.y + volume_size.z);
+
+ for (; time_curr < max_time; time_curr += time_step)
+ {
+ float tsdf_prev = tsdf;
+
+ int3 g = getVoxel ( ray_start + ray_dir * (time_curr + time_step) );
+ if (!checkInds (g))
+ break;
+
+ tsdf = readTsdf (g.x, g.y, g.z);
+
+ if (tsdf_prev < 0.f && tsdf > 0.f)
+ break;
+
+ if (tsdf_prev > 0.f && tsdf < 0.f) //zero crossing
+ {
+ float Ftdt = interpolateTrilineary (ray_start, ray_dir, time_curr + time_step);
+ if (isnan (Ftdt))
+ break;
+
+ float Ft = interpolateTrilineary (ray_start, ray_dir, time_curr);
+ if (isnan (Ft))
+ break;
+
+ //float Ts = time_curr - time_step * Ft/(Ftdt - Ft);
+ float Ts = time_curr - time_step * Ft / (Ftdt - Ft);
+
+ float3 vetex_found = ray_start + ray_dir * Ts;
+
+ vmap.ptr (y )[x] = vetex_found.x;
+ vmap.ptr (y + rows)[x] = vetex_found.y;
+ vmap.ptr (y + 2 * rows)[x] = vetex_found.z;
+
+ int3 g = getVoxel ( ray_start + ray_dir * time_curr );
+ if (g.x > 1 && g.y > 1 && g.z > 1 && g.x < VOLUME_X - 2 && g.y < VOLUME_Y - 2 && g.z < VOLUME_Z - 2)
+ {
+ float3 t;
+ float3 n;
+
+ t = vetex_found;
+ t.x += cell_size.x;
+ float Fx1 = interpolateTrilineary (t);
+
+ t = vetex_found;
+ t.x -= cell_size.x;
+ float Fx2 = interpolateTrilineary (t);
+
+ n.x = (Fx1 - Fx2);
+
+ t = vetex_found;
+ t.y += cell_size.y;
+ float Fy1 = interpolateTrilineary (t);
+
+ t = vetex_found;
+ t.y -= cell_size.y;
+ float Fy2 = interpolateTrilineary (t);
+
+ n.y = (Fy1 - Fy2);
+
+ t = vetex_found;
+ t.z += cell_size.z;
+ float Fz1 = interpolateTrilineary (t);
+
+ t = vetex_found;
+ t.z -= cell_size.z;
+ float Fz2 = interpolateTrilineary (t);
+
+ n.z = (Fz1 - Fz2);
+
+ n = normalized (n);
+
+ nmap.ptr (y )[x] = n.x;
+ nmap.ptr (y + rows)[x] = n.y;
+ nmap.ptr (y + 2 * rows)[x] = n.z;
+ }
+ break;
+ }
+
+ } /* for(;;) */
+ }
+ };
+
+ __global__ void
+ rayCastKernel (const RayCaster rc) {
+ rc ();
+ }
+ }
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::device::raycast (const Intr& intr, const Mat33& Rcurr, const float3& tcurr,
+ float tranc_dist, const float3& volume_size,
+ const PtrStep<short2>& volume, MapArr& vmap, MapArr& nmap)
+{
+ RayCaster rc;
+
+ rc.Rcurr = Rcurr;
+ rc.tcurr = tcurr;
+
+ rc.time_step = tranc_dist * 0.8f;
+
+ rc.volume_size = volume_size;
+
+ rc.cell_size.x = volume_size.x / VOLUME_X;
+ rc.cell_size.y = volume_size.y / VOLUME_Y;
+ rc.cell_size.z = volume_size.z / VOLUME_Z;
+
+ rc.cols = vmap.cols ();
+ rc.rows = vmap.rows () / 3;
+
+ rc.intr = intr;
+
+ rc.volume = volume;
+ rc.vmap = vmap;
+ rc.nmap = nmap;
+
+ dim3 block (RayCaster::CTA_SIZE_X, RayCaster::CTA_SIZE_Y);
+ dim3 grid (divUp (rc.cols, block.x), divUp (rc.rows, block.y));
+
+ rayCastKernel<<<grid, block>>>(rc);
+ cudaSafeCall (cudaGetLastError ());
+ //cudaSafeCall(cudaDeviceSynchronize());
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+
+using namespace pcl::device;
+
+namespace pcl
+{
+ namespace device
+ {
+ template<typename T>
+ __global__ void
+ initializeVolume (PtrStep<T> volume)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+
+ if (x < VOLUME_X && y < VOLUME_Y)
+ {
+ T *pos = volume.ptr(y) + x;
+ int z_step = VOLUME_Y * volume.step / sizeof(*pos);
+
+#pragma unroll
+ for(int z = 0; z < VOLUME_Z; ++z, pos+=z_step)
+ pack_tsdf (0.f, 0, *pos);
+ }
+ }
+ }
+}
+
+void
+pcl::device::initVolume (PtrStep<short2> volume)
+{
+ dim3 block (16, 16);
+ dim3 grid (1, 1, 1);
+ grid.x = divUp (VOLUME_X, block.x);
+ grid.y = divUp (VOLUME_Y, block.y);
+
+ initializeVolume<<<grid, block>>>(volume);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+}
+
+namespace pcl
+{
+ namespace device
+ {
+ struct Tsdf
+ {
+ enum
+ {
+ CTA_SIZE_X = 32, CTA_SIZE_Y = 8,
+ MAX_WEIGHT = 1 << 7
+ };
+
+ mutable PtrStep<short2> volume;
+ float3 cell_size;
+
+ Intr intr;
+
+ Mat33 Rcurr_inv;
+ float3 tcurr;
+
+ PtrStepSz<ushort> depth_raw; //depth in mm
+
+ float tranc_dist_mm;
+
+ __device__ __forceinline__ float3
+ getVoxelGCoo (int x, int y, int z) const
+ {
+ float3 coo = make_float3 (x, y, z);
+ coo += 0.5f; //shift to cell center;
+
+ coo.x *= cell_size.x;
+ coo.y *= cell_size.y;
+ coo.z *= cell_size.z;
+
+ return coo;
+ }
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int x = threadIdx.x + blockIdx.x * CTA_SIZE_X;
+ int y = threadIdx.y + blockIdx.y * CTA_SIZE_Y;
+
+ if (x >= VOLUME_X || y >= VOLUME_Y)
+ return;
+
+ short2 *pos = volume.ptr (y) + x;
+ int elem_step = volume.step * VOLUME_Y / sizeof(*pos);
+
+ for (int z = 0; z < VOLUME_Z; ++z, pos += elem_step)
+ {
+ float3 v_g = getVoxelGCoo (x, y, z); //3 // p
+
+ //tranform to curr cam coo space
+ float3 v = Rcurr_inv * (v_g - tcurr); //4
+
+ int2 coo; //project to current cam
+ coo.x = __float2int_rn (v.x * intr.fx / v.z + intr.cx);
+ coo.y = __float2int_rn (v.y * intr.fy / v.z + intr.cy);
+
+ if (v.z > 0 && coo.x >= 0 && coo.y >= 0 && coo.x < depth_raw.cols && coo.y < depth_raw.rows) //6
+ {
+ int Dp = depth_raw.ptr (coo.y)[coo.x];
+
+ if (Dp != 0)
+ {
+ float xl = (coo.x - intr.cx) / intr.fx;
+ float yl = (coo.y - intr.cy) / intr.fy;
+ float lambda_inv = rsqrtf (xl * xl + yl * yl + 1);
+
+ float sdf = 1000 * norm (tcurr - v_g) * lambda_inv - Dp; //mm
+
+ sdf *= (-1);
+
+ if (sdf >= -tranc_dist_mm)
+ {
+ float tsdf = fmin (1.f, sdf / tranc_dist_mm);
+
+ int weight_prev;
+ float tsdf_prev;
+
+ //read and unpack
+ unpack_tsdf (*pos, tsdf_prev, weight_prev);
+
+ const int Wrk = 1;
+
+ float tsdf_new = (tsdf_prev * weight_prev + Wrk * tsdf) / (weight_prev + Wrk);
+ int weight_new = min (weight_prev + Wrk, MAX_WEIGHT);
+
+ pack_tsdf (tsdf_new, weight_new, *pos);
+ }
+ }
+ }
+ }
+ }
+ };
+
+ __global__ void
+ integrateTsdfKernel (const Tsdf tsdf) {
+ tsdf ();
+ }
+
+ __global__ void
+ tsdf2 (PtrStep<short2> volume, const float tranc_dist_mm, const Mat33 Rcurr_inv, float3 tcurr,
+ const Intr intr, const PtrStepSz<ushort> depth_raw, const float3 cell_size)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x >= VOLUME_X || y >= VOLUME_Y)
+ return;
+
+ short2 *pos = volume.ptr (y) + x;
+ int elem_step = volume.step * VOLUME_Y / sizeof(short2);
+
+ float v_g_x = (x + 0.5f) * cell_size.x - tcurr.x;
+ float v_g_y = (y + 0.5f) * cell_size.y - tcurr.y;
+ float v_g_z = (0 + 0.5f) * cell_size.z - tcurr.z;
+
+ float v_x = Rcurr_inv.data[0].x * v_g_x + Rcurr_inv.data[0].y * v_g_y + Rcurr_inv.data[0].z * v_g_z;
+ float v_y = Rcurr_inv.data[1].x * v_g_x + Rcurr_inv.data[1].y * v_g_y + Rcurr_inv.data[1].z * v_g_z;
+ float v_z = Rcurr_inv.data[2].x * v_g_x + Rcurr_inv.data[2].y * v_g_y + Rcurr_inv.data[2].z * v_g_z;
+
+//#pragma unroll
+ for (int z = 0; z < VOLUME_Z; ++z)
+ {
+ float3 vr;
+ vr.x = v_g_x;
+ vr.y = v_g_y;
+ vr.z = (v_g_z + z * cell_size.z);
+
+ float3 v;
+ v.x = v_x + Rcurr_inv.data[0].z * z * cell_size.z;
+ v.y = v_y + Rcurr_inv.data[1].z * z * cell_size.z;
+ v.z = v_z + Rcurr_inv.data[2].z * z * cell_size.z;
+
+ int2 coo; //project to current cam
+ coo.x = __float2int_rn (v.x * intr.fx / v.z + intr.cx);
+ coo.y = __float2int_rn (v.y * intr.fy / v.z + intr.cy);
+
+
+ if (v.z > 0 && coo.x >= 0 && coo.y >= 0 && coo.x < depth_raw.cols && coo.y < depth_raw.rows) //6
+ {
+ int Dp = depth_raw.ptr (coo.y)[coo.x]; //mm
+
+ if (Dp != 0)
+ {
+ float xl = (coo.x - intr.cx) / intr.fx;
+ float yl = (coo.y - intr.cy) / intr.fy;
+ float lambda_inv = rsqrtf (xl * xl + yl * yl + 1);
+
+ float sdf = Dp - norm (vr) * lambda_inv * 1000; //mm
+
+
+ if (sdf >= -tranc_dist_mm)
+ {
+ float tsdf = fmin (1.f, sdf / tranc_dist_mm);
+
+ int weight_prev;
+ float tsdf_prev;
+
+ //read and unpack
+ unpack_tsdf (*pos, tsdf_prev, weight_prev);
+
+ const int Wrk = 1;
+
+ float tsdf_new = (tsdf_prev * weight_prev + Wrk * tsdf) / (weight_prev + Wrk);
+ int weight_new = min (weight_prev + Wrk, Tsdf::MAX_WEIGHT);
+
+ pack_tsdf (tsdf_new, weight_new, *pos);
+ }
+ }
+ }
+ pos += elem_step;
+ } /* for(int z = 0; z < VOLUME_Z; ++z) */
+ } /* __global__ */
+ }
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::device::integrateTsdfVolume (const PtrStepSz<ushort>& depth_raw, const Intr& intr, const float3& volume_size,
+ const Mat33& Rcurr_inv, const float3& tcurr, float tranc_dist,
+ PtrStep<short2> volume)
+{
+ Tsdf tsdf;
+
+ tsdf.volume = volume;
+ tsdf.cell_size.x = volume_size.x / VOLUME_X;
+ tsdf.cell_size.y = volume_size.y / VOLUME_Y;
+ tsdf.cell_size.z = volume_size.z / VOLUME_Z;
+
+ tsdf.intr = intr;
+
+ tsdf.Rcurr_inv = Rcurr_inv;
+ tsdf.tcurr = tcurr;
+ tsdf.depth_raw = depth_raw;
+
+ tsdf.tranc_dist_mm = tranc_dist*1000; //mm
+
+ dim3 block (Tsdf::CTA_SIZE_X, Tsdf::CTA_SIZE_Y);
+ dim3 grid (divUp (VOLUME_X, block.x), divUp (VOLUME_Y, block.y));
+
+#if 0
+ //tsdf2<<<grid, block>>>(volume, tranc_dist, Rcurr_inv, tcurr, intr, depth_raw, tsdf.cell_size);
+ integrateTsdfKernel<<<grid, block>>>(tsdf);
+#endif
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+}
+
+
+namespace pcl
+{
+ namespace device
+ {
+ __global__ void
+ scaleDepth (const PtrStepSz<ushort> depth, PtrStep<float> scaled, const Intr intr)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x >= depth.cols || y >= depth.rows)
+ return;
+
+ int Dp = depth.ptr (y)[x];
+
+ float xl = (x - intr.cx) / intr.fx;
+ float yl = (y - intr.cy) / intr.fy;
+ float lambda = sqrtf (xl * xl + yl * yl + 1);
+
+ scaled.ptr (y)[x] = Dp * lambda/1000.f; //meters
+ }
+
+ __global__ void
+ tsdf23 (const PtrStepSz<float> depthScaled, PtrStep<short2> volume,
+ const float tranc_dist, const Mat33 Rcurr_inv, const float3 tcurr, const Intr intr, const float3 cell_size)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x >= VOLUME_X || y >= VOLUME_Y)
+ return;
+
+ float v_g_x = (x + 0.5f) * cell_size.x - tcurr.x;
+ float v_g_y = (y + 0.5f) * cell_size.y - tcurr.y;
+ float v_g_z = (0 + 0.5f) * cell_size.z - tcurr.z;
+
+ float v_g_part_norm = v_g_x * v_g_x + v_g_y * v_g_y;
+
+ float v_x = (Rcurr_inv.data[0].x * v_g_x + Rcurr_inv.data[0].y * v_g_y + Rcurr_inv.data[0].z * v_g_z) * intr.fx;
+ float v_y = (Rcurr_inv.data[1].x * v_g_x + Rcurr_inv.data[1].y * v_g_y + Rcurr_inv.data[1].z * v_g_z) * intr.fy;
+ float v_z = (Rcurr_inv.data[2].x * v_g_x + Rcurr_inv.data[2].y * v_g_y + Rcurr_inv.data[2].z * v_g_z);
+
+ float z_scaled = 0;
+
+ float Rcurr_inv_0_z_scaled = Rcurr_inv.data[0].z * cell_size.z * intr.fx;
+ float Rcurr_inv_1_z_scaled = Rcurr_inv.data[1].z * cell_size.z * intr.fy;
+
+ float tranc_dist_inv = 1.0f / tranc_dist;
+
+ short2* pos = volume.ptr (y) + x;
+ int elem_step = volume.step * VOLUME_Y / sizeof(short2);
+
+//#pragma unroll
+ for (int z = 0; z < VOLUME_Z;
+ ++z,
+ v_g_z += cell_size.z,
+ z_scaled += cell_size.z,
+ v_x += Rcurr_inv_0_z_scaled,
+ v_y += Rcurr_inv_1_z_scaled,
+ pos += elem_step)
+ {
+ float inv_z = 1.0f / (v_z + Rcurr_inv.data[2].z * z_scaled);
+ if (inv_z < 0)
+ continue;
+
+ // project to current cam
+ int2 coo =
+ {
+ __float2int_rn (v_x * inv_z + intr.cx),
+ __float2int_rn (v_y * inv_z + intr.cy)
+ };
+
+ if (coo.x >= 0 && coo.y >= 0 && coo.x < depthScaled.cols && coo.y < depthScaled.rows) //6
+ {
+ float Dp_scaled = depthScaled.ptr (coo.y)[coo.x]; //meters
+
+ float sdf = Dp_scaled - sqrtf (v_g_z * v_g_z + v_g_part_norm);
+
+ if (Dp_scaled != 0 && sdf >= -tranc_dist) //meters
+ {
+ float tsdf = fmin (1.0f, sdf * tranc_dist_inv);
+
+ //read and unpack
+ float tsdf_prev;
+ int weight_prev;
+ unpack_tsdf (*pos, tsdf_prev, weight_prev);
+
+ const int Wrk = 1;
+
+ float tsdf_new = (tsdf_prev * weight_prev + Wrk * tsdf) / (weight_prev + Wrk);
+ int weight_new = min (weight_prev + Wrk, Tsdf::MAX_WEIGHT);
+
+ pack_tsdf (tsdf_new, weight_new, *pos);
+ }
+ }
+ } // for(int z = 0; z < VOLUME_Z; ++z)
+ } // __global__
+
+ __global__ void
+ tsdf23normal_hack (const PtrStepSz<float> depthScaled, PtrStep<short2> volume,
+ const float tranc_dist, const Mat33 Rcurr_inv, const float3 tcurr, const Intr intr, const float3 cell_size)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x >= VOLUME_X || y >= VOLUME_Y)
+ return;
+
+ const float v_g_x = (x + 0.5f) * cell_size.x - tcurr.x;
+ const float v_g_y = (y + 0.5f) * cell_size.y - tcurr.y;
+ float v_g_z = (0 + 0.5f) * cell_size.z - tcurr.z;
+
+ float v_g_part_norm = v_g_x * v_g_x + v_g_y * v_g_y;
+
+ float v_x = (Rcurr_inv.data[0].x * v_g_x + Rcurr_inv.data[0].y * v_g_y + Rcurr_inv.data[0].z * v_g_z) * intr.fx;
+ float v_y = (Rcurr_inv.data[1].x * v_g_x + Rcurr_inv.data[1].y * v_g_y + Rcurr_inv.data[1].z * v_g_z) * intr.fy;
+ float v_z = (Rcurr_inv.data[2].x * v_g_x + Rcurr_inv.data[2].y * v_g_y + Rcurr_inv.data[2].z * v_g_z);
+
+ float z_scaled = 0;
+
+ float Rcurr_inv_0_z_scaled = Rcurr_inv.data[0].z * cell_size.z * intr.fx;
+ float Rcurr_inv_1_z_scaled = Rcurr_inv.data[1].z * cell_size.z * intr.fy;
+
+ float tranc_dist_inv = 1.0f / tranc_dist;
+
+ short2* pos = volume.ptr (y) + x;
+ int elem_step = volume.step * VOLUME_Y / sizeof(short2);
+
+ //#pragma unroll
+ for (int z = 0; z < VOLUME_Z;
+ ++z,
+ v_g_z += cell_size.z,
+ z_scaled += cell_size.z,
+ v_x += Rcurr_inv_0_z_scaled,
+ v_y += Rcurr_inv_1_z_scaled,
+ pos += elem_step)
+ {
+ float inv_z = 1.0f / (v_z + Rcurr_inv.data[2].z * z_scaled);
+ if (inv_z < 0)
+ continue;
+
+ // project to current cam
+ int2 coo =
+ {
+ __float2int_rn (v_x * inv_z + intr.cx),
+ __float2int_rn (v_y * inv_z + intr.cy)
+ };
+
+ if (coo.x >= 0 && coo.y >= 0 && coo.x < depthScaled.cols && coo.y < depthScaled.rows) //6
+ {
+ float Dp_scaled = depthScaled.ptr (coo.y)[coo.x]; //meters
+
+ float sdf = Dp_scaled - sqrtf (v_g_z * v_g_z + v_g_part_norm);
+
+ if (Dp_scaled != 0 && sdf >= -tranc_dist) //meters
+ {
+ float tsdf = fmin (1.0f, sdf * tranc_dist_inv);
+
+ bool integrate = true;
+ if ((x > 0 && x < VOLUME_X-2) && (y > 0 && y < VOLUME_Y-2) && (z > 0 && z < VOLUME_Z-2))
+ {
+ const float qnan = numeric_limits<float>::quiet_NaN();
+ float3 normal = make_float3(qnan, qnan, qnan);
+
+ float Fn, Fp;
+ int Wn = 0, Wp = 0;
+ unpack_tsdf (*(pos + elem_step), Fn, Wn);
+ unpack_tsdf (*(pos - elem_step), Fp, Wp);
+
+ if (Wn > 16 && Wp > 16)
+ normal.z = (Fn - Fp)/cell_size.z;
+
+ unpack_tsdf (*(pos + volume.step/sizeof(short2) ), Fn, Wn);
+ unpack_tsdf (*(pos - volume.step/sizeof(short2) ), Fp, Wp);
+
+ if (Wn > 16 && Wp > 16)
+ normal.y = (Fn - Fp)/cell_size.y;
+
+ unpack_tsdf (*(pos + 1), Fn, Wn);
+ unpack_tsdf (*(pos - 1), Fp, Wp);
+
+ if (Wn > 16 && Wp > 16)
+ normal.x = (Fn - Fp)/cell_size.x;
+
+ if (normal.x != qnan && normal.y != qnan && normal.z != qnan)
+ {
+ float norm2 = dot(normal, normal);
+ if (norm2 >= 1e-10)
+ {
+ normal *= rsqrt(norm2);
+
+ float nt = v_g_x * normal.x + v_g_y * normal.y + v_g_z * normal.z;
+ float cosine = nt * rsqrt(v_g_x * v_g_x + v_g_y * v_g_y + v_g_z * v_g_z);
+
+ if (cosine < 0.5)
+ integrate = false;
+ }
+ }
+ }
+
+ if (integrate)
+ {
+ //read and unpack
+ float tsdf_prev;
+ int weight_prev;
+ unpack_tsdf (*pos, tsdf_prev, weight_prev);
+
+ const int Wrk = 1;
+
+ float tsdf_new = (tsdf_prev * weight_prev + Wrk * tsdf) / (weight_prev + Wrk);
+ int weight_new = min (weight_prev + Wrk, Tsdf::MAX_WEIGHT);
+
+ pack_tsdf (tsdf_new, weight_new, *pos);
+ }
+ }
+ }
+ } // for(int z = 0; z < VOLUME_Z; ++z)
+ } // __global__
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::device::integrateTsdfVolume (const PtrStepSz<ushort>& depth, const Intr& intr,
+ const float3& volume_size, const Mat33& Rcurr_inv, const float3& tcurr,
+ float tranc_dist,
+ PtrStep<short2> volume, DeviceArray2D<float>& depthScaled)
+{
+ depthScaled.create (depth.rows, depth.cols);
+
+ dim3 block_scale (32, 8);
+ dim3 grid_scale (divUp (depth.cols, block_scale.x), divUp (depth.rows, block_scale.y));
+
+ //scales depth along ray and converts mm -> meters.
+ scaleDepth<<<grid_scale, block_scale>>>(depth, depthScaled, intr);
+ cudaSafeCall ( cudaGetLastError () );
+
+ float3 cell_size;
+ cell_size.x = volume_size.x / VOLUME_X;
+ cell_size.y = volume_size.y / VOLUME_Y;
+ cell_size.z = volume_size.z / VOLUME_Z;
+
+ //dim3 block(Tsdf::CTA_SIZE_X, Tsdf::CTA_SIZE_Y);
+ dim3 block (16, 16);
+ dim3 grid (divUp (VOLUME_X, block.x), divUp (VOLUME_Y, block.y));
+
+ tsdf23<<<grid, block>>>(depthScaled, volume, tranc_dist, Rcurr_inv, tcurr, intr, cell_size);
+ //tsdf23normal_hack<<<grid, block>>>(depthScaled, volume, tranc_dist, Rcurr_inv, tcurr, intr, cell_size);
+
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+
+#ifndef PCL_GPU_KINFU_CUDA_UTILS_HPP_
+#define PCL_GPU_KINFU_CUDA_UTILS_HPP_
+
+
+namespace pcl
+{
+ namespace device
+ {
+ template <class T>
+ __device__ __host__ __forceinline__ void swap ( T& a, T& b )
+ {
+ T c(a); a=b; b=c;
+ }
+
+ template<typename T> struct numeric_limits;
+
+ template<> struct numeric_limits<float>
+ {
+ __device__ __forceinline__ static float
+ quiet_NaN() { return __int_as_float(0x7fffffff); /*CUDART_NAN_F*/ };
+ __device__ __forceinline__ static float
+ epsilon() { return 1.192092896e-07f/*FLT_EPSILON*/; };
+
+ __device__ __forceinline__ static float
+ min() { return 1.175494351e-38f/*FLT_MIN*/; };
+ __device__ __forceinline__ static float
+ max() { return 3.402823466e+38f/*FLT_MAX*/; };
+ };
+
+ template<> struct numeric_limits<short>
+ {
+ __device__ __forceinline__ static short
+ max() { return SHRT_MAX; };
+ };
+
+ __device__ __forceinline__ float
+ dot(const float3& v1, const float3& v2)
+ {
+ return v1.x * v2.x + v1.y*v2.y + v1.z*v2.z;
+ }
+
+ __device__ __forceinline__ float3&
+ operator+=(float3& vec, const float& v)
+ {
+ vec.x += v; vec.y += v; vec.z += v; return vec;
+ }
+
+ __device__ __forceinline__ float3
+ operator+(const float3& v1, const float3& v2)
+ {
+ return make_float3(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
+ }
+
+ __device__ __forceinline__ float3&
+ operator*=(float3& vec, const float& v)
+ {
+ vec.x *= v; vec.y *= v; vec.z *= v; return vec;
+ }
+
+ __device__ __forceinline__ float3
+ operator-(const float3& v1, const float3& v2)
+ {
+ return make_float3(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
+ }
+
+ __device__ __forceinline__ float3
+ operator*(const float3& v1, const float& v)
+ {
+ return make_float3(v1.x * v, v1.y * v, v1.z * v);
+ }
+
+ __device__ __forceinline__ float
+ norm(const float3& v)
+ {
+ return sqrt(dot(v, v));
+ }
+
+ __device__ __forceinline__ float3
+ normalized(const float3& v)
+ {
+ return v * rsqrt(dot(v, v));
+ }
+
+ __device__ __host__ __forceinline__ float3
+ cross(const float3& v1, const float3& v2)
+ {
+ return make_float3(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x);
+ }
+
+ __device__ __forceinline__ void computeRoots2(const float& b, const float& c, float3& roots)
+ {
+ roots.x = 0.f;
+ float d = b * b - 4.f * c;
+ if (d < 0.f) // no real roots!!!! THIS SHOULD NOT HAPPEN!
+ d = 0.f;
+
+ float sd = sqrtf(d);
+
+ roots.z = 0.5f * (b + sd);
+ roots.y = 0.5f * (b - sd);
+ }
+
+ __device__ __forceinline__ void
+ computeRoots3(float c0, float c1, float c2, float3& roots)
+ {
+ if ( fabsf(c0) < numeric_limits<float>::epsilon())// one root is 0 -> quadratic equation
+ {
+ computeRoots2 (c2, c1, roots);
+ }
+ else
+ {
+ const float s_inv3 = 1.f/3.f;
+ const float s_sqrt3 = sqrtf(3.f);
+ // Construct the parameters used in classifying the roots of the equation
+ // and in solving the equation for the roots in closed form.
+ float c2_over_3 = c2 * s_inv3;
+ float a_over_3 = (c1 - c2*c2_over_3)*s_inv3;
+ if (a_over_3 > 0.f)
+ a_over_3 = 0.f;
+
+ float half_b = 0.5f * (c0 + c2_over_3 * (2.f * c2_over_3 * c2_over_3 - c1));
+
+ float q = half_b * half_b + a_over_3 * a_over_3 * a_over_3;
+ if (q > 0.f)
+ q = 0.f;
+
+ // Compute the eigenvalues by solving for the roots of the polynomial.
+ float rho = sqrtf(-a_over_3);
+ float theta = atan2f (sqrtf (-q), half_b)*s_inv3;
+ float cos_theta = __cosf (theta);
+ float sin_theta = __sinf (theta);
+ roots.x = c2_over_3 + 2.f * rho * cos_theta;
+ roots.y = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
+ roots.z = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
+
+ // Sort in increasing order.
+ if (roots.x >= roots.y)
+ swap(roots.x, roots.y);
+
+ if (roots.y >= roots.z)
+ {
+ swap(roots.y, roots.z);
+
+ if (roots.x >= roots.y)
+ swap (roots.x, roots.y);
+ }
+ if (roots.x <= 0) // eigenval for symetric positive semi-definite matrix can not be negative! Set it to 0
+ computeRoots2 (c2, c1, roots);
+ }
+ }
+
+ struct Eigen33
+ {
+ public:
+ template<int Rows>
+ struct MiniMat
+ {
+ float3 data[Rows];
+ __device__ __host__ __forceinline__ float3& operator[](int i) { return data[i]; }
+ __device__ __host__ __forceinline__ const float3& operator[](int i) const { return data[i]; }
+ };
+ typedef MiniMat<3> Mat33;
+ typedef MiniMat<4> Mat43;
+
+
+ static __forceinline__ __device__ float3
+ unitOrthogonal (const float3& src)
+ {
+ float3 perp;
+ /* Let us compute the crossed product of *this with a vector
+ * that is not too close to being colinear to *this.
+ */
+
+ /* unless the x and y coords are both close to zero, we can
+ * simply take ( -y, x, 0 ) and normalize it.
+ */
+ if(!isMuchSmallerThan(src.x, src.z) || !isMuchSmallerThan(src.y, src.z))
+ {
+ float invnm = rsqrtf(src.x*src.x + src.y*src.y);
+ perp.x = -src.y * invnm;
+ perp.y = src.x * invnm;
+ perp.z = 0.0f;
+ }
+ /* if both x and y are close to zero, then the vector is close
+ * to the z-axis, so it's far from colinear to the x-axis for instance.
+ * So we take the crossed product with (1,0,0) and normalize it.
+ */
+ else
+ {
+ float invnm = rsqrtf(src.z * src.z + src.y * src.y);
+ perp.x = 0.0f;
+ perp.y = -src.z * invnm;
+ perp.z = src.y * invnm;
+ }
+
+ return perp;
+ }
+
+ __device__ __forceinline__
+ Eigen33(volatile float* mat_pkg_arg) : mat_pkg(mat_pkg_arg) {}
+ __device__ __forceinline__ void
+ compute(Mat33& tmp, Mat33& vec_tmp, Mat33& evecs, float3& evals)
+ {
+ // Scale the matrix so its entries are in [-1,1]. The scaling is applied
+ // only when at least one matrix entry has magnitude larger than 1.
+
+ float max01 = fmaxf( fabsf(mat_pkg[0]), fabsf(mat_pkg[1]) );
+ float max23 = fmaxf( fabsf(mat_pkg[2]), fabsf(mat_pkg[3]) );
+ float max45 = fmaxf( fabsf(mat_pkg[4]), fabsf(mat_pkg[5]) );
+ float m0123 = fmaxf( max01, max23);
+ float scale = fmaxf( max45, m0123);
+
+ if (scale <= numeric_limits<float>::min())
+ scale = 1.f;
+
+ mat_pkg[0] /= scale;
+ mat_pkg[1] /= scale;
+ mat_pkg[2] /= scale;
+ mat_pkg[3] /= scale;
+ mat_pkg[4] /= scale;
+ mat_pkg[5] /= scale;
+
+ // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The
+ // eigenvalues are the roots to this equation, all guaranteed to be
+ // real-valued, because the matrix is symmetric.
+ float c0 = m00() * m11() * m22()
+ + 2.f * m01() * m02() * m12()
+ - m00() * m12() * m12()
+ - m11() * m02() * m02()
+ - m22() * m01() * m01();
+ float c1 = m00() * m11() -
+ m01() * m01() +
+ m00() * m22() -
+ m02() * m02() +
+ m11() * m22() -
+ m12() * m12();
+ float c2 = m00() + m11() + m22();
+
+ computeRoots3(c0, c1, c2, evals);
+
+ if(evals.z - evals.x <= numeric_limits<float>::epsilon())
+ {
+ evecs[0] = make_float3(1.f, 0.f, 0.f);
+ evecs[1] = make_float3(0.f, 1.f, 0.f);
+ evecs[2] = make_float3(0.f, 0.f, 1.f);
+ }
+ else if (evals.y - evals.x <= numeric_limits<float>::epsilon() )
+ {
+ // first and second equal
+ tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
+ tmp[0].x -= evals.z; tmp[1].y -= evals.z; tmp[2].z -= evals.z;
+
+ vec_tmp[0] = cross(tmp[0], tmp[1]);
+ vec_tmp[1] = cross(tmp[0], tmp[2]);
+ vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+ float len1 = dot (vec_tmp[0], vec_tmp[0]);
+ float len2 = dot (vec_tmp[1], vec_tmp[1]);
+ float len3 = dot (vec_tmp[2], vec_tmp[2]);
+
+ if (len1 >= len2 && len1 >= len3)
+ {
+ evecs[2] = vec_tmp[0] * rsqrtf (len1);
+ }
+ else if (len2 >= len1 && len2 >= len3)
+ {
+ evecs[2] = vec_tmp[1] * rsqrtf (len2);
+ }
+ else
+ {
+ evecs[2] = vec_tmp[2] * rsqrtf (len3);
+ }
+
+ evecs[1] = unitOrthogonal(evecs[2]);
+ evecs[0] = cross(evecs[1], evecs[2]);
+ }
+ else if (evals.z - evals.y <= numeric_limits<float>::epsilon() )
+ {
+ // second and third equal
+ tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
+ tmp[0].x -= evals.x; tmp[1].y -= evals.x; tmp[2].z -= evals.x;
+
+ vec_tmp[0] = cross(tmp[0], tmp[1]);
+ vec_tmp[1] = cross(tmp[0], tmp[2]);
+ vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+ float len1 = dot(vec_tmp[0], vec_tmp[0]);
+ float len2 = dot(vec_tmp[1], vec_tmp[1]);
+ float len3 = dot(vec_tmp[2], vec_tmp[2]);
+
+ if (len1 >= len2 && len1 >= len3)
+ {
+ evecs[0] = vec_tmp[0] * rsqrtf(len1);
+ }
+ else if (len2 >= len1 && len2 >= len3)
+ {
+ evecs[0] = vec_tmp[1] * rsqrtf(len2);
+ }
+ else
+ {
+ evecs[0] = vec_tmp[2] * rsqrtf(len3);
+ }
+
+ evecs[1] = unitOrthogonal( evecs[0] );
+ evecs[2] = cross(evecs[0], evecs[1]);
+ }
+ else
+ {
+
+ tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
+ tmp[0].x -= evals.z; tmp[1].y -= evals.z; tmp[2].z -= evals.z;
+
+ vec_tmp[0] = cross(tmp[0], tmp[1]);
+ vec_tmp[1] = cross(tmp[0], tmp[2]);
+ vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+ float len1 = dot(vec_tmp[0], vec_tmp[0]);
+ float len2 = dot(vec_tmp[1], vec_tmp[1]);
+ float len3 = dot(vec_tmp[2], vec_tmp[2]);
+
+ float mmax[3];
+
+ unsigned int min_el = 2;
+ unsigned int max_el = 2;
+ if (len1 >= len2 && len1 >= len3)
+ {
+ mmax[2] = len1;
+ evecs[2] = vec_tmp[0] * rsqrtf (len1);
+ }
+ else if (len2 >= len1 && len2 >= len3)
+ {
+ mmax[2] = len2;
+ evecs[2] = vec_tmp[1] * rsqrtf (len2);
+ }
+ else
+ {
+ mmax[2] = len3;
+ evecs[2] = vec_tmp[2] * rsqrtf (len3);
+ }
+
+ tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
+ tmp[0].x -= evals.y; tmp[1].y -= evals.y; tmp[2].z -= evals.y;
+
+ vec_tmp[0] = cross(tmp[0], tmp[1]);
+ vec_tmp[1] = cross(tmp[0], tmp[2]);
+ vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+ len1 = dot(vec_tmp[0], vec_tmp[0]);
+ len2 = dot(vec_tmp[1], vec_tmp[1]);
+ len3 = dot(vec_tmp[2], vec_tmp[2]);
+
+ if (len1 >= len2 && len1 >= len3)
+ {
+ mmax[1] = len1;
+ evecs[1] = vec_tmp[0] * rsqrtf (len1);
+ min_el = len1 <= mmax[min_el] ? 1 : min_el;
+ max_el = len1 > mmax[max_el] ? 1 : max_el;
+ }
+ else if (len2 >= len1 && len2 >= len3)
+ {
+ mmax[1] = len2;
+ evecs[1] = vec_tmp[1] * rsqrtf (len2);
+ min_el = len2 <= mmax[min_el] ? 1 : min_el;
+ max_el = len2 > mmax[max_el] ? 1 : max_el;
+ }
+ else
+ {
+ mmax[1] = len3;
+ evecs[1] = vec_tmp[2] * rsqrtf (len3);
+ min_el = len3 <= mmax[min_el] ? 1 : min_el;
+ max_el = len3 > mmax[max_el] ? 1 : max_el;
+ }
+
+ tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
+ tmp[0].x -= evals.x; tmp[1].y -= evals.x; tmp[2].z -= evals.x;
+
+ vec_tmp[0] = cross(tmp[0], tmp[1]);
+ vec_tmp[1] = cross(tmp[0], tmp[2]);
+ vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+ len1 = dot (vec_tmp[0], vec_tmp[0]);
+ len2 = dot (vec_tmp[1], vec_tmp[1]);
+ len3 = dot (vec_tmp[2], vec_tmp[2]);
+
+
+ if (len1 >= len2 && len1 >= len3)
+ {
+ mmax[0] = len1;
+ evecs[0] = vec_tmp[0] * rsqrtf (len1);
+ min_el = len3 <= mmax[min_el] ? 0 : min_el;
+ max_el = len3 > mmax[max_el] ? 0 : max_el;
+ }
+ else if (len2 >= len1 && len2 >= len3)
+ {
+ mmax[0] = len2;
+ evecs[0] = vec_tmp[1] * rsqrtf (len2);
+ min_el = len3 <= mmax[min_el] ? 0 : min_el;
+ max_el = len3 > mmax[max_el] ? 0 : max_el;
+ }
+ else
+ {
+ mmax[0] = len3;
+ evecs[0] = vec_tmp[2] * rsqrtf (len3);
+ min_el = len3 <= mmax[min_el] ? 0 : min_el;
+ max_el = len3 > mmax[max_el] ? 0 : max_el;
+ }
+
+ unsigned mid_el = 3 - min_el - max_el;
+ evecs[min_el] = normalized( cross( evecs[(min_el+1) % 3], evecs[(min_el+2) % 3] ) );
+ evecs[mid_el] = normalized( cross( evecs[(mid_el+1) % 3], evecs[(mid_el+2) % 3] ) );
+ }
+ // Rescale back to the original size.
+ evals *= scale;
+ }
+ private:
+ volatile float* mat_pkg;
+
+ __device__ __forceinline__ float m00() const { return mat_pkg[0]; }
+ __device__ __forceinline__ float m01() const { return mat_pkg[1]; }
+ __device__ __forceinline__ float m02() const { return mat_pkg[2]; }
+ __device__ __forceinline__ float m10() const { return mat_pkg[1]; }
+ __device__ __forceinline__ float m11() const { return mat_pkg[3]; }
+ __device__ __forceinline__ float m12() const { return mat_pkg[4]; }
+ __device__ __forceinline__ float m20() const { return mat_pkg[2]; }
+ __device__ __forceinline__ float m21() const { return mat_pkg[4]; }
+ __device__ __forceinline__ float m22() const { return mat_pkg[5]; }
+
+ __device__ __forceinline__ float3 row0() const { return make_float3( m00(), m01(), m02() ); }
+ __device__ __forceinline__ float3 row1() const { return make_float3( m10(), m11(), m12() ); }
+ __device__ __forceinline__ float3 row2() const { return make_float3( m20(), m21(), m22() ); }
+
+ __device__ __forceinline__ static bool isMuchSmallerThan (float x, float y)
+ {
+ // copied from <eigen>/include/Eigen/src/Core/NumTraits.h
+ const float prec_sqr = numeric_limits<float>::epsilon() * numeric_limits<float>::epsilon();
+ return x * x <= prec_sqr * y * y;
+ }
+ };
+
+ struct Block
+ {
+ static __device__ __forceinline__ unsigned int stride()
+ {
+ return blockDim.x * blockDim.y * blockDim.z;
+ }
+
+ static __device__ __forceinline__ int
+ flattenedThreadId()
+ {
+ return threadIdx.z * blockDim.x * blockDim.y + threadIdx.y * blockDim.x + threadIdx.x;
+ }
+
+ template<int CTA_SIZE, typename T, class BinOp>
+ static __device__ __forceinline__ void reduce(volatile T* buffer, BinOp op)
+ {
+ int tid = flattenedThreadId();
+ T val = buffer[tid];
+
+ if (CTA_SIZE >= 1024) { if (tid < 512) buffer[tid] = val = op(val, buffer[tid + 512]); __syncthreads(); }
+ if (CTA_SIZE >= 512) { if (tid < 256) buffer[tid] = val = op(val, buffer[tid + 256]); __syncthreads(); }
+ if (CTA_SIZE >= 256) { if (tid < 128) buffer[tid] = val = op(val, buffer[tid + 128]); __syncthreads(); }
+ if (CTA_SIZE >= 128) { if (tid < 64) buffer[tid] = val = op(val, buffer[tid + 64]); __syncthreads(); }
+
+ if (tid < 32)
+ {
+ if (CTA_SIZE >= 64) { buffer[tid] = val = op(val, buffer[tid + 32]); }
+ if (CTA_SIZE >= 32) { buffer[tid] = val = op(val, buffer[tid + 16]); }
+ if (CTA_SIZE >= 16) { buffer[tid] = val = op(val, buffer[tid + 8]); }
+ if (CTA_SIZE >= 8) { buffer[tid] = val = op(val, buffer[tid + 4]); }
+ if (CTA_SIZE >= 4) { buffer[tid] = val = op(val, buffer[tid + 2]); }
+ if (CTA_SIZE >= 2) { buffer[tid] = val = op(val, buffer[tid + 1]); }
+ }
+ }
+
+ template<int CTA_SIZE, typename T, class BinOp>
+ static __device__ __forceinline__ T reduce(volatile T* buffer, T init, BinOp op)
+ {
+ int tid = flattenedThreadId();
+ T val = buffer[tid] = init;
+ __syncthreads();
+
+ if (CTA_SIZE >= 1024) { if (tid < 512) buffer[tid] = val = op(val, buffer[tid + 512]); __syncthreads(); }
+ if (CTA_SIZE >= 512) { if (tid < 256) buffer[tid] = val = op(val, buffer[tid + 256]); __syncthreads(); }
+ if (CTA_SIZE >= 256) { if (tid < 128) buffer[tid] = val = op(val, buffer[tid + 128]); __syncthreads(); }
+ if (CTA_SIZE >= 128) { if (tid < 64) buffer[tid] = val = op(val, buffer[tid + 64]); __syncthreads(); }
+
+ if (tid < 32)
+ {
+ if (CTA_SIZE >= 64) { buffer[tid] = val = op(val, buffer[tid + 32]); }
+ if (CTA_SIZE >= 32) { buffer[tid] = val = op(val, buffer[tid + 16]); }
+ if (CTA_SIZE >= 16) { buffer[tid] = val = op(val, buffer[tid + 8]); }
+ if (CTA_SIZE >= 8) { buffer[tid] = val = op(val, buffer[tid + 4]); }
+ if (CTA_SIZE >= 4) { buffer[tid] = val = op(val, buffer[tid + 2]); }
+ if (CTA_SIZE >= 2) { buffer[tid] = val = op(val, buffer[tid + 1]); }
+ }
+ __syncthreads();
+ return buffer[0];
+ }
+ };
+
+ struct Warp
+ {
+ enum
+ {
+ LOG_WARP_SIZE = 5,
+ WARP_SIZE = 1 << LOG_WARP_SIZE,
+ STRIDE = WARP_SIZE
+ };
+
+ /** \brief Returns the warp lane ID of the calling thread. */
+ static __device__ __forceinline__ unsigned int
+ laneId()
+ {
+ unsigned int ret;
+ asm("mov.u32 %0, %laneid;" : "=r"(ret) );
+ return ret;
+ }
+
+ static __device__ __forceinline__ unsigned int id()
+ {
+ int tid = threadIdx.z * blockDim.x * blockDim.y + threadIdx.y * blockDim.x + threadIdx.x;
+ return tid >> LOG_WARP_SIZE;
+ }
+
+ static __device__ __forceinline__
+ int laneMaskLt()
+ {
+#if (__CUDA_ARCH__ >= 200)
+ unsigned int ret;
+ asm("mov.u32 %0, %lanemask_lt;" : "=r"(ret) );
+ return ret;
+#else
+ return 0xFFFFFFFF >> (32 - laneId());
+#endif
+ }
+
+ static __device__ __forceinline__ int binaryExclScan(int ballot_mask)
+ {
+ return __popc(Warp::laneMaskLt() & ballot_mask);
+ }
+ };
+
+
+ struct Emulation
+ {
+ static __device__ __forceinline__ int
+ warp_reduce ( volatile int *ptr , const unsigned int tid)
+ {
+ const unsigned int lane = tid & 31; // index of thread in warp (0..31)
+
+ if (lane < 16)
+ {
+ int partial = ptr[tid];
+
+ ptr[tid] = partial = partial + ptr[tid + 16];
+ ptr[tid] = partial = partial + ptr[tid + 8];
+ ptr[tid] = partial = partial + ptr[tid + 4];
+ ptr[tid] = partial = partial + ptr[tid + 2];
+ ptr[tid] = partial = partial + ptr[tid + 1];
+ }
+ return ptr[tid - lane];
+ }
+
+ static __forceinline__ __device__ int
+ Ballot(int predicate, volatile int* cta_buffer)
+ {
+#if __CUDA_ARCH__ >= 200
+ (void)cta_buffer;
+ return __ballot(predicate);
+#else
+ int tid = Block::flattenedThreadId();
+ cta_buffer[tid] = predicate ? (1 << (tid & 31)) : 0;
+ return warp_reduce(cta_buffer, tid);
+#endif
+ }
+
+ static __forceinline__ __device__ bool
+ All(int predicate, volatile int* cta_buffer)
+ {
+#if __CUDA_ARCH__ >= 200
+ (void)cta_buffer;
+ return __all(predicate);
+#else
+ int tid = Block::flattenedThreadId();
+ cta_buffer[tid] = predicate ? 1 : 0;
+ return warp_reduce(cta_buffer, tid) == 32;
+#endif
+ }
+ };
+ }
+}
+
+#endif /* PCL_GPU_KINFU_CUDA_UTILS_HPP_ */
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_KINFU_INTERNAL_HPP_
+#define PCL_KINFU_INTERNAL_HPP_
+
+#include <pcl/gpu/containers/device_array.h>
+//#include <pcl/gpu/utils/safe_call.hpp>
+#include "safe_call.hpp"
+
+namespace pcl
+{
+ namespace device
+ {
+ ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Types
+ typedef unsigned short ushort;
+ typedef DeviceArray2D<float> MapArr;
+ typedef DeviceArray2D<ushort> DepthMap;
+ typedef float4 PointType;
+
+ //TSDF fixed point divisor (if old format is enabled)
+ const int DIVISOR = 32767; // SHRT_MAX;
+
+ //Should be multiple of 32
+ enum { VOLUME_X = 512, VOLUME_Y = 512, VOLUME_Z = 512 };
+
+
+ const float VOLUME_SIZE = 3.0f; // in meters
+
+ /** \brief Camera intrinsics structure
+ */
+ struct Intr
+ {
+ float fx, fy, cx, cy;
+ Intr () {}
+ Intr (float fx_, float fy_, float cx_, float cy_) : fx(fx_), fy(fy_), cx(cx_), cy(cy_) {}
+
+ Intr operator()(int level_index) const
+ {
+ int div = 1 << level_index;
+ return (Intr (fx / div, fy / div, cx / div, cy / div));
+ }
+ };
+
+ /** \brief 3x3 Matrix for device code
+ */
+ struct Mat33
+ {
+ float3 data[3];
+ };
+
+ /** \brief Light source collection
+ */
+ struct LightSource
+ {
+ float3 pos[1];
+ int number;
+ };
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Maps
+
+ /** \brief Perfoms bilateral filtering of disparity map
+ * \param[in] src soruce map
+ * \param[out] dst output map
+ */
+ void
+ bilateralFilter (const DepthMap& src, DepthMap& dst);
+
+ /** \brief Computes depth pyramid
+ * \param[in] src source
+ * \param[out] dst destination
+ */
+ void
+ pyrDown (const DepthMap& src, DepthMap& dst);
+
+ /** \brief Computes vertex map
+ * \param[in] intr depth camera intrinsics
+ * \param[in] depth depth
+ * \param[out] vmap vertex map
+ */
+ void
+ createVMap (const Intr& intr, const DepthMap& depth, MapArr& vmap);
+
+ /** \brief Computes normal map using cross product
+ * \param[in] vmap vertex map
+ * \param[out] nmap normal map
+ */
+ void
+ createNMap (const MapArr& vmap, MapArr& nmap);
+
+ /** \brief Computes normal map using Eigen/PCA approach
+ * \param[in] vmap vertex map
+ * \param[out] nmap normal map
+ */
+ void
+ computeNormalsEigen (const MapArr& vmap, MapArr& nmap);
+
+ /** \brief Performs affine tranform of vertex and normal maps
+ * \param[in] vmap_src source vertex map
+ * \param[in] nmap_src source vertex map
+ * \param[in] Rmat Rotation mat
+ * \param[in] tvec translation
+ * \param[out] vmap_dst destination vertex map
+ * \param[out] nmap_dst destination vertex map
+ */
+ void
+ tranformMaps (const MapArr& vmap_src, const MapArr& nmap_src, const Mat33& Rmat, const float3& tvec, MapArr& vmap_dst, MapArr& nmap_dst);
+
+ /** \brief Performs depth truncation
+ * \param[out] depth depth map to truncation
+ * \param[in] max_distance truncation threshold, values that are higher than the threshold are reset to zero (means not measurement)
+ */
+ void
+ truncateDepth(DepthMap& depth, float max_distance);
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // ICP
+
+ /** \brief (now it's exra code) Computes corespondances map
+ * \param[in] vmap_g_curr current vertex map in global coo space
+ * \param[in] nmap_g_curr current normals map in global coo space
+ * \param[in] Rprev_inv inverse camera rotation at previous pose
+ * \param[in] tprev camera translation at previous pose
+ * \param[in] intr camera intrinsics
+ * \param[in] vmap_g_prev previous vertex map in global coo space
+ * \param[in] nmap_g_prev previous vertex map in global coo space
+ * \param[in] distThres distance filtering threshold
+ * \param[in] angleThres angle filtering threshold. Represents sine of angle between normals
+ * \param[out] coresp
+ */
+ void
+ findCoresp (const MapArr& vmap_g_curr, const MapArr& nmap_g_curr, const Mat33& Rprev_inv, const float3& tprev, const Intr& intr,
+ const MapArr& vmap_g_prev, const MapArr& nmap_g_prev, float distThres, float angleThres, PtrStepSz<short2> coresp);
+
+ /** \brief (now it's exra code) Computation Ax=b for ICP iteration
+ * \param[in] v_dst destination vertex map (previous frame cloud)
+ * \param[in] n_dst destination normal map (previous frame normals)
+ * \param[in] v_src source normal map (current frame cloud)
+ * \param[in] coresp Corespondances
+ * \param[out] gbuf temp buffer for GPU reduction
+ * \param[out] mbuf output GPU buffer for matrix computed
+ * \param[out] matrixA_host A
+ * \param[out] vectorB_host b
+ */
+ void
+ estimateTransform (const MapArr& v_dst, const MapArr& n_dst, const MapArr& v_src, const PtrStepSz<short2>& coresp,
+ DeviceArray2D<float>& gbuf, DeviceArray<float>& mbuf, float* matrixA_host, float* vectorB_host);
+
+
+ /** \brief Computation Ax=b for ICP iteration
+ * \param[in] Rcurr Rotation of current camera pose guess
+ * \param[in] tcurr translation of current camera pose guess
+ * \param[in] vmap_curr current vertex map in camera coo space
+ * \param[in] nmap_curr current vertex map in camera coo space
+ * \param[in] Rprev_inv inverse camera rotation at previous pose
+ * \param[in] tprev camera translation at previous pose
+ * \param[in] intr camera intrinsics
+ * \param[in] vmap_g_prev previous vertex map in global coo space
+ * \param[in] nmap_g_prev previous vertex map in global coo space
+ * \param[in] distThres distance filtering threshold
+ * \param[in] angleThres angle filtering threshold. Represents sine of angle between normals
+ * \param[out] gbuf temp buffer for GPU reduction
+ * \param[out] mbuf output GPU buffer for matrix computed
+ * \param[out] matrixA_host A
+ * \param[out] vectorB_host b
+ */
+ void
+ estimateCombined (const Mat33& Rcurr, const float3& tcurr, const MapArr& vmap_curr, const MapArr& nmap_curr, const Mat33& Rprev_inv, const float3& tprev, const Intr& intr,
+ const MapArr& vmap_g_prev, const MapArr& nmap_g_prev, float distThres, float angleThres,
+ DeviceArray2D<float>& gbuf, DeviceArray<float>& mbuf, float* matrixA_host, float* vectorB_host);
+
+
+ void
+ estimateCombined (const Mat33& Rcurr, const float3& tcurr, const MapArr& vmap_curr, const MapArr& nmap_curr, const Mat33& Rprev_inv, const float3& tprev, const Intr& intr,
+ const MapArr& vmap_g_prev, const MapArr& nmap_g_prev, float distThres, float angleThres,
+ DeviceArray2D<double>& gbuf, DeviceArray<double>& mbuf, double* matrixA_host, double* vectorB_host);
+
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // TSDF volume functions
+
+ /** \brief Perform tsdf volume initialization
+ * \param[out] array volume to be initialized
+ */
+ PCL_EXPORTS void
+ initVolume(PtrStep<short2> array);
+
+ //first version
+ /** \brief Performs Tsfg volume uptation (extra obsolete now)
+ * \param[in] depth_raw Kinect depth image
+ * \param[in] intr camera intrinsics
+ * \param[in] volume_size size of volume in mm
+ * \param[in] Rcurr_inv inverse rotation for current camera pose
+ * \param[in] tcurr translation for current camera pose
+ * \param[in] tranc_dist tsdf truncation distance
+ * \param[in] volume tsdf volume to be updated
+ */
+ void
+ integrateTsdfVolume (const PtrStepSz<ushort>& depth_raw, const Intr& intr, const float3& volume_size,
+ const Mat33& Rcurr_inv, const float3& tcurr, float tranc_dist, PtrStep<short2> volume);
+
+ //second version
+ /** \brief Function that integrates volume if volume element contains: 2 bytes for round(tsdf*SHORT_MAX) and 2 bytes for integer weight.
+ * \param[in] depth_raw Kinect depth image
+ * \param[in] intr camera intrinsics
+ * \param[in] volume_size size of volume in mm
+ * \param[in] Rcurr_inv inverse rotation for current camera pose
+ * \param[in] tcurr translation for current camera pose
+ * \param[in] tranc_dist tsdf truncation distance
+ * \param[in] volume tsdf volume to be updated
+ * \param[out] depthRawScaled Buffer for scaled depth along ray
+ */
+ PCL_EXPORTS void
+ integrateTsdfVolume (const PtrStepSz<ushort>& depth_raw, const Intr& intr, const float3& volume_size,
+ const Mat33& Rcurr_inv, const float3& tcurr, float tranc_dist, PtrStep<short2> volume, DeviceArray2D<float>& depthRawScaled);
+
+ /** \brief Initialzied color volume
+ * \param[out] color_volume color volume for initialization
+ */
+
+ void
+ initColorVolume(PtrStep<uchar4> color_volume);
+
+ /** \brief Performs integration in color volume
+ * \param[in] intr Depth camera intrionsics structure
+ * \param[in] tranc_dist tsdf truncation distance
+ * \param[in] R_inv Inverse camera rotation
+ * \param[in] t camera translation
+ * \param[in] vmap Raycasted vertex map
+ * \param[in] colors RGB colors for current frame
+ * \param[in] volume_size volume size in meters
+ * \param[in] color_volume color volume to be integrated
+ * \param[in] max_weight max weight for running color average. Zero means not average, one means average with prev value, etc.
+ */
+ void
+ updateColorVolume(const Intr& intr, float tranc_dist, const Mat33& R_inv, const float3& t, const MapArr& vmap,
+ const PtrStepSz<uchar3>& colors, const float3& volume_size, PtrStep<uchar4> color_volume, int max_weight = 1);
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Raycast and view generation
+ /** \brief Generation vertex and normal maps from volume for current camera pose
+ * \param[in] intr camera intrinsices
+ * \param[in] Rcurr current rotation
+ * \param[in] tcurr current translation
+ * \param[in] tranc_dist volume truncation distance
+ * \param[in] volume_size volume size in mm
+ * \param[in] volume tsdf volume
+ * \param[out] vmap output vertex map
+ * \param[out] nmap output normals map
+ */
+ void
+ raycast (const Intr& intr, const Mat33& Rcurr, const float3& tcurr, float tranc_dist, const float3& volume_size,
+ const PtrStep<short2>& volume, MapArr& vmap, MapArr& nmap);
+
+ /** \brief Renders 3D image of the scene
+ * \param[in] vmap vetex map
+ * \param[in] nmap normals map
+ * \param[in] light poase of light source
+ * \param[out] dst buffer where image is generated
+ */
+ void
+ generateImage (const MapArr& vmap, const MapArr& nmap, const LightSource& light, PtrStepSz<uchar3> dst);
+
+
+ /** \brief Renders depth image from give pose
+ * \param[in] R_inv inverse camera rotation
+ * \param[in] t camera translation
+ * \param[in] vmap vertex map
+ * \param[out] dst buffer where depth is generated
+ */
+ void
+ generateDepth (const Mat33& R_inv, const float3& t, const MapArr& vmap, DepthMap& dst);
+
+ /** \brief Paints 3D view with color map
+ * \param[in] colors rgb color frame from OpenNI
+ * \param[out] dst output 3D view
+ * \param[in] colors_weight weight for colors
+ */
+ void
+ paint3DView(const PtrStep<uchar3>& colors, PtrStepSz<uchar3> dst, float colors_weight = 0.5f);
+
+ /** \brief Performs resize of vertex map to next pyramid level by averaging each four points
+ * \param[in] input vertext map
+ * \param[out] output resized vertex map
+ */
+ void
+ resizeVMap (const MapArr& input, MapArr& output);
+
+ /** \brief Performs resize of vertex map to next pyramid level by averaging each four normals
+ * \param[in] input normal map
+ * \param[out] output vertex map
+ */
+ void
+ resizeNMap (const MapArr& input, MapArr& output);
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Cloud extraction
+
+ /** \brief Perform point cloud extraction from tsdf volume
+ * \param[in] volume tsdf volume
+ * \param[in] volume_size size of the volume
+ * \param[out] output buffer large enought to store point cloud
+ * \return number of point stored to passed buffer
+ */
+ PCL_EXPORTS size_t
+ extractCloud (const PtrStep<short2>& volume, const float3& volume_size, PtrSz<PointType> output);
+
+ /** \brief Performs normals computation for given poins using tsdf volume
+ * \param[in] volume tsdf volume
+ * \param[in] volume_size volume size
+ * \param[in] input points where normals are computed
+ * \param[out] output normals. Could be float4 or float8. If for a point normal can't be computed, such normal is marked as nan.
+ */
+ template<typename NormalType>
+ void
+ extractNormals (const PtrStep<short2>& volume, const float3& volume_size, const PtrSz<PointType>& input, NormalType* output);
+
+ /** \brief Performs colors exctraction from color volume
+ * \param[in] color_volume color volume
+ * \param[in] volume_size volume size
+ * \param[in] points points for which color are computed
+ * \param[out] colors output array with colors.
+ */
+ void
+ exctractColors(const PtrStep<uchar4>& color_volume, const float3& volume_size, const PtrSz<PointType>& points, uchar4* colors);
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Utility
+ struct float8 { float x, y, z, w, c1, c2, c3, c4; };
+ struct float12 { float x, y, z, w, normal_x, normal_y, normal_z, n4, c1, c2, c3, c4; };
+
+ /** \brief Conversion from SOA to AOS
+ * \param[in] vmap SOA map
+ * \param[out] output Array of 3D points. Can be float4 or float8.
+ */
+ template<typename T>
+ void
+ convert (const MapArr& vmap, DeviceArray2D<T>& output);
+
+ /** \brief Merges pcl::PointXYZ and pcl::Normal to PointNormal
+ * \param[in] cloud points cloud
+ * \param[in] normals normals cloud
+ * \param[out] output array of PointNomals.
+ */
+ void
+ mergePointNormal(const DeviceArray<float4>& cloud, const DeviceArray<float8>& normals, const DeviceArray<float12>& output);
+
+ /** \brief Check for qnan (unused now)
+ * \param[in] value
+ */
+ inline bool
+ valid_host (float value)
+ {
+ return *reinterpret_cast<int*>(&value) != 0x7fffffff; //QNAN
+ }
+
+ /** \brief synchronizes CUDA execution */
+ inline
+ void
+ sync () { cudaSafeCall (cudaDeviceSynchronize ()); }
+
+
+ template<class D, class Matx> D&
+ device_cast (Matx& matx)
+ {
+ return (*reinterpret_cast<D*>(matx.data ()));
+ }
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Marching cubes implementation
+
+ /** \brief Binds marching cubes tables to texture references */
+ void
+ bindTextures(const int *edgeBuf, const int *triBuf, const int *numVertsBuf);
+
+ /** \brief Unbinds */
+ void
+ unbindTextures();
+
+ /** \brief Scans tsdf volume and retrieves occuped voxes
+ * \param[in] volume tsdf volume
+ * \param[out] occupied_voxels buffer for occuped voxels. The function fulfills first row with voxel ids and second row with number of vertextes.
+ * \return number of voxels in the buffer
+ */
+ int
+ getOccupiedVoxels(const PtrStep<short2>& volume, DeviceArray2D<int>& occupied_voxels);
+
+ /** \brief Computes total number of vertexes for all voxels and offsets of vertexes in final triangle array
+ * \param[out] occupied_voxels buffer with occuped voxels. The function fulfills 3nd only with offsets
+ * \return total number of vertexes
+ */
+ int
+ computeOffsetsAndTotalVertexes(DeviceArray2D<int>& occupied_voxels);
+
+ /** \brief Generates final triangle array
+ * \param[in] volume tsdf volume
+ * \param[in] occupied_voxels occuped voxel ids (first row), number of vertexes(second row), offsets(third row).
+ * \param[in] volume_size volume size in meters
+ * \param[out] output triangle array
+ */
+ void
+ generateTriangles(const PtrStep<short2>& volume, const DeviceArray2D<int>& occupied_voxels, const float3& volume_size, DeviceArray<PointType>& output);
+ }
+}
+
+#endif /* PCL_KINFU_INTERNAL_HPP_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <iostream>
+#include <algorithm>
+
+#include <pcl/common/time.h>
+#include <pcl/gpu/kinfu/kinfu.h>
+#include "internal.h"
+
+#include <Eigen/Core>
+#include <Eigen/SVD>
+#include <Eigen/Cholesky>
+#include <Eigen/Geometry>
+#include <Eigen/LU>
+
+#ifdef HAVE_OPENCV
+ #include <opencv2/opencv.hpp>
+ #include <opencv2/gpu/gpu.hpp>
+#endif
+
+using namespace std;
+using namespace pcl::device;
+using namespace pcl::gpu;
+
+using Eigen::AngleAxisf;
+using Eigen::Array3f;
+using Eigen::Vector3i;
+using Eigen::Vector3f;
+
+namespace pcl
+{
+ namespace gpu
+ {
+ Eigen::Vector3f rodrigues2(const Eigen::Matrix3f& matrix);
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::gpu::KinfuTracker::KinfuTracker (int rows, int cols) : rows_(rows), cols_(cols), global_time_(0), max_icp_distance_(0), integration_metric_threshold_(0.f), disable_icp_(false)
+{
+ const Vector3f volume_size = Vector3f::Constant (VOLUME_SIZE);
+ const Vector3i volume_resolution(VOLUME_X, VOLUME_Y, VOLUME_Z);
+
+ tsdf_volume_ = TsdfVolume::Ptr( new TsdfVolume(volume_resolution) );
+ tsdf_volume_->setSize(volume_size);
+
+ setDepthIntrinsics (KINFU_DEFAULT_DEPTH_FOCAL_X, KINFU_DEFAULT_DEPTH_FOCAL_Y); // default values, can be overwritten
+
+ init_Rcam_ = Eigen::Matrix3f::Identity ();// * AngleAxisf(-30.f/180*3.1415926, Vector3f::UnitX());
+ init_tcam_ = volume_size * 0.5f - Vector3f (0, 0, volume_size (2) / 2 * 1.2f);
+
+ const int iters[] = {10, 5, 4};
+ std::copy (iters, iters + LEVELS, icp_iterations_);
+
+ const float default_distThres = 0.10f; //meters
+ const float default_angleThres = sin (20.f * 3.14159254f / 180.f);
+ const float default_tranc_dist = 0.03f; //meters
+
+ setIcpCorespFilteringParams (default_distThres, default_angleThres);
+ tsdf_volume_->setTsdfTruncDist (default_tranc_dist);
+
+ allocateBufffers (rows, cols);
+
+ rmats_.reserve (30000);
+ tvecs_.reserve (30000);
+
+ reset ();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::KinfuTracker::setDepthIntrinsics (float fx, float fy, float cx, float cy)
+{
+ fx_ = fx;
+ fy_ = fy;
+ cx_ = (cx == -1) ? cols_/2-0.5f : cx;
+ cy_ = (cy == -1) ? rows_/2-0.5f : cy;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::KinfuTracker::getDepthIntrinsics (float& fx, float& fy, float& cx, float& cy)
+{
+ fx = fx_;
+ fy = fy_;
+ cx = cx_;
+ cy = cy_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::KinfuTracker::setInitalCameraPose (const Eigen::Affine3f& pose)
+{
+ init_Rcam_ = pose.rotation ();
+ init_tcam_ = pose.translation ();
+ reset ();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::KinfuTracker::setDepthTruncationForICP (float max_icp_distance)
+{
+ max_icp_distance_ = max_icp_distance;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::KinfuTracker::setCameraMovementThreshold(float threshold)
+{
+ integration_metric_threshold_ = threshold;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::KinfuTracker::setIcpCorespFilteringParams (float distThreshold, float sineOfAngle)
+{
+ distThres_ = distThreshold; //mm
+ angleThres_ = sineOfAngle;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+int
+pcl::gpu::KinfuTracker::cols ()
+{
+ return (cols_);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+int
+pcl::gpu::KinfuTracker::rows ()
+{
+ return (rows_);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::KinfuTracker::reset()
+{
+ if (global_time_)
+ cout << "Reset" << endl;
+
+ global_time_ = 0;
+ rmats_.clear ();
+ tvecs_.clear ();
+
+ rmats_.push_back (init_Rcam_);
+ tvecs_.push_back (init_tcam_);
+
+ tsdf_volume_->reset();
+
+ if (color_volume_) // color integration mode is enabled
+ color_volume_->reset();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::KinfuTracker::allocateBufffers (int rows, int cols)
+{
+ depths_curr_.resize (LEVELS);
+ vmaps_g_curr_.resize (LEVELS);
+ nmaps_g_curr_.resize (LEVELS);
+
+ vmaps_g_prev_.resize (LEVELS);
+ nmaps_g_prev_.resize (LEVELS);
+
+ vmaps_curr_.resize (LEVELS);
+ nmaps_curr_.resize (LEVELS);
+
+ coresps_.resize (LEVELS);
+
+ for (int i = 0; i < LEVELS; ++i)
+ {
+ int pyr_rows = rows >> i;
+ int pyr_cols = cols >> i;
+
+ depths_curr_[i].create (pyr_rows, pyr_cols);
+
+ vmaps_g_curr_[i].create (pyr_rows*3, pyr_cols);
+ nmaps_g_curr_[i].create (pyr_rows*3, pyr_cols);
+
+ vmaps_g_prev_[i].create (pyr_rows*3, pyr_cols);
+ nmaps_g_prev_[i].create (pyr_rows*3, pyr_cols);
+
+ vmaps_curr_[i].create (pyr_rows*3, pyr_cols);
+ nmaps_curr_[i].create (pyr_rows*3, pyr_cols);
+
+ coresps_[i].create (pyr_rows, pyr_cols);
+ }
+ depthRawScaled_.create (rows, cols);
+ // see estimate tranform for the magic numbers
+ gbuf_.create (27, 20*60);
+ sumbuf_.create (27);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::gpu::KinfuTracker::operator() (const DepthMap& depth_raw,
+ Eigen::Affine3f *hint)
+{
+ device::Intr intr (fx_, fy_, cx_, cy_);
+
+ if (!disable_icp_)
+ {
+ {
+ //ScopeTime time(">>> Bilateral, pyr-down-all, create-maps-all");
+ //depth_raw.copyTo(depths_curr[0]);
+ device::bilateralFilter (depth_raw, depths_curr_[0]);
+
+ if (max_icp_distance_ > 0)
+ device::truncateDepth(depths_curr_[0], max_icp_distance_);
+
+ for (int i = 1; i < LEVELS; ++i)
+ device::pyrDown (depths_curr_[i-1], depths_curr_[i]);
+
+ for (int i = 0; i < LEVELS; ++i)
+ {
+ device::createVMap (intr(i), depths_curr_[i], vmaps_curr_[i]);
+ //device::createNMap(vmaps_curr_[i], nmaps_curr_[i]);
+ computeNormalsEigen (vmaps_curr_[i], nmaps_curr_[i]);
+ }
+ pcl::device::sync ();
+ }
+
+ //can't perform more on first frame
+ if (global_time_ == 0)
+ {
+ Matrix3frm init_Rcam = rmats_[0]; // [Ri|ti] - pos of camera, i.e.
+ Vector3f init_tcam = tvecs_[0]; // transform from camera to global coo space for (i-1)th camera pose
+
+ Mat33& device_Rcam = device_cast<Mat33> (init_Rcam);
+ float3& device_tcam = device_cast<float3>(init_tcam);
+
+ Matrix3frm init_Rcam_inv = init_Rcam.inverse ();
+ Mat33& device_Rcam_inv = device_cast<Mat33> (init_Rcam_inv);
+ float3 device_volume_size = device_cast<const float3>(tsdf_volume_->getSize());
+
+ //integrateTsdfVolume(depth_raw, intr, device_volume_size, device_Rcam_inv, device_tcam, tranc_dist, volume_);
+ device::integrateTsdfVolume(depth_raw, intr, device_volume_size, device_Rcam_inv, device_tcam, tsdf_volume_->getTsdfTruncDist(), tsdf_volume_->data(), depthRawScaled_);
+
+ for (int i = 0; i < LEVELS; ++i)
+ device::tranformMaps (vmaps_curr_[i], nmaps_curr_[i], device_Rcam, device_tcam, vmaps_g_prev_[i], nmaps_g_prev_[i]);
+
+ ++global_time_;
+ return (false);
+ }
+
+ ///////////////////////////////////////////////////////////////////////////////////////////
+ // Iterative Closest Point
+ Matrix3frm Rprev = rmats_[global_time_ - 1]; // [Ri|ti] - pos of camera, i.e.
+ Vector3f tprev = tvecs_[global_time_ - 1]; // tranfrom from camera to global coo space for (i-1)th camera pose
+ Matrix3frm Rprev_inv = Rprev.inverse (); //Rprev.t();
+
+ //Mat33& device_Rprev = device_cast<Mat33> (Rprev);
+ Mat33& device_Rprev_inv = device_cast<Mat33> (Rprev_inv);
+ float3& device_tprev = device_cast<float3> (tprev);
+ Matrix3frm Rcurr;
+ Vector3f tcurr;
+ if(hint)
+ {
+ Rcurr = hint->rotation().matrix();
+ tcurr = hint->translation().matrix();
+ }
+ else
+ {
+ Rcurr = Rprev; // tranform to global coo for ith camera pose
+ tcurr = tprev;
+ }
+ {
+ //ScopeTime time("icp-all");
+ for (int level_index = LEVELS-1; level_index>=0; --level_index)
+ {
+ int iter_num = icp_iterations_[level_index];
+
+ MapArr& vmap_curr = vmaps_curr_[level_index];
+ MapArr& nmap_curr = nmaps_curr_[level_index];
+
+ //MapArr& vmap_g_curr = vmaps_g_curr_[level_index];
+ //MapArr& nmap_g_curr = nmaps_g_curr_[level_index];
+
+ MapArr& vmap_g_prev = vmaps_g_prev_[level_index];
+ MapArr& nmap_g_prev = nmaps_g_prev_[level_index];
+
+ //CorespMap& coresp = coresps_[level_index];
+
+ for (int iter = 0; iter < iter_num; ++iter)
+ {
+ Mat33& device_Rcurr = device_cast<Mat33> (Rcurr);
+ float3& device_tcurr = device_cast<float3>(tcurr);
+
+ Eigen::Matrix<double, 6, 6, Eigen::RowMajor> A;
+ Eigen::Matrix<double, 6, 1> b;
+ #if 0
+ device::tranformMaps(vmap_curr, nmap_curr, device_Rcurr, device_tcurr, vmap_g_curr, nmap_g_curr);
+ findCoresp(vmap_g_curr, nmap_g_curr, device_Rprev_inv, device_tprev, intr(level_index), vmap_g_prev, nmap_g_prev, distThres_, angleThres_, coresp);
+ device::estimateTransform(vmap_g_prev, nmap_g_prev, vmap_g_curr, coresp, gbuf_, sumbuf_, A.data(), b.data());
+
+ //cv::gpu::GpuMat ma(coresp.rows(), coresp.cols(), CV_32S, coresp.ptr(), coresp.step());
+ //cv::Mat cpu;
+ //ma.download(cpu);
+ //cv::imshow(names[level_index] + string(" --- coresp white == -1"), cpu == -1);
+ #else
+ estimateCombined (device_Rcurr, device_tcurr, vmap_curr, nmap_curr, device_Rprev_inv, device_tprev, intr (level_index),
+ vmap_g_prev, nmap_g_prev, distThres_, angleThres_, gbuf_, sumbuf_, A.data (), b.data ());
+ #endif
+ //checking nullspace
+ double det = A.determinant ();
+
+ if (fabs (det) < 1e-15 || pcl_isnan (det))
+ {
+ if (pcl_isnan (det)) cout << "qnan" << endl;
+
+ reset ();
+ return (false);
+ }
+ //float maxc = A.maxCoeff();
+
+ Eigen::Matrix<float, 6, 1> result = A.llt ().solve (b).cast<float>();
+ //Eigen::Matrix<float, 6, 1> result = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
+
+ float alpha = result (0);
+ float beta = result (1);
+ float gamma = result (2);
+
+ Eigen::Matrix3f Rinc = (Eigen::Matrix3f)AngleAxisf (gamma, Vector3f::UnitZ ()) * AngleAxisf (beta, Vector3f::UnitY ()) * AngleAxisf (alpha, Vector3f::UnitX ());
+ Vector3f tinc = result.tail<3> ();
+
+ //compose
+ tcurr = Rinc * tcurr + tinc;
+ Rcurr = Rinc * Rcurr;
+ }
+ }
+ }
+ //save tranform
+ rmats_.push_back (Rcurr);
+ tvecs_.push_back (tcurr);
+ }
+ else /* if (disable_icp_) */
+ {
+ if (global_time_ == 0)
+ ++global_time_;
+
+ Matrix3frm Rcurr = rmats_[global_time_ - 1];
+ Vector3f tcurr = tvecs_[global_time_ - 1];
+
+ rmats_.push_back (Rcurr);
+ tvecs_.push_back (tcurr);
+
+ }
+
+ Matrix3frm Rprev = rmats_[global_time_ - 1];
+ Vector3f tprev = tvecs_[global_time_ - 1];
+
+ Matrix3frm Rcurr = rmats_.back();
+ Vector3f tcurr = tvecs_.back();
+
+ ///////////////////////////////////////////////////////////////////////////////////////////
+ // Integration check - We do not integrate volume if camera does not move.
+ float rnorm = rodrigues2(Rcurr.inverse() * Rprev).norm();
+ float tnorm = (tcurr - tprev).norm();
+ const float alpha = 1.f;
+ bool integrate = (rnorm + alpha * tnorm)/2 >= integration_metric_threshold_;
+
+ if (disable_icp_)
+ integrate = true;
+
+ ///////////////////////////////////////////////////////////////////////////////////////////
+ // Volume integration
+ float3 device_volume_size = device_cast<const float3> (tsdf_volume_->getSize());
+
+ Matrix3frm Rcurr_inv = Rcurr.inverse ();
+ Mat33& device_Rcurr_inv = device_cast<Mat33> (Rcurr_inv);
+ float3& device_tcurr = device_cast<float3> (tcurr);
+ if (integrate)
+ {
+ //ScopeTime time("tsdf");
+ //integrateTsdfVolume(depth_raw, intr, device_volume_size, device_Rcurr_inv, device_tcurr, tranc_dist, volume_);
+ integrateTsdfVolume (depth_raw, intr, device_volume_size, device_Rcurr_inv, device_tcurr, tsdf_volume_->getTsdfTruncDist(), tsdf_volume_->data(), depthRawScaled_);
+ }
+
+ ///////////////////////////////////////////////////////////////////////////////////////////
+ // Ray casting
+ Mat33& device_Rcurr = device_cast<Mat33> (Rcurr);
+ {
+ //ScopeTime time("ray-cast-all");
+ raycast (intr, device_Rcurr, device_tcurr, tsdf_volume_->getTsdfTruncDist(), device_volume_size, tsdf_volume_->data(), vmaps_g_prev_[0], nmaps_g_prev_[0]);
+ for (int i = 1; i < LEVELS; ++i)
+ {
+ resizeVMap (vmaps_g_prev_[i-1], vmaps_g_prev_[i]);
+ resizeNMap (nmaps_g_prev_[i-1], nmaps_g_prev_[i]);
+ }
+ pcl::device::sync ();
+ }
+
+ ++global_time_;
+ return (true);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Eigen::Affine3f
+pcl::gpu::KinfuTracker::getCameraPose (int time) const
+{
+ if (time > (int)rmats_.size () || time < 0)
+ time = rmats_.size () - 1;
+
+ Eigen::Affine3f aff;
+ aff.linear () = rmats_[time];
+ aff.translation () = tvecs_[time];
+ return (aff);
+}
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+size_t
+pcl::gpu::KinfuTracker::getNumberOfPoses () const
+{
+ return rmats_.size();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+const TsdfVolume&
+pcl::gpu::KinfuTracker::volume() const
+{
+ return *tsdf_volume_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+TsdfVolume&
+pcl::gpu::KinfuTracker::volume()
+{
+ return *tsdf_volume_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+const ColorVolume&
+pcl::gpu::KinfuTracker::colorVolume() const
+{
+ return *color_volume_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ColorVolume&
+pcl::gpu::KinfuTracker::colorVolume()
+{
+ return *color_volume_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::KinfuTracker::getImage (View& view) const
+{
+ //Eigen::Vector3f light_source_pose = tsdf_volume_->getSize() * (-3.f);
+ Eigen::Vector3f light_source_pose = tvecs_[tvecs_.size () - 1];
+
+ device::LightSource light;
+ light.number = 1;
+ light.pos[0] = device_cast<const float3>(light_source_pose);
+
+ view.create (rows_, cols_);
+ generateImage (vmaps_g_prev_[0], nmaps_g_prev_[0], light, view);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::KinfuTracker::getLastFrameCloud (DeviceArray2D<PointType>& cloud) const
+{
+ cloud.create (rows_, cols_);
+ DeviceArray2D<float4>& c = (DeviceArray2D<float4>&)cloud;
+ device::convert (vmaps_g_prev_[0], c);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::KinfuTracker::getLastFrameNormals (DeviceArray2D<NormalType>& normals) const
+{
+ normals.create (rows_, cols_);
+ DeviceArray2D<float8>& n = (DeviceArray2D<float8>&)normals;
+ device::convert (nmaps_g_prev_[0], n);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::KinfuTracker::disableIcp() { disable_icp_ = true; }
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::KinfuTracker::initColorIntegration(int max_weight)
+{
+ color_volume_ = pcl::gpu::ColorVolume::Ptr( new ColorVolume(*tsdf_volume_, max_weight) );
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::gpu::KinfuTracker::operator() (const DepthMap& depth, const View& colors)
+{
+ bool res = (*this)(depth);
+
+ if (res && color_volume_)
+ {
+ const float3 device_volume_size = device_cast<const float3> (tsdf_volume_->getSize());
+ device::Intr intr(fx_, fy_, cx_, cy_);
+
+ Matrix3frm R_inv = rmats_.back().inverse();
+ Vector3f t = tvecs_.back();
+
+ Mat33& device_Rcurr_inv = device_cast<Mat33> (R_inv);
+ float3& device_tcurr = device_cast<float3> (t);
+
+ device::updateColorVolume(intr, tsdf_volume_->getTsdfTruncDist(), device_Rcurr_inv, device_tcurr, vmaps_g_prev_[0],
+ colors, device_volume_size, color_volume_->data(), color_volume_->getMaxWeight());
+ }
+
+ return res;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+ namespace gpu
+ {
+ PCL_EXPORTS void
+ paint3DView(const KinfuTracker::View& rgb24, KinfuTracker::View& view, float colors_weight = 0.5f)
+ {
+ device::paint3DView(rgb24, view, colors_weight);
+ }
+
+ PCL_EXPORTS void
+ mergePointNormal(const DeviceArray<PointXYZ>& cloud, const DeviceArray<Normal>& normals, DeviceArray<PointNormal>& output)
+ {
+ const size_t size = min(cloud.size(), normals.size());
+ output.create(size);
+
+ const DeviceArray<float4>& c = (const DeviceArray<float4>&)cloud;
+ const DeviceArray<float8>& n = (const DeviceArray<float8>&)normals;
+ const DeviceArray<float12>& o = (const DeviceArray<float12>&)output;
+ device::mergePointNormal(c, n, o);
+ }
+
+ Eigen::Vector3f rodrigues2(const Eigen::Matrix3f& matrix)
+ {
+ Eigen::JacobiSVD<Eigen::Matrix3f> svd(matrix, Eigen::ComputeFullV | Eigen::ComputeFullU);
+ Eigen::Matrix3f R = svd.matrixU() * svd.matrixV().transpose();
+
+ double rx = R(2, 1) - R(1, 2);
+ double ry = R(0, 2) - R(2, 0);
+ double rz = R(1, 0) - R(0, 1);
+
+ double s = sqrt((rx*rx + ry*ry + rz*rz)*0.25);
+ double c = (R.trace() - 1) * 0.5;
+ c = c > 1. ? 1. : c < -1. ? -1. : c;
+
+ double theta = acos(c);
+
+ if( s < 1e-5 )
+ {
+ double t;
+
+ if( c > 0 )
+ rx = ry = rz = 0;
+ else
+ {
+ t = (R(0, 0) + 1)*0.5;
+ rx = sqrt( std::max(t, 0.0) );
+ t = (R(1, 1) + 1)*0.5;
+ ry = sqrt( std::max(t, 0.0) ) * (R(0, 1) < 0 ? -1.0 : 1.0);
+ t = (R(2, 2) + 1)*0.5;
+ rz = sqrt( std::max(t, 0.0) ) * (R(0, 2) < 0 ? -1.0 : 1.0);
+
+ if( fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R(1, 2) > 0) != (ry*rz > 0) )
+ rz = -rz;
+ theta /= sqrt(rx*rx + ry*ry + rz*rz);
+ rx *= theta;
+ ry *= theta;
+ rz *= theta;
+ }
+ }
+ else
+ {
+ double vth = 1/(2*s);
+ vth *= theta;
+ rx *= vth; ry *= vth; rz *= vth;
+ }
+ return Eigen::Vector3d(rx, ry, rz).cast<float>();
+ }
+ }
+}
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Point Cloud Library (PCL) - www.pointclouds.org
+* Copyright (c) 2011, Willow Garage, Inc.
+*
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+*/
+
+#include <pcl/gpu/kinfu/kinfu.h>
+#include <pcl/gpu/kinfu/marching_cubes.h>
+#include "internal.h"
+
+using namespace pcl;
+using namespace pcl::gpu;
+using pcl::device::device_cast;
+
+extern const int edgeTable[256];
+extern const int triTable[256][16];
+extern const int numVertsTable[256];
+
+pcl::gpu::MarchingCubes::MarchingCubes()
+{
+ edgeTable_.upload(edgeTable, 256);
+ numVertsTable_.upload(numVertsTable, 256);
+ triTable_.upload(&triTable[0][0], 256 * 16);
+}
+
+pcl::gpu::MarchingCubes::~MarchingCubes() {}
+
+DeviceArray<pcl::gpu::MarchingCubes::PointType>
+pcl::gpu::MarchingCubes::run(const TsdfVolume& tsdf, DeviceArray<PointType>& triangles_buffer)
+{
+ if (triangles_buffer.empty())
+ triangles_buffer.create(DEFAULT_TRIANGLES_BUFFER_SIZE);
+ occupied_voxels_buffer_.create(3, static_cast<int> (triangles_buffer.size () / 3));
+
+ device::bindTextures(edgeTable_, triTable_, numVertsTable_);
+
+ int active_voxels = device::getOccupiedVoxels(tsdf.data(), occupied_voxels_buffer_);
+ if(!active_voxels)
+ {
+ device::unbindTextures();
+ return DeviceArray<PointType>();
+ }
+
+ DeviceArray2D<int> occupied_voxels(3, active_voxels, occupied_voxels_buffer_.ptr(), occupied_voxels_buffer_.step());
+
+ int total_vertexes = device::computeOffsetsAndTotalVertexes(occupied_voxels);
+
+ float3 volume_size = device_cast<const float3>(tsdf.getSize());
+ device::generateTriangles(tsdf.data(), occupied_voxels, volume_size, (DeviceArray<device::PointType>&)triangles_buffer);
+
+ device::unbindTextures();
+ return DeviceArray<PointType>(triangles_buffer.ptr(), total_vertexes);
+}
+
+
+// edge table maps 8-bit flag representing which cube vertices are inside
+// the isosurface to 12-bit number indicating which edges are intersected
+const int edgeTable[256] =
+{
+ 0x0 , 0x109, 0x203, 0x30a, 0x406, 0x50f, 0x605, 0x70c,
+ 0x80c, 0x905, 0xa0f, 0xb06, 0xc0a, 0xd03, 0xe09, 0xf00,
+ 0x190, 0x99 , 0x393, 0x29a, 0x596, 0x49f, 0x795, 0x69c,
+ 0x99c, 0x895, 0xb9f, 0xa96, 0xd9a, 0xc93, 0xf99, 0xe90,
+ 0x230, 0x339, 0x33 , 0x13a, 0x636, 0x73f, 0x435, 0x53c,
+ 0xa3c, 0xb35, 0x83f, 0x936, 0xe3a, 0xf33, 0xc39, 0xd30,
+ 0x3a0, 0x2a9, 0x1a3, 0xaa , 0x7a6, 0x6af, 0x5a5, 0x4ac,
+ 0xbac, 0xaa5, 0x9af, 0x8a6, 0xfaa, 0xea3, 0xda9, 0xca0,
+ 0x460, 0x569, 0x663, 0x76a, 0x66 , 0x16f, 0x265, 0x36c,
+ 0xc6c, 0xd65, 0xe6f, 0xf66, 0x86a, 0x963, 0xa69, 0xb60,
+ 0x5f0, 0x4f9, 0x7f3, 0x6fa, 0x1f6, 0xff , 0x3f5, 0x2fc,
+ 0xdfc, 0xcf5, 0xfff, 0xef6, 0x9fa, 0x8f3, 0xbf9, 0xaf0,
+ 0x650, 0x759, 0x453, 0x55a, 0x256, 0x35f, 0x55 , 0x15c,
+ 0xe5c, 0xf55, 0xc5f, 0xd56, 0xa5a, 0xb53, 0x859, 0x950,
+ 0x7c0, 0x6c9, 0x5c3, 0x4ca, 0x3c6, 0x2cf, 0x1c5, 0xcc ,
+ 0xfcc, 0xec5, 0xdcf, 0xcc6, 0xbca, 0xac3, 0x9c9, 0x8c0,
+ 0x8c0, 0x9c9, 0xac3, 0xbca, 0xcc6, 0xdcf, 0xec5, 0xfcc,
+ 0xcc , 0x1c5, 0x2cf, 0x3c6, 0x4ca, 0x5c3, 0x6c9, 0x7c0,
+ 0x950, 0x859, 0xb53, 0xa5a, 0xd56, 0xc5f, 0xf55, 0xe5c,
+ 0x15c, 0x55 , 0x35f, 0x256, 0x55a, 0x453, 0x759, 0x650,
+ 0xaf0, 0xbf9, 0x8f3, 0x9fa, 0xef6, 0xfff, 0xcf5, 0xdfc,
+ 0x2fc, 0x3f5, 0xff , 0x1f6, 0x6fa, 0x7f3, 0x4f9, 0x5f0,
+ 0xb60, 0xa69, 0x963, 0x86a, 0xf66, 0xe6f, 0xd65, 0xc6c,
+ 0x36c, 0x265, 0x16f, 0x66 , 0x76a, 0x663, 0x569, 0x460,
+ 0xca0, 0xda9, 0xea3, 0xfaa, 0x8a6, 0x9af, 0xaa5, 0xbac,
+ 0x4ac, 0x5a5, 0x6af, 0x7a6, 0xaa , 0x1a3, 0x2a9, 0x3a0,
+ 0xd30, 0xc39, 0xf33, 0xe3a, 0x936, 0x83f, 0xb35, 0xa3c,
+ 0x53c, 0x435, 0x73f, 0x636, 0x13a, 0x33 , 0x339, 0x230,
+ 0xe90, 0xf99, 0xc93, 0xd9a, 0xa96, 0xb9f, 0x895, 0x99c,
+ 0x69c, 0x795, 0x49f, 0x596, 0x29a, 0x393, 0x99 , 0x190,
+ 0xf00, 0xe09, 0xd03, 0xc0a, 0xb06, 0xa0f, 0x905, 0x80c,
+ 0x70c, 0x605, 0x50f, 0x406, 0x30a, 0x203, 0x109, 0x0
+};
+
+// triangle table maps same cube vertex index to a list of up to 5 triangles
+// which are built from the interpolated edge vertices
+const int triTable[256][16] =
+{
+ {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 1, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 8, 3, 9, 8, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 2, 10, 0, 2, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {2, 8, 3, 2, 10, 8, 10, 9, 8, -1, -1, -1, -1, -1, -1, -1},
+ {3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 11, 2, 8, 11, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 9, 0, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 11, 2, 1, 9, 11, 9, 8, 11, -1, -1, -1, -1, -1, -1, -1},
+ {3, 10, 1, 11, 10, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 10, 1, 0, 8, 10, 8, 11, 10, -1, -1, -1, -1, -1, -1, -1},
+ {3, 9, 0, 3, 11, 9, 11, 10, 9, -1, -1, -1, -1, -1, -1, -1},
+ {9, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 3, 0, 7, 3, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 1, 9, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 1, 9, 4, 7, 1, 7, 3, 1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {3, 4, 7, 3, 0, 4, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1},
+ {9, 2, 10, 9, 0, 2, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1},
+ {2, 10, 9, 2, 9, 7, 2, 7, 3, 7, 9, 4, -1, -1, -1, -1},
+ {8, 4, 7, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {11, 4, 7, 11, 2, 4, 2, 0, 4, -1, -1, -1, -1, -1, -1, -1},
+ {9, 0, 1, 8, 4, 7, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1},
+ {4, 7, 11, 9, 4, 11, 9, 11, 2, 9, 2, 1, -1, -1, -1, -1},
+ {3, 10, 1, 3, 11, 10, 7, 8, 4, -1, -1, -1, -1, -1, -1, -1},
+ {1, 11, 10, 1, 4, 11, 1, 0, 4, 7, 11, 4, -1, -1, -1, -1},
+ {4, 7, 8, 9, 0, 11, 9, 11, 10, 11, 0, 3, -1, -1, -1, -1},
+ {4, 7, 11, 4, 11, 9, 9, 11, 10, -1, -1, -1, -1, -1, -1, -1},
+ {9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 5, 4, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 5, 4, 1, 5, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {8, 5, 4, 8, 3, 5, 3, 1, 5, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {3, 0, 8, 1, 2, 10, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1},
+ {5, 2, 10, 5, 4, 2, 4, 0, 2, -1, -1, -1, -1, -1, -1, -1},
+ {2, 10, 5, 3, 2, 5, 3, 5, 4, 3, 4, 8, -1, -1, -1, -1},
+ {9, 5, 4, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 11, 2, 0, 8, 11, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1},
+ {0, 5, 4, 0, 1, 5, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1},
+ {2, 1, 5, 2, 5, 8, 2, 8, 11, 4, 8, 5, -1, -1, -1, -1},
+ {10, 3, 11, 10, 1, 3, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1},
+ {4, 9, 5, 0, 8, 1, 8, 10, 1, 8, 11, 10, -1, -1, -1, -1},
+ {5, 4, 0, 5, 0, 11, 5, 11, 10, 11, 0, 3, -1, -1, -1, -1},
+ {5, 4, 8, 5, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1},
+ {9, 7, 8, 5, 7, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 3, 0, 9, 5, 3, 5, 7, 3, -1, -1, -1, -1, -1, -1, -1},
+ {0, 7, 8, 0, 1, 7, 1, 5, 7, -1, -1, -1, -1, -1, -1, -1},
+ {1, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 7, 8, 9, 5, 7, 10, 1, 2, -1, -1, -1, -1, -1, -1, -1},
+ {10, 1, 2, 9, 5, 0, 5, 3, 0, 5, 7, 3, -1, -1, -1, -1},
+ {8, 0, 2, 8, 2, 5, 8, 5, 7, 10, 5, 2, -1, -1, -1, -1},
+ {2, 10, 5, 2, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1},
+ {7, 9, 5, 7, 8, 9, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1},
+ {9, 5, 7, 9, 7, 2, 9, 2, 0, 2, 7, 11, -1, -1, -1, -1},
+ {2, 3, 11, 0, 1, 8, 1, 7, 8, 1, 5, 7, -1, -1, -1, -1},
+ {11, 2, 1, 11, 1, 7, 7, 1, 5, -1, -1, -1, -1, -1, -1, -1},
+ {9, 5, 8, 8, 5, 7, 10, 1, 3, 10, 3, 11, -1, -1, -1, -1},
+ {5, 7, 0, 5, 0, 9, 7, 11, 0, 1, 0, 10, 11, 10, 0, -1},
+ {11, 10, 0, 11, 0, 3, 10, 5, 0, 8, 0, 7, 5, 7, 0, -1},
+ {11, 10, 5, 7, 11, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 0, 1, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 8, 3, 1, 9, 8, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1},
+ {1, 6, 5, 2, 6, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 6, 5, 1, 2, 6, 3, 0, 8, -1, -1, -1, -1, -1, -1, -1},
+ {9, 6, 5, 9, 0, 6, 0, 2, 6, -1, -1, -1, -1, -1, -1, -1},
+ {5, 9, 8, 5, 8, 2, 5, 2, 6, 3, 2, 8, -1, -1, -1, -1},
+ {2, 3, 11, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {11, 0, 8, 11, 2, 0, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1},
+ {0, 1, 9, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1},
+ {5, 10, 6, 1, 9, 2, 9, 11, 2, 9, 8, 11, -1, -1, -1, -1},
+ {6, 3, 11, 6, 5, 3, 5, 1, 3, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 11, 0, 11, 5, 0, 5, 1, 5, 11, 6, -1, -1, -1, -1},
+ {3, 11, 6, 0, 3, 6, 0, 6, 5, 0, 5, 9, -1, -1, -1, -1},
+ {6, 5, 9, 6, 9, 11, 11, 9, 8, -1, -1, -1, -1, -1, -1, -1},
+ {5, 10, 6, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 3, 0, 4, 7, 3, 6, 5, 10, -1, -1, -1, -1, -1, -1, -1},
+ {1, 9, 0, 5, 10, 6, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1},
+ {10, 6, 5, 1, 9, 7, 1, 7, 3, 7, 9, 4, -1, -1, -1, -1},
+ {6, 1, 2, 6, 5, 1, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 5, 5, 2, 6, 3, 0, 4, 3, 4, 7, -1, -1, -1, -1},
+ {8, 4, 7, 9, 0, 5, 0, 6, 5, 0, 2, 6, -1, -1, -1, -1},
+ {7, 3, 9, 7, 9, 4, 3, 2, 9, 5, 9, 6, 2, 6, 9, -1},
+ {3, 11, 2, 7, 8, 4, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1},
+ {5, 10, 6, 4, 7, 2, 4, 2, 0, 2, 7, 11, -1, -1, -1, -1},
+ {0, 1, 9, 4, 7, 8, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1},
+ {9, 2, 1, 9, 11, 2, 9, 4, 11, 7, 11, 4, 5, 10, 6, -1},
+ {8, 4, 7, 3, 11, 5, 3, 5, 1, 5, 11, 6, -1, -1, -1, -1},
+ {5, 1, 11, 5, 11, 6, 1, 0, 11, 7, 11, 4, 0, 4, 11, -1},
+ {0, 5, 9, 0, 6, 5, 0, 3, 6, 11, 6, 3, 8, 4, 7, -1},
+ {6, 5, 9, 6, 9, 11, 4, 7, 9, 7, 11, 9, -1, -1, -1, -1},
+ {10, 4, 9, 6, 4, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 10, 6, 4, 9, 10, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1},
+ {10, 0, 1, 10, 6, 0, 6, 4, 0, -1, -1, -1, -1, -1, -1, -1},
+ {8, 3, 1, 8, 1, 6, 8, 6, 4, 6, 1, 10, -1, -1, -1, -1},
+ {1, 4, 9, 1, 2, 4, 2, 6, 4, -1, -1, -1, -1, -1, -1, -1},
+ {3, 0, 8, 1, 2, 9, 2, 4, 9, 2, 6, 4, -1, -1, -1, -1},
+ {0, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {8, 3, 2, 8, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1},
+ {10, 4, 9, 10, 6, 4, 11, 2, 3, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 2, 2, 8, 11, 4, 9, 10, 4, 10, 6, -1, -1, -1, -1},
+ {3, 11, 2, 0, 1, 6, 0, 6, 4, 6, 1, 10, -1, -1, -1, -1},
+ {6, 4, 1, 6, 1, 10, 4, 8, 1, 2, 1, 11, 8, 11, 1, -1},
+ {9, 6, 4, 9, 3, 6, 9, 1, 3, 11, 6, 3, -1, -1, -1, -1},
+ {8, 11, 1, 8, 1, 0, 11, 6, 1, 9, 1, 4, 6, 4, 1, -1},
+ {3, 11, 6, 3, 6, 0, 0, 6, 4, -1, -1, -1, -1, -1, -1, -1},
+ {6, 4, 8, 11, 6, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {7, 10, 6, 7, 8, 10, 8, 9, 10, -1, -1, -1, -1, -1, -1, -1},
+ {0, 7, 3, 0, 10, 7, 0, 9, 10, 6, 7, 10, -1, -1, -1, -1},
+ {10, 6, 7, 1, 10, 7, 1, 7, 8, 1, 8, 0, -1, -1, -1, -1},
+ {10, 6, 7, 10, 7, 1, 1, 7, 3, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 6, 1, 6, 8, 1, 8, 9, 8, 6, 7, -1, -1, -1, -1},
+ {2, 6, 9, 2, 9, 1, 6, 7, 9, 0, 9, 3, 7, 3, 9, -1},
+ {7, 8, 0, 7, 0, 6, 6, 0, 2, -1, -1, -1, -1, -1, -1, -1},
+ {7, 3, 2, 6, 7, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {2, 3, 11, 10, 6, 8, 10, 8, 9, 8, 6, 7, -1, -1, -1, -1},
+ {2, 0, 7, 2, 7, 11, 0, 9, 7, 6, 7, 10, 9, 10, 7, -1},
+ {1, 8, 0, 1, 7, 8, 1, 10, 7, 6, 7, 10, 2, 3, 11, -1},
+ {11, 2, 1, 11, 1, 7, 10, 6, 1, 6, 7, 1, -1, -1, -1, -1},
+ {8, 9, 6, 8, 6, 7, 9, 1, 6, 11, 6, 3, 1, 3, 6, -1},
+ {0, 9, 1, 11, 6, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {7, 8, 0, 7, 0, 6, 3, 11, 0, 11, 6, 0, -1, -1, -1, -1},
+ {7, 11, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {3, 0, 8, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 1, 9, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {8, 1, 9, 8, 3, 1, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1},
+ {10, 1, 2, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, 3, 0, 8, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1},
+ {2, 9, 0, 2, 10, 9, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1},
+ {6, 11, 7, 2, 10, 3, 10, 8, 3, 10, 9, 8, -1, -1, -1, -1},
+ {7, 2, 3, 6, 2, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {7, 0, 8, 7, 6, 0, 6, 2, 0, -1, -1, -1, -1, -1, -1, -1},
+ {2, 7, 6, 2, 3, 7, 0, 1, 9, -1, -1, -1, -1, -1, -1, -1},
+ {1, 6, 2, 1, 8, 6, 1, 9, 8, 8, 7, 6, -1, -1, -1, -1},
+ {10, 7, 6, 10, 1, 7, 1, 3, 7, -1, -1, -1, -1, -1, -1, -1},
+ {10, 7, 6, 1, 7, 10, 1, 8, 7, 1, 0, 8, -1, -1, -1, -1},
+ {0, 3, 7, 0, 7, 10, 0, 10, 9, 6, 10, 7, -1, -1, -1, -1},
+ {7, 6, 10, 7, 10, 8, 8, 10, 9, -1, -1, -1, -1, -1, -1, -1},
+ {6, 8, 4, 11, 8, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {3, 6, 11, 3, 0, 6, 0, 4, 6, -1, -1, -1, -1, -1, -1, -1},
+ {8, 6, 11, 8, 4, 6, 9, 0, 1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 4, 6, 9, 6, 3, 9, 3, 1, 11, 3, 6, -1, -1, -1, -1},
+ {6, 8, 4, 6, 11, 8, 2, 10, 1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, 3, 0, 11, 0, 6, 11, 0, 4, 6, -1, -1, -1, -1},
+ {4, 11, 8, 4, 6, 11, 0, 2, 9, 2, 10, 9, -1, -1, -1, -1},
+ {10, 9, 3, 10, 3, 2, 9, 4, 3, 11, 3, 6, 4, 6, 3, -1},
+ {8, 2, 3, 8, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1},
+ {0, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 9, 0, 2, 3, 4, 2, 4, 6, 4, 3, 8, -1, -1, -1, -1},
+ {1, 9, 4, 1, 4, 2, 2, 4, 6, -1, -1, -1, -1, -1, -1, -1},
+ {8, 1, 3, 8, 6, 1, 8, 4, 6, 6, 10, 1, -1, -1, -1, -1},
+ {10, 1, 0, 10, 0, 6, 6, 0, 4, -1, -1, -1, -1, -1, -1, -1},
+ {4, 6, 3, 4, 3, 8, 6, 10, 3, 0, 3, 9, 10, 9, 3, -1},
+ {10, 9, 4, 6, 10, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 9, 5, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, 4, 9, 5, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1},
+ {5, 0, 1, 5, 4, 0, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1},
+ {11, 7, 6, 8, 3, 4, 3, 5, 4, 3, 1, 5, -1, -1, -1, -1},
+ {9, 5, 4, 10, 1, 2, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1},
+ {6, 11, 7, 1, 2, 10, 0, 8, 3, 4, 9, 5, -1, -1, -1, -1},
+ {7, 6, 11, 5, 4, 10, 4, 2, 10, 4, 0, 2, -1, -1, -1, -1},
+ {3, 4, 8, 3, 5, 4, 3, 2, 5, 10, 5, 2, 11, 7, 6, -1},
+ {7, 2, 3, 7, 6, 2, 5, 4, 9, -1, -1, -1, -1, -1, -1, -1},
+ {9, 5, 4, 0, 8, 6, 0, 6, 2, 6, 8, 7, -1, -1, -1, -1},
+ {3, 6, 2, 3, 7, 6, 1, 5, 0, 5, 4, 0, -1, -1, -1, -1},
+ {6, 2, 8, 6, 8, 7, 2, 1, 8, 4, 8, 5, 1, 5, 8, -1},
+ {9, 5, 4, 10, 1, 6, 1, 7, 6, 1, 3, 7, -1, -1, -1, -1},
+ {1, 6, 10, 1, 7, 6, 1, 0, 7, 8, 7, 0, 9, 5, 4, -1},
+ {4, 0, 10, 4, 10, 5, 0, 3, 10, 6, 10, 7, 3, 7, 10, -1},
+ {7, 6, 10, 7, 10, 8, 5, 4, 10, 4, 8, 10, -1, -1, -1, -1},
+ {6, 9, 5, 6, 11, 9, 11, 8, 9, -1, -1, -1, -1, -1, -1, -1},
+ {3, 6, 11, 0, 6, 3, 0, 5, 6, 0, 9, 5, -1, -1, -1, -1},
+ {0, 11, 8, 0, 5, 11, 0, 1, 5, 5, 6, 11, -1, -1, -1, -1},
+ {6, 11, 3, 6, 3, 5, 5, 3, 1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, 9, 5, 11, 9, 11, 8, 11, 5, 6, -1, -1, -1, -1},
+ {0, 11, 3, 0, 6, 11, 0, 9, 6, 5, 6, 9, 1, 2, 10, -1},
+ {11, 8, 5, 11, 5, 6, 8, 0, 5, 10, 5, 2, 0, 2, 5, -1},
+ {6, 11, 3, 6, 3, 5, 2, 10, 3, 10, 5, 3, -1, -1, -1, -1},
+ {5, 8, 9, 5, 2, 8, 5, 6, 2, 3, 8, 2, -1, -1, -1, -1},
+ {9, 5, 6, 9, 6, 0, 0, 6, 2, -1, -1, -1, -1, -1, -1, -1},
+ {1, 5, 8, 1, 8, 0, 5, 6, 8, 3, 8, 2, 6, 2, 8, -1},
+ {1, 5, 6, 2, 1, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 3, 6, 1, 6, 10, 3, 8, 6, 5, 6, 9, 8, 9, 6, -1},
+ {10, 1, 0, 10, 0, 6, 9, 5, 0, 5, 6, 0, -1, -1, -1, -1},
+ {0, 3, 8, 5, 6, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {10, 5, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {11, 5, 10, 7, 5, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {11, 5, 10, 11, 7, 5, 8, 3, 0, -1, -1, -1, -1, -1, -1, -1},
+ {5, 11, 7, 5, 10, 11, 1, 9, 0, -1, -1, -1, -1, -1, -1, -1},
+ {10, 7, 5, 10, 11, 7, 9, 8, 1, 8, 3, 1, -1, -1, -1, -1},
+ {11, 1, 2, 11, 7, 1, 7, 5, 1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, 1, 2, 7, 1, 7, 5, 7, 2, 11, -1, -1, -1, -1},
+ {9, 7, 5, 9, 2, 7, 9, 0, 2, 2, 11, 7, -1, -1, -1, -1},
+ {7, 5, 2, 7, 2, 11, 5, 9, 2, 3, 2, 8, 9, 8, 2, -1},
+ {2, 5, 10, 2, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1},
+ {8, 2, 0, 8, 5, 2, 8, 7, 5, 10, 2, 5, -1, -1, -1, -1},
+ {9, 0, 1, 5, 10, 3, 5, 3, 7, 3, 10, 2, -1, -1, -1, -1},
+ {9, 8, 2, 9, 2, 1, 8, 7, 2, 10, 2, 5, 7, 5, 2, -1},
+ {1, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 7, 0, 7, 1, 1, 7, 5, -1, -1, -1, -1, -1, -1, -1},
+ {9, 0, 3, 9, 3, 5, 5, 3, 7, -1, -1, -1, -1, -1, -1, -1},
+ {9, 8, 7, 5, 9, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {5, 8, 4, 5, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1},
+ {5, 0, 4, 5, 11, 0, 5, 10, 11, 11, 3, 0, -1, -1, -1, -1},
+ {0, 1, 9, 8, 4, 10, 8, 10, 11, 10, 4, 5, -1, -1, -1, -1},
+ {10, 11, 4, 10, 4, 5, 11, 3, 4, 9, 4, 1, 3, 1, 4, -1},
+ {2, 5, 1, 2, 8, 5, 2, 11, 8, 4, 5, 8, -1, -1, -1, -1},
+ {0, 4, 11, 0, 11, 3, 4, 5, 11, 2, 11, 1, 5, 1, 11, -1},
+ {0, 2, 5, 0, 5, 9, 2, 11, 5, 4, 5, 8, 11, 8, 5, -1},
+ {9, 4, 5, 2, 11, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {2, 5, 10, 3, 5, 2, 3, 4, 5, 3, 8, 4, -1, -1, -1, -1},
+ {5, 10, 2, 5, 2, 4, 4, 2, 0, -1, -1, -1, -1, -1, -1, -1},
+ {3, 10, 2, 3, 5, 10, 3, 8, 5, 4, 5, 8, 0, 1, 9, -1},
+ {5, 10, 2, 5, 2, 4, 1, 9, 2, 9, 4, 2, -1, -1, -1, -1},
+ {8, 4, 5, 8, 5, 3, 3, 5, 1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 4, 5, 1, 0, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {8, 4, 5, 8, 5, 3, 9, 0, 5, 0, 3, 5, -1, -1, -1, -1},
+ {9, 4, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 11, 7, 4, 9, 11, 9, 10, 11, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, 4, 9, 7, 9, 11, 7, 9, 10, 11, -1, -1, -1, -1},
+ {1, 10, 11, 1, 11, 4, 1, 4, 0, 7, 4, 11, -1, -1, -1, -1},
+ {3, 1, 4, 3, 4, 8, 1, 10, 4, 7, 4, 11, 10, 11, 4, -1},
+ {4, 11, 7, 9, 11, 4, 9, 2, 11, 9, 1, 2, -1, -1, -1, -1},
+ {9, 7, 4, 9, 11, 7, 9, 1, 11, 2, 11, 1, 0, 8, 3, -1},
+ {11, 7, 4, 11, 4, 2, 2, 4, 0, -1, -1, -1, -1, -1, -1, -1},
+ {11, 7, 4, 11, 4, 2, 8, 3, 4, 3, 2, 4, -1, -1, -1, -1},
+ {2, 9, 10, 2, 7, 9, 2, 3, 7, 7, 4, 9, -1, -1, -1, -1},
+ {9, 10, 7, 9, 7, 4, 10, 2, 7, 8, 7, 0, 2, 0, 7, -1},
+ {3, 7, 10, 3, 10, 2, 7, 4, 10, 1, 10, 0, 4, 0, 10, -1},
+ {1, 10, 2, 8, 7, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 9, 1, 4, 1, 7, 7, 1, 3, -1, -1, -1, -1, -1, -1, -1},
+ {4, 9, 1, 4, 1, 7, 0, 8, 1, 8, 7, 1, -1, -1, -1, -1},
+ {4, 0, 3, 7, 4, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 8, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {3, 0, 9, 3, 9, 11, 11, 9, 10, -1, -1, -1, -1, -1, -1, -1},
+ {0, 1, 10, 0, 10, 8, 8, 10, 11, -1, -1, -1, -1, -1, -1, -1},
+ {3, 1, 10, 11, 3, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 11, 1, 11, 9, 9, 11, 8, -1, -1, -1, -1, -1, -1, -1},
+ {3, 0, 9, 3, 9, 11, 1, 2, 9, 2, 11, 9, -1, -1, -1, -1},
+ {0, 2, 11, 8, 0, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {3, 2, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {2, 3, 8, 2, 8, 10, 10, 8, 9, -1, -1, -1, -1, -1, -1, -1},
+ {9, 10, 2, 0, 9, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {2, 3, 8, 2, 8, 10, 0, 1, 8, 1, 10, 8, -1, -1, -1, -1},
+ {1, 10, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 3, 8, 9, 1, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 9, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 3, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}
+};
+
+// number of vertices for each case above
+const int numVertsTable[256] =
+{
+ 0,
+ 3,
+ 3,
+ 6,
+ 3,
+ 6,
+ 6,
+ 9,
+ 3,
+ 6,
+ 6,
+ 9,
+ 6,
+ 9,
+ 9,
+ 6,
+ 3,
+ 6,
+ 6,
+ 9,
+ 6,
+ 9,
+ 9,
+ 12,
+ 6,
+ 9,
+ 9,
+ 12,
+ 9,
+ 12,
+ 12,
+ 9,
+ 3,
+ 6,
+ 6,
+ 9,
+ 6,
+ 9,
+ 9,
+ 12,
+ 6,
+ 9,
+ 9,
+ 12,
+ 9,
+ 12,
+ 12,
+ 9,
+ 6,
+ 9,
+ 9,
+ 6,
+ 9,
+ 12,
+ 12,
+ 9,
+ 9,
+ 12,
+ 12,
+ 9,
+ 12,
+ 15,
+ 15,
+ 6,
+ 3,
+ 6,
+ 6,
+ 9,
+ 6,
+ 9,
+ 9,
+ 12,
+ 6,
+ 9,
+ 9,
+ 12,
+ 9,
+ 12,
+ 12,
+ 9,
+ 6,
+ 9,
+ 9,
+ 12,
+ 9,
+ 12,
+ 12,
+ 15,
+ 9,
+ 12,
+ 12,
+ 15,
+ 12,
+ 15,
+ 15,
+ 12,
+ 6,
+ 9,
+ 9,
+ 12,
+ 9,
+ 12,
+ 6,
+ 9,
+ 9,
+ 12,
+ 12,
+ 15,
+ 12,
+ 15,
+ 9,
+ 6,
+ 9,
+ 12,
+ 12,
+ 9,
+ 12,
+ 15,
+ 9,
+ 6,
+ 12,
+ 15,
+ 15,
+ 12,
+ 15,
+ 6,
+ 12,
+ 3,
+ 3,
+ 6,
+ 6,
+ 9,
+ 6,
+ 9,
+ 9,
+ 12,
+ 6,
+ 9,
+ 9,
+ 12,
+ 9,
+ 12,
+ 12,
+ 9,
+ 6,
+ 9,
+ 9,
+ 12,
+ 9,
+ 12,
+ 12,
+ 15,
+ 9,
+ 6,
+ 12,
+ 9,
+ 12,
+ 9,
+ 15,
+ 6,
+ 6,
+ 9,
+ 9,
+ 12,
+ 9,
+ 12,
+ 12,
+ 15,
+ 9,
+ 12,
+ 12,
+ 15,
+ 12,
+ 15,
+ 15,
+ 12,
+ 9,
+ 12,
+ 12,
+ 9,
+ 12,
+ 15,
+ 15,
+ 12,
+ 12,
+ 9,
+ 15,
+ 6,
+ 15,
+ 12,
+ 6,
+ 3,
+ 6,
+ 9,
+ 9,
+ 12,
+ 9,
+ 12,
+ 12,
+ 15,
+ 9,
+ 12,
+ 12,
+ 15,
+ 6,
+ 9,
+ 9,
+ 6,
+ 9,
+ 12,
+ 12,
+ 15,
+ 12,
+ 15,
+ 15,
+ 6,
+ 12,
+ 9,
+ 15,
+ 12,
+ 9,
+ 6,
+ 12,
+ 3,
+ 9,
+ 12,
+ 12,
+ 15,
+ 12,
+ 15,
+ 9,
+ 12,
+ 12,
+ 15,
+ 15,
+ 6,
+ 9,
+ 12,
+ 6,
+ 3,
+ 6,
+ 9,
+ 9,
+ 6,
+ 9,
+ 12,
+ 6,
+ 3,
+ 9,
+ 6,
+ 12,
+ 3,
+ 6,
+ 3,
+ 3,
+ 0,
+};
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/gpu/kinfu/raycaster.h>
+#include <pcl/gpu/kinfu/tsdf_volume.h>
+#include "internal.h"
+
+using namespace pcl;
+using namespace pcl::gpu;
+using namespace pcl::device;
+using namespace Eigen;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+pcl::gpu::RayCaster::RayCaster(int rows_arg, int cols_arg, float fx, float fy, float cx, float cy)
+ : cols(cols_arg), rows(rows_arg), fx_(fx), fy_(fy), cx_(cx < 0 ? cols/2 : cx), cy_(cy < 0 ? rows/2 : cy)
+{
+ vertex_map_.create(rows * 3, cols);
+ normal_map_.create(rows * 3, cols);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+pcl::gpu::RayCaster::~RayCaster()
+{
+
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::RayCaster::setIntrinsics(float fx, float fy, float cx, float cy)
+{
+ fx_ = fx;
+ fy_ = fy;
+ cx_ = cx < 0 ? cols/2 : cx;
+ cy_ = cy < 0 ? rows/2 : cy;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::RayCaster::run(const TsdfVolume& volume, const Affine3f& camera_pose)
+{
+ camera_pose_.linear() = camera_pose.linear();
+ camera_pose_.translation() = camera_pose.translation();
+ volume_size_ = volume.getSize();
+ device::Intr intr (fx_, fy_, cx_, cy_);
+
+ vertex_map_.create(rows * 3, cols);
+ normal_map_.create(rows * 3, cols);
+
+ typedef Matrix<float, 3, 3, RowMajor> Matrix3f;
+
+ Matrix3f R = camera_pose_.linear();
+ Vector3f t = camera_pose_.translation();
+
+ const Mat33& device_R = device_cast<const Mat33>(R);
+ const float3& device_t = device_cast<const float3>(t);
+
+ float tranc_dist = volume.getTsdfTruncDist();
+ device::raycast (intr, device_R, device_t, tranc_dist, device_cast<const float3>(volume_size_), volume.data(), vertex_map_, normal_map_);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::RayCaster::generateSceneView(View& view) const
+{
+ generateSceneView(view, volume_size_ * (-3.f));
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::RayCaster::generateSceneView(View& view, const Vector3f& light_source_pose) const
+{
+ device::LightSource light;
+ light.number = 1;
+ light.pos[0] = device_cast<const float3>(light_source_pose);
+
+ view.create(rows, cols);
+ device::generateImage (vertex_map_, normal_map_, light, view);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::RayCaster::generateDepthImage(Depth& depth) const
+{
+ device::Intr intr (fx_, fy_, cx_, cy_);
+
+ depth.create(rows, cols);
+
+ Matrix<float, 3, 3, RowMajor> R_inv = camera_pose_.linear().inverse();
+ Vector3f t = camera_pose_.translation();
+
+ device::generateDepth(device_cast<Mat33>(R_inv), device_cast<const float3>(t), vertex_map_, depth);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::gpu::RayCaster::MapArr
+pcl::gpu::RayCaster::getVertexMap() const
+{
+ return vertex_map_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::gpu::RayCaster::MapArr
+pcl::gpu::RayCaster::getNormalMap() const
+{
+ return normal_map_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+ namespace gpu
+ {
+ template<> PCL_EXPORTS void
+ convertMapToOranizedCloud<pcl::PointXYZ>(const RayCaster::MapArr& map, DeviceArray2D<pcl::PointXYZ>& cloud)
+ {
+ cloud.create (map.rows()/3, map.cols());
+ DeviceArray2D<float4>& c = (DeviceArray2D<float4>&)cloud;
+ device::convert (map, c);
+ }
+
+ template<> PCL_EXPORTS void
+ convertMapToOranizedCloud<pcl::Normal> (const RayCaster::MapArr& map, DeviceArray2D<pcl::Normal>& cloud)
+ {
+ cloud.create (map.rows()/3, map.cols());
+ DeviceArray2D<float8>& n = (DeviceArray2D<float8>&)cloud;
+ device::convert(map, n);
+ }
+ }
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#ifndef __PCL_CUDA_SAFE_CALL_HPP__
+#define __PCL_CUDA_SAFE_CALL_HPP__
+
+#include "cuda_runtime_api.h"
+#include <pcl/gpu/containers/initialization.h>
+
+#if defined(__GNUC__)
+ #define cudaSafeCall(expr) pcl::gpu::___cudaSafeCall(expr, __FILE__, __LINE__, __func__)
+#else /* defined(__CUDACC__) || defined(__MSVC__) */
+ #define cudaSafeCall(expr) pcl::gpu::___cudaSafeCall(expr, __FILE__, __LINE__)
+#endif
+
+namespace pcl
+{
+ namespace gpu
+ {
+ static inline void ___cudaSafeCall(cudaError_t err, const char *file, const int line, const char *func = "")
+ {
+ if (cudaSuccess != err)
+ error(cudaGetErrorString(err), file, line, func);
+ }
+
+ static inline int divUp(int total, int grain) { return (total + grain - 1) / grain; }
+ }
+
+ namespace device
+ {
+ using pcl::gpu::divUp;
+ }
+}
+
+
+#endif /* __PCL_CUDA_SAFE_CALL_HPP__ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/gpu/kinfu/tsdf_volume.h>
+#include "internal.h"
+#include <algorithm>
+#include <Eigen/Core>
+
+using namespace pcl;
+using namespace pcl::gpu;
+using namespace Eigen;
+using pcl::device::device_cast;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+pcl::gpu::TsdfVolume::TsdfVolume(const Vector3i& resolution) : resolution_(resolution)
+{
+ int volume_x = resolution_(0);
+ int volume_y = resolution_(1);
+ int volume_z = resolution_(2);
+
+ volume_.create (volume_y * volume_z, volume_x);
+
+ const Vector3f default_volume_size = Vector3f::Constant (3.f); //meters
+ const float default_tranc_dist = 0.03f; //meters
+
+ setSize(default_volume_size);
+ setTsdfTruncDist(default_tranc_dist);
+
+ reset();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::TsdfVolume::setSize(const Vector3f& size)
+{
+ size_ = size;
+ setTsdfTruncDist(tranc_dist_);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::TsdfVolume::setTsdfTruncDist (float distance)
+{
+ float cx = size_(0) / resolution_(0);
+ float cy = size_(1) / resolution_(1);
+ float cz = size_(2) / resolution_(2);
+
+ tranc_dist_ = std::max (distance, 2.1f * std::max (cx, std::max (cy, cz)));
+
+ /*if (tranc_dist_ != distance)
+ PCL_WARN ("Tsdf truncation distance can't be less than 2 * voxel_size. Passed value '%f', but setting minimal possible '%f'.\n", distance, tranc_dist_);*/
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+pcl::gpu::DeviceArray2D<int>
+pcl::gpu::TsdfVolume::data() const
+{
+ return volume_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+const Eigen::Vector3f&
+pcl::gpu::TsdfVolume::getSize() const
+{
+ return size_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+const Eigen::Vector3i&
+pcl::gpu::TsdfVolume::getResolution() const
+{
+ return resolution_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+const Eigen::Vector3f
+pcl::gpu::TsdfVolume::getVoxelSize() const
+{
+ return size_.array () / resolution_.array().cast<float>();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+float
+pcl::gpu::TsdfVolume::getTsdfTruncDist () const
+{
+ return tranc_dist_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::TsdfVolume::reset()
+{
+ device::initVolume(volume_);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::TsdfVolume::fetchCloudHost (PointCloud<PointType>& cloud, bool connected26) const
+{
+ int volume_x = resolution_(0);
+ int volume_y = resolution_(1);
+ int volume_z = resolution_(2);
+
+ int cols;
+ std::vector<int> volume_host;
+ volume_.download (volume_host, cols);
+
+ cloud.points.clear ();
+ cloud.points.reserve (10000);
+
+ const int DIVISOR = device::DIVISOR; // SHRT_MAX;
+
+#define FETCH(x, y, z) volume_host[(x) + (y) * volume_x + (z) * volume_y * volume_x]
+
+ Array3f cell_size = getVoxelSize();
+
+ for (int x = 1; x < volume_x-1; ++x)
+ {
+ for (int y = 1; y < volume_y-1; ++y)
+ {
+ for (int z = 0; z < volume_z-1; ++z)
+ {
+ int tmp = FETCH (x, y, z);
+ int W = reinterpret_cast<short2*>(&tmp)->y;
+ int F = reinterpret_cast<short2*>(&tmp)->x;
+
+ if (W == 0 || F == DIVISOR)
+ continue;
+
+ Vector3f V = ((Array3i(x, y, z).cast<float>() + 0.5f) * cell_size).matrix ();
+
+ if (connected26)
+ {
+ int dz = 1;
+ for (int dy = -1; dy < 2; ++dy)
+ for (int dx = -1; dx < 2; ++dx)
+ {
+ int tmp = FETCH (x+dx, y+dy, z+dz);
+
+ int Wn = reinterpret_cast<short2*>(&tmp)->y;
+ int Fn = reinterpret_cast<short2*>(&tmp)->x;
+ if (Wn == 0 || Fn == DIVISOR)
+ continue;
+
+ if ((F > 0 && Fn < 0) || (F < 0 && Fn > 0))
+ {
+ Vector3f Vn = ((Array3i (x+dx, y+dy, z+dz).cast<float>() + 0.5f) * cell_size).matrix ();
+ Vector3f point = (V * (float)abs (Fn) + Vn * (float)abs (F)) / (float)(abs (F) + abs (Fn));
+
+ pcl::PointXYZ xyz;
+ xyz.x = point (0);
+ xyz.y = point (1);
+ xyz.z = point (2);
+
+ cloud.points.push_back (xyz);
+ }
+ }
+ dz = 0;
+ for (int dy = 0; dy < 2; ++dy)
+ for (int dx = -1; dx < dy * 2; ++dx)
+ {
+ int tmp = FETCH (x+dx, y+dy, z+dz);
+
+ int Wn = reinterpret_cast<short2*>(&tmp)->y;
+ int Fn = reinterpret_cast<short2*>(&tmp)->x;
+ if (Wn == 0 || Fn == DIVISOR)
+ continue;
+
+ if ((F > 0 && Fn < 0) || (F < 0 && Fn > 0))
+ {
+ Vector3f Vn = ((Array3i (x+dx, y+dy, z+dz).cast<float>() + 0.5f) * cell_size).matrix ();
+ Vector3f point = (V * (float)abs(Fn) + Vn * (float)abs(F))/(float)(abs(F) + abs (Fn));
+
+ pcl::PointXYZ xyz;
+ xyz.x = point (0);
+ xyz.y = point (1);
+ xyz.z = point (2);
+
+ cloud.points.push_back (xyz);
+ }
+ }
+ }
+ else /* if (connected26) */
+ {
+ for (int i = 0; i < 3; ++i)
+ {
+ int ds[] = {0, 0, 0};
+ ds[i] = 1;
+
+ int dx = ds[0];
+ int dy = ds[1];
+ int dz = ds[2];
+
+ int tmp = FETCH (x+dx, y+dy, z+dz);
+
+ int Wn = reinterpret_cast<short2*>(&tmp)->y;
+ int Fn = reinterpret_cast<short2*>(&tmp)->x;
+ if (Wn == 0 || Fn == DIVISOR)
+ continue;
+
+ if ((F > 0 && Fn < 0) || (F < 0 && Fn > 0))
+ {
+ Vector3f Vn = ((Array3i (x+dx, y+dy, z+dz).cast<float>() + 0.5f) * cell_size).matrix ();
+ Vector3f point = (V * (float)abs (Fn) + Vn * (float)abs (F)) / (float)(abs (F) + abs (Fn));
+
+ pcl::PointXYZ xyz;
+ xyz.x = point (0);
+ xyz.y = point (1);
+ xyz.z = point (2);
+
+ cloud.points.push_back (xyz);
+ }
+ }
+ } /* if (connected26) */
+ }
+ }
+ }
+#undef FETCH
+ cloud.width = (int)cloud.points.size ();
+ cloud.height = 1;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::gpu::DeviceArray<pcl::gpu::TsdfVolume::PointType>
+pcl::gpu::TsdfVolume::fetchCloud (DeviceArray<PointType>& cloud_buffer) const
+{
+ if (cloud_buffer.empty ())
+ cloud_buffer.create (DEFAULT_CLOUD_BUFFER_SIZE);
+
+ float3 device_volume_size = device_cast<const float3> (size_);
+ size_t size = device::extractCloud (volume_, device_volume_size, cloud_buffer);
+ return (DeviceArray<PointType> (cloud_buffer.ptr (), size));
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::TsdfVolume::fetchNormals (const DeviceArray<PointType>& cloud, DeviceArray<PointType>& normals) const
+{
+ normals.create (cloud.size ());
+ const float3 device_volume_size = device_cast<const float3> (size_);
+ device::extractNormals (volume_, device_volume_size, cloud, (device::PointType*)normals.ptr ());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::TsdfVolume::fetchNormals (const DeviceArray<PointType>& cloud, DeviceArray<NormalType>& normals) const
+{
+ normals.create (cloud.size ());
+ const float3 device_volume_size = device_cast<const float3> (size_);
+ device::extractNormals (volume_, device_volume_size, cloud, (device::float8*)normals.ptr ());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::TsdfVolume::downloadTsdf (std::vector<float>& tsdf) const
+{
+ tsdf.resize (volume_.cols() * volume_.rows());
+ volume_.download(&tsdf[0], volume_.cols() * sizeof(int));
+
+#pragma omp parallel for
+ for(int i = 0; i < (int) tsdf.size(); ++i)
+ {
+ float tmp = reinterpret_cast<short2*>(&tsdf[i])->x;
+ tsdf[i] = tmp/device::DIVISOR;
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::TsdfVolume::downloadTsdfAndWeighs (std::vector<float>& tsdf, std::vector<short>& weights) const
+{
+ int volumeSize = volume_.cols() * volume_.rows();
+ tsdf.resize (volumeSize);
+ weights.resize (volumeSize);
+ volume_.download(&tsdf[0], volume_.cols() * sizeof(int));
+
+#pragma omp parallel for
+ for(int i = 0; i < (int) tsdf.size(); ++i)
+ {
+ short2 elem = *reinterpret_cast<short2*>(&tsdf[i]);
+ tsdf[i] = (float)(elem.x)/device::DIVISOR;
+ weights[i] = (short)(elem.y);
+ }
+}
\ No newline at end of file
--- /dev/null
+if(WITH_OPENNI)
+ if(NOT VTK_FOUND)
+ set(DEFAULT FALSE)
+ set(REASON "VTK was not found.")
+ else(NOT VTK_FOUND)
+ set(DEFAULT TRUE)
+ set(REASON)
+ set(VTK_USE_FILE "${VTK_USE_FILE}" CACHE INTERNAL "VTK_USE_FILE")
+ include("${VTK_USE_FILE}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
+ endif(NOT VTK_FOUND)
+
+ FILE(GLOB hdrs "*.h*")
+ include_directories(${VTK_INCLUDE_DIRS} ${OPENNI_INCLUDE_DIRS})
+
+ ## KINECT FUSION
+ set(the_target pcl_kinfu_app)
+ set(srcs kinfu_app.cpp capture.cpp evaluation.cpp)
+
+ source_group("Source Files" FILES ${srcs} )
+
+ PCL_ADD_EXECUTABLE_OPT_BUNDLE("${the_target}" "${SUBSYS_NAME}" ${srcs} ${hdrs})
+ target_link_libraries("${the_target}" pcl_common pcl_io ${OPENNI_LIBRARIES} pcl_visualization pcl_gpu_kinfu)
+
+ if(OpenCV_FOUND)
+ target_link_libraries("${the_target}" ${OpenCV_LIBS})
+ endif()
+
+ ## RECORD TSDFVOLUME
+ set(the_target pcl_record_tsdfvolume)
+ set(srcs record_tsdfvolume.cpp capture.cpp)
+
+ PCL_ADD_EXECUTABLE("${the_target}" "${SUBSYS_NAME}" ${srcs} ${hdrs})
+ target_link_libraries("${the_target}" pcl_common pcl_io ${OPENNI_LIBRARIES} pcl_visualization pcl_gpu_kinfu)
+
+ ## KINECT FUSION SIMULATION - DISABLED FOR NOW
+ #find_package(GLEW)
+ #if(GLEW_FOUND)
+ # find_package(GLUT)
+ # if(GLUT_FOUND)
+ # include_directories("${GLUT_INCLUDE_DIR}" "${GLEW_INCLUDE_DIR}")
+ # ## KINECT FUSION SIMULATION
+ # set(the_target kinfu_app_sim)
+ # set(srcs kinfu_app_sim.cpp capture.cpp evaluation.cpp)
+ #
+ # source_group("Source Files" FILES ${srcs} )
+ # find_package( OpenCV REQUIRED )
+ # PCL_ADD_EXECUTABLE("${the_target}" "${SUBSYS_NAME}" ${srcs} ${hdrs})
+ # target_link_libraries("${the_target}" pcl_common pcl_io
+ # pcl_visualization pcl_kdtree pcl_simulation ${OpenCV_LIBS}
+ # ${OPENNI_LIBRARIES} pcl_gpu_kinfu
+ # ${GLEW_LIBRARIES} ${GLUT_LIBRARIES} ${OPENGL_LIBRARIES} ${GLEW_LIBRARIES})
+ #
+ # if(OpenCV_FOUND)
+ # target_link_libraries("${the_target}" ${OpenCV_LIBS})
+ # endif()
+ # endif(GLUT_FOUND)
+ #endif (GLEW_FOUND)
+
+endif()
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Marco Paladini <marco.paladini@ocado.com>
+ * Date: March 2014
+ */
+
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <fstream>
+
+/**
+ * @brief The CameraPoseProcessor class is an interface to extract
+ * camera pose data generated by the pcl_kinfu_app program.
+ * Use the CameraPoseWriter implementation if writing the camera
+ * pose to a file is all you need, or provide an alternative implementation.
+ */
+class CameraPoseProcessor
+{
+ public:
+ virtual ~CameraPoseProcessor () {}
+
+ /// process the camera pose, this method is called at every frame.
+ virtual void
+ processPose (const Eigen::Affine3f &pose)=0;
+};
+
+/**
+ * @brief CameraPoseWriter writes all camera poses computed by
+ * the KinfuTracker to a file on disk.
+ *
+ */
+class CameraPoseWriter : public CameraPoseProcessor
+{
+ std::string output_filename_;
+ std::ofstream out_stream_;
+ public:
+ /**
+ * @param output_filename name of file to write
+ */
+ CameraPoseWriter (std::string output_filename) :
+ output_filename_ (output_filename)
+ {
+ out_stream_.open (output_filename_.c_str () );
+ }
+
+ ~CameraPoseWriter ()
+ {
+ if (out_stream_.is_open ())
+ {
+ out_stream_.close ();
+ std::cout << "wrote camera poses to file " << output_filename_ << std::endl;
+ }
+ }
+
+ void
+ processPose (const Eigen::Affine3f &pose)
+ {
+ if (out_stream_.good ())
+ {
+ // convert 3x4 affine transformation to quaternion and write to file
+ Eigen::Quaternionf q (pose.rotation ());
+ Eigen::Vector3f t (pose.translation ());
+ // write translation , quaternion in a row
+ out_stream_ << t[0] << "," << t[1] << "," << t[2]
+ << "," << q.w () << "," << q.x ()
+ << "," << q.y ()<< "," << q.z () << std::endl;
+ }
+ }
+
+};
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include <pcl/io/openni_camera/openni.h>
+
+#include "openni_capture.h"
+#include <pcl/gpu/containers/initialization.h>
+
+using namespace std;
+using namespace pcl;
+using namespace pcl::gpu;
+using namespace xn;
+
+//const std::string XMLConfig =
+//"<OpenNI>"
+// "<Licenses>"
+// "<License vendor=\"PrimeSense\" key=\"0KOIk2JeIBYClPWVnMoRKn5cdY4=\"/>"
+// "</Licenses>"
+// "<Log writeToConsole=\"false\" writeToFile=\"false\">"
+// "<LogLevel value=\"3\"/>"
+// "<Masks>"
+// "<Mask name=\"ALL\" on=\"true\"/>"
+// "</Masks>"
+// "<Dumps>"
+// "</Dumps>"
+// "</Log>"
+// "<ProductionNodes>"
+// "<Node type=\"Image\" name=\"Image1\">"
+// "<Configuration>"
+// "<MapOutputMode xRes=\"640\" yRes=\"480\" FPS=\"30\"/>"
+// "<Mirror on=\"false\"/>"
+// "</Configuration>"
+// "</Node> "
+// "<Node type=\"Depth\" name=\"Depth1\">"
+// "<Configuration>"
+// "<MapOutputMode xRes=\"640\" yRes=\"480\" FPS=\"30\"/>"
+// "<Mirror on=\"false\"/>"
+// "</Configuration>"
+// "</Node>"
+// "</ProductionNodes>"
+//"</OpenNI>";
+
+#define REPORT_ERROR(msg) pcl::gpu::error ((msg), __FILE__, __LINE__)
+
+struct pcl::gpu::CaptureOpenNI::Impl
+{
+ Context context;
+ ScriptNode scriptNode;
+ DepthGenerator depth;
+ ImageGenerator image;
+ ProductionNode node;
+ DepthMetaData depthMD;
+ ImageMetaData imageMD;
+ XnChar strError[1024];
+
+ bool has_depth;
+ bool has_image;
+};
+
+pcl::gpu::CaptureOpenNI::CaptureOpenNI() : depth_focal_length_VGA (0.f), baseline (0.f), shadow_value (0), no_sample_value (0), pixelSize (0.0), max_depth (0) {}
+pcl::gpu::CaptureOpenNI::CaptureOpenNI(int device) {open (device); }
+pcl::gpu::CaptureOpenNI::CaptureOpenNI(const string& filename) {open (filename); }
+pcl::gpu::CaptureOpenNI::~CaptureOpenNI() { release (); }
+
+void
+pcl::gpu::CaptureOpenNI::open (int device)
+{
+ impl_.reset ( new Impl () );
+
+ XnMapOutputMode mode;
+ mode.nXRes = XN_VGA_X_RES;
+ mode.nYRes = XN_VGA_Y_RES;
+ mode.nFPS = 30;
+
+ XnStatus rc;
+ rc = impl_->context.Init ();
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "Init failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+
+ xn::NodeInfoList devicesList;
+ rc = impl_->context.EnumerateProductionTrees ( XN_NODE_TYPE_DEVICE, NULL, devicesList, 0 );
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "Init failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+
+ xn::NodeInfoList::Iterator it = devicesList.Begin ();
+ for (int i = 0; i < device; ++i)
+ it++;
+
+ NodeInfo node = *it;
+ rc = impl_->context.CreateProductionTree ( node, impl_->node );
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "Init failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+
+ XnLicense license;
+ const char* vendor = "PrimeSense";
+ const char* key = "0KOIk2JeIBYClPWVnMoRKn5cdY4=";
+ sprintf (license.strKey, key);
+ sprintf (license.strVendor, vendor);
+
+ rc = impl_->context.AddLicense (license);
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "licence failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+
+ rc = impl_->depth.Create (impl_->context);
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "Depth generator failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+ //rc = impl_->depth.SetIntProperty("HoleFilter", 1);
+ rc = impl_->depth.SetMapOutputMode (mode);
+ impl_->has_depth = true;
+
+ rc = impl_->image.Create (impl_->context);
+ if (rc != XN_STATUS_OK)
+ {
+ printf ("Image generator creation failed: %s\n", xnGetStatusString (rc));
+ impl_->has_image = false;
+ }
+ else
+ {
+ impl_->has_image = true;
+ rc = impl_->image.SetMapOutputMode (mode);
+ }
+
+ getParams ();
+
+ rc = impl_->context.StartGeneratingAll ();
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "Start failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+}
+
+void
+pcl::gpu::CaptureOpenNI::open (const std::string& filename)
+{
+ impl_.reset ( new Impl () );
+
+ XnStatus rc;
+
+ rc = impl_->context.Init ();
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "Init failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+
+ rc = impl_->context.OpenFileRecording (filename.c_str (), impl_->node);
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "Open failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+
+ rc = impl_->context.FindExistingNode (XN_NODE_TYPE_DEPTH, impl_->depth);
+ impl_->has_depth = (rc == XN_STATUS_OK);
+
+ rc = impl_->context.FindExistingNode (XN_NODE_TYPE_IMAGE, impl_->image);
+ impl_->has_image = (rc == XN_STATUS_OK);
+
+ if (!impl_->has_depth)
+ REPORT_ERROR ("No depth nodes. Check your configuration");
+
+ if (impl_->has_depth)
+ impl_->depth.GetMetaData (impl_->depthMD);
+
+ if (impl_->has_image)
+ impl_->image.GetMetaData (impl_->imageMD);
+
+ // RGB is the only image format supported.
+ if (impl_->imageMD.PixelFormat () != XN_PIXEL_FORMAT_RGB24)
+ REPORT_ERROR ("Image format must be RGB24\n");
+
+ getParams ();
+}
+
+void
+pcl::gpu::CaptureOpenNI::release ()
+{
+ if (impl_)
+ {
+ impl_->context.StopGeneratingAll ();
+ impl_->context.Release ();
+ }
+
+ impl_.reset ();
+ depth_focal_length_VGA = 0;
+ baseline = 0.f;
+ shadow_value = 0;
+ no_sample_value = 0;
+ pixelSize = 0.0;
+}
+
+bool
+pcl::gpu::CaptureOpenNI::grab (PtrStepSz<const unsigned short>& depth, PtrStepSz<const RGB>& rgb24)
+{
+ XnStatus rc = XN_STATUS_OK;
+
+ rc = impl_->context.WaitAndUpdateAll ();
+ if (rc != XN_STATUS_OK)
+ return printf ("Read failed: %s\n", xnGetStatusString (rc)), false;
+
+ if (impl_->has_depth)
+ {
+ impl_->depth.GetMetaData (impl_->depthMD);
+ const XnDepthPixel* pDepth = impl_->depthMD.Data ();
+ int x = impl_->depthMD.FullXRes ();
+ int y = impl_->depthMD.FullYRes ();
+ depth.cols = x;
+ depth.rows = y;
+ depth.data = pDepth;
+ depth.step = x * depth.elemSize ();
+ }
+ else
+ printf ("no depth\n");
+
+ if (impl_->has_image)
+ {
+ impl_->image.GetMetaData (impl_->imageMD);
+ const XnRGB24Pixel* pImage = impl_->imageMD.RGB24Data ();
+ int x = impl_->imageMD.FullXRes ();
+ int y = impl_->imageMD.FullYRes ();
+
+ rgb24.data = (const RGB*)pImage;
+ rgb24.cols = x;
+ rgb24.rows = y;
+ rgb24.step = x * rgb24.elemSize ();
+ }
+ else
+ {
+ printf ("no image\n");
+ rgb24.data = 0;
+ rgb24.cols = rgb24.rows = rgb24.step = 0;
+ }
+
+ return impl_->has_image || impl_->has_depth;
+}
+
+void
+pcl::gpu::CaptureOpenNI::getParams ()
+{
+ XnStatus rc = XN_STATUS_OK;
+
+ max_depth = impl_->depth.GetDeviceMaxDepth ();
+
+ rc = impl_->depth.GetRealProperty ( "ZPPS", pixelSize ); // in mm
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "ZPPS failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+
+ XnUInt64 depth_focal_length_SXGA_mm; //in mm
+ rc = impl_->depth.GetIntProperty ("ZPD", depth_focal_length_SXGA_mm);
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "ZPD failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+
+ XnDouble baseline_local;
+ rc = impl_->depth.GetRealProperty ("LDDIS", baseline_local);
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "ZPD failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+
+ XnUInt64 shadow_value_local;
+ rc = impl_->depth.GetIntProperty ("ShadowValue", shadow_value_local);
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "ShadowValue failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+ shadow_value = (int)shadow_value_local;
+
+ XnUInt64 no_sample_value_local;
+ rc = impl_->depth.GetIntProperty ("NoSampleValue", no_sample_value_local);
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "NoSampleValue failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+ no_sample_value = (int)no_sample_value_local;
+
+
+ // baseline from cm -> mm
+ baseline = (float)(baseline_local * 10);
+
+ //focal length from mm -> pixels (valid for 1280x1024)
+ float depth_focal_length_SXGA = static_cast<float>(depth_focal_length_SXGA_mm / pixelSize);
+ depth_focal_length_VGA = depth_focal_length_SXGA / 2;
+}
+
+bool
+pcl::gpu::CaptureOpenNI::setRegistration (bool value)
+{
+ XnStatus rc = XN_STATUS_OK;
+
+ if (value)
+ {
+ if (!impl_->has_image)
+ return false;
+
+ if (impl_->depth.GetAlternativeViewPointCap ().IsViewPointAs (impl_->image) )
+ return true;
+
+ if (!impl_->depth.GetAlternativeViewPointCap ().IsViewPointSupported (impl_->image) )
+ {
+ printf ("SetRegistration failed: Unsupported viewpoint.\n");
+ return false;
+ }
+
+ rc = impl_->depth.GetAlternativeViewPointCap ().SetViewPoint (impl_->image);
+ if (rc != XN_STATUS_OK)
+ printf ("SetRegistration failed: %s\n", xnGetStatusString (rc));
+
+ }
+ else // "off"
+ {
+ rc = impl_->depth.GetAlternativeViewPointCap ().ResetViewPoint ();
+ if (rc != XN_STATUS_OK)
+ printf ("SetRegistration failed: %s\n", xnGetStatusString (rc));
+ }
+
+ getParams ();
+ return rc == XN_STATUS_OK;
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include "evaluation.h"
+
+#include<iostream>
+
+using namespace pcl::gpu;
+using namespace std;
+
+const float Evaluation::fx = 525.0f;
+const float Evaluation::fy = 525.0f;
+const float Evaluation::cx = 319.5f;
+const float Evaluation::cy = 239.5f;
+
+#ifndef HAVE_OPENCV
+
+struct Evaluation::Impl {};
+
+Evaluation::Evaluation(const std::string&) { cout << "Evaluation requires OpenCV. Please enable it in cmake-file" << endl; exit(0); }
+void Evaluation::setMatchFile(const std::string&) { }
+bool Evaluation::grab (double stamp, pcl::gpu::PtrStepSz<const RGB>& rgb24) { return false; }
+bool Evaluation::grab (double stamp, pcl::gpu::PtrStepSz<const unsigned short>& depth) { return false; }
+bool Evaluation::grab (double stamp, pcl::gpu::PtrStepSz<const unsigned short>& depth, pcl::gpu::PtrStepSz<const RGB>& rgb24) { return false; }
+void Evaluation::saveAllPoses(const pcl::gpu::KinfuTracker& kinfu, int frame_number, const std::string& logfile) const {}
+
+#else
+
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include<fstream>
+
+using namespace cv;
+
+struct Evaluation::Impl
+{
+ Mat depth_buffer;
+ Mat rgb_buffer;
+};
+
+
+
+Evaluation::Evaluation(const std::string& folder) : folder_(folder), visualization_(false)
+{
+ impl_.reset( new Impl() );
+
+ if (folder_[folder_.size() - 1] != '\\' && folder_[folder_.size() - 1] != '/')
+ folder_.push_back('/');
+
+ cout << "Initializing evaluation from folder: " << folder_ << endl;
+ string depth_file = folder_ + "depth.txt";
+ string rgb_file = folder_ + "rgb.txt";
+
+ readFile(depth_file, depth_stamps_and_filenames_);
+ readFile(rgb_file, rgb_stamps_and_filenames_);
+
+ string associated_file = folder_ + "associated.txt";
+}
+
+void Evaluation::setMatchFile(const std::string& file)
+{
+ string full = folder_ + file;
+ ifstream iff(full.c_str());
+ if(!iff)
+ {
+ cout << "Can't read " << file << endl;
+ exit(1);
+ }
+
+ accociations_.clear();
+ while (!iff.eof())
+ {
+ Association acc;
+ iff >> acc.time1 >> acc.name1 >> acc.time2 >> acc.name2;
+ accociations_.push_back(acc);
+ }
+}
+
+void Evaluation::readFile(const string& file, vector< pair<double,string> >& output)
+{
+ char buffer[4096];
+ vector< pair<double,string> > tmp;
+
+ ifstream iff(file.c_str());
+ if(!iff)
+ {
+ cout << "Can't read " << file << endl;
+ exit(1);
+ }
+
+ // ignore three header lines
+ iff.getline(buffer, sizeof(buffer));
+ iff.getline(buffer, sizeof(buffer));
+ iff.getline(buffer, sizeof(buffer));
+
+ // each line consists of the timestamp and the filename of the depth image
+ while (!iff.eof())
+ {
+ double time; string name;
+ iff >> time >> name;
+ tmp.push_back(make_pair(time, name));
+ }
+ tmp.swap(output);
+}
+
+bool Evaluation::grab (double stamp, PtrStepSz<const RGB>& rgb24)
+{
+ size_t i = static_cast<size_t>(stamp); // temporary solution, now it expects only index
+ size_t total = accociations_.empty() ? rgb_stamps_and_filenames_.size() : accociations_.size();
+
+ if ( i>= total)
+ return false;
+
+ string file = folder_ + (accociations_.empty() ? rgb_stamps_and_filenames_[i].second : accociations_[i].name2);
+
+ cv::Mat bgr = cv::imread(file);
+ if(bgr.empty())
+ return false;
+
+ cv::cvtColor(bgr, impl_->rgb_buffer, CV_BGR2RGB);
+
+ rgb24.data = impl_->rgb_buffer.ptr<RGB>();
+ rgb24.cols = impl_->rgb_buffer.cols;
+ rgb24.rows = impl_->rgb_buffer.rows;
+ rgb24.step = impl_->rgb_buffer.cols*sizeof(unsigned char);
+
+ if (visualization_)
+ {
+ cv::imshow("Color channel", bgr);
+ cv::waitKey(3);
+ }
+ return true;
+}
+
+bool Evaluation::grab (double stamp, PtrStepSz<const unsigned short>& depth)
+{
+ size_t i = static_cast<size_t>(stamp); // temporary solution, now it expects only index
+ size_t total = accociations_.empty() ? depth_stamps_and_filenames_.size() : accociations_.size();
+
+ if ( i>= total)
+ return false;
+
+ string file = folder_ + (accociations_.empty() ? depth_stamps_and_filenames_[i].second : accociations_[i].name1);
+
+ cv::Mat d_img = cv::imread(file, CV_LOAD_IMAGE_ANYDEPTH | CV_LOAD_IMAGE_ANYCOLOR);
+ if(d_img.empty())
+ return false;
+
+ if (d_img.elemSize() != sizeof(unsigned short))
+ {
+ cout << "Image was not opend in 16-bit format. Please use OpenCV 2.3.1 or higher" << endl;
+ exit(1);
+ }
+
+ // Datasets are with factor 5000 (pixel to m)
+ // http://cvpr.in.tum.de/data/datasets/rgbd-dataset/file_formats#color_images_and_depth_maps
+
+ d_img.convertTo(impl_->depth_buffer, d_img.type(), 0.2);
+ depth.data = impl_->depth_buffer.ptr<ushort>();
+ depth.cols = impl_->depth_buffer.cols;
+ depth.rows = impl_->depth_buffer.rows;
+ depth.step = impl_->depth_buffer.cols*sizeof(ushort); // 1280 = 640*2
+
+ if (visualization_)
+ {
+ cv::Mat scaled = impl_->depth_buffer/5000.0*65535;
+ cv::imshow("Depth channel", scaled);
+ cv::waitKey(3);
+ }
+ return true;
+}
+
+bool Evaluation::grab (double stamp, PtrStepSz<const unsigned short>& depth, PtrStepSz<const RGB>& rgb24)
+{
+ if (accociations_.empty())
+ {
+ cout << "Please set match file" << endl;
+ exit(0);
+ }
+
+ size_t i = static_cast<size_t>(stamp); // temporary solution, now it expects only index
+
+ if ( i>= accociations_.size())
+ return false;
+
+ string depth_file = folder_ + accociations_[i].name1;
+ string color_file = folder_ + accociations_[i].name2;
+
+ cv::Mat d_img = cv::imread(depth_file, CV_LOAD_IMAGE_ANYDEPTH | CV_LOAD_IMAGE_ANYCOLOR);
+ if(d_img.empty())
+ return false;
+
+ if (d_img.elemSize() != sizeof(unsigned short))
+ {
+ cout << "Image was not opend in 16-bit format. Please use OpenCV 2.3.1 or higher" << endl;
+ exit(1);
+ }
+
+ // Datasets are with factor 5000 (pixel to m)
+ // http://cvpr.in.tum.de/data/datasets/rgbd-dataset/file_formats#color_images_and_depth_maps
+
+ d_img.convertTo(impl_->depth_buffer, d_img.type(), 0.2);
+ depth.data = impl_->depth_buffer.ptr<ushort>();
+ depth.cols = impl_->depth_buffer.cols;
+ depth.rows = impl_->depth_buffer.rows;
+ depth.step = impl_->depth_buffer.cols*depth.elemSize(); // 1280 = 640*2
+
+ cv::Mat bgr = cv::imread(color_file);
+ if(bgr.empty())
+ return false;
+
+ cv::cvtColor(bgr, impl_->rgb_buffer, CV_BGR2RGB);
+
+ rgb24.data = impl_->rgb_buffer.ptr<RGB>();
+ rgb24.cols = impl_->rgb_buffer.cols;
+ rgb24.rows = impl_->rgb_buffer.rows;
+ rgb24.step = impl_->rgb_buffer.cols*sizeof(unsigned char);
+
+ return true;
+}
+
+void Evaluation::saveAllPoses(const pcl::gpu::KinfuTracker& kinfu, int frame_number, const std::string& logfile) const
+{
+ size_t total = accociations_.empty() ? depth_stamps_and_filenames_.size() : accociations_.size();
+
+ if (frame_number < 0)
+ frame_number = (int)total;
+
+ frame_number = std::min(frame_number, (int)kinfu.getNumberOfPoses());
+
+ cout << "Writing " << frame_number << " poses to " << logfile << endl;
+
+ ofstream path_file_stream(logfile.c_str());
+ path_file_stream.setf(ios::fixed,ios::floatfield);
+
+ for(int i = 0; i < frame_number; ++i)
+ {
+ Eigen::Affine3f pose = kinfu.getCameraPose(i);
+ Eigen::Quaternionf q(pose.rotation());
+ Eigen::Vector3f t = pose.translation();
+
+ double stamp = accociations_.empty() ? depth_stamps_and_filenames_[i].first : accociations_[i].time1;
+
+ path_file_stream << stamp << " ";
+ path_file_stream << t[0] << " " << t[1] << " " << t[2] << " ";
+ path_file_stream << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;
+ }
+}
+
+
+#endif /* HAVE_OPENCV */
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#pragma once
+
+#include <string>
+#include <boost/shared_ptr.hpp>
+#include <pcl/gpu/containers/kernel_containers.h>
+#include <pcl/gpu/kinfu/kinfu.h>
+
+
+/** \brief class for RGB-D SLAM Dataset and Benchmark
+ * \author Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+class Evaluation
+{
+public:
+ typedef boost::shared_ptr<Evaluation> Ptr;
+ typedef pcl::gpu::KinfuTracker::PixelRGB RGB;
+
+ Evaluation(const std::string& folder);
+
+ /** \brief Sets file with matches between depth and rgb */
+ void setMatchFile(const std::string& file);
+
+ /** \brief Reads rgb frame from the folder
+ * \param stamp index of frame to read (stamps are not implemented)
+ * \param rgb24
+ */
+ bool grab (double stamp, pcl::gpu::PtrStepSz<const RGB>& rgb24);
+
+ /** \brief Reads depth frame from the folder
+ * \param stamp index of frame to read (stamps are not implemented)
+ * \param depth
+ */
+ bool grab (double stamp, pcl::gpu::PtrStepSz<const unsigned short>& depth);
+
+ /** \brief Reads depth & rgb frame from the folder. Before calling this folder please call 'setMatchFile', or an error will be returned otherwise.
+ * \param stamp index of accociated frame pair (stamps are not implemented)
+ * \param depth
+ * \param rgb24
+ */
+ bool grab (double stamp, pcl::gpu::PtrStepSz<const unsigned short>& depth, pcl::gpu::PtrStepSz<const RGB>& rgb24);
+
+ const static float fx, fy, cx, cy;
+
+
+ void saveAllPoses(const pcl::gpu::KinfuTracker& kinfu, int frame_number = -1, const std::string& logfile = "kinfu_poses.txt") const;
+
+private:
+ std::string folder_;
+ bool visualization_;
+
+ std::vector< std::pair<double, std::string> > rgb_stamps_and_filenames_;
+ std::vector< std::pair<double, std::string> > depth_stamps_and_filenames_;
+
+ struct Association
+ {
+ double time1, time2;
+ std::string name1, name2;
+ };
+
+ std::vector< Association > accociations_;
+
+ void readFile(const std::string& file, std::vector< std::pair<double, std::string> >& output);
+
+ struct Impl;
+ boost::shared_ptr<Impl> impl_;
+};
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+
+#define _CRT_SECURE_NO_DEPRECATE
+
+#include <iostream>
+#include <vector>
+
+#include <pcl/console/parse.h>
+
+#include <boost/filesystem.hpp>
+
+#include <pcl/gpu/kinfu/kinfu.h>
+#include <pcl/gpu/kinfu/raycaster.h>
+#include <pcl/gpu/kinfu/marching_cubes.h>
+#include <pcl/gpu/containers/initialization.h>
+
+#include <pcl/common/time.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/visualization/image_viewer.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/vtk_io.h>
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/oni_grabber.h>
+#include <pcl/io/pcd_grabber.h>
+#include <pcl/exceptions.h>
+
+#include "openni_capture.h"
+#include <pcl/visualization/point_cloud_color_handlers.h>
+#include "evaluation.h"
+
+#include <pcl/common/angles.h>
+
+#include "tsdf_volume.h"
+#include "tsdf_volume.hpp"
+
+#include "camera_pose.h"
+
+#ifdef HAVE_OPENCV
+ #include <opencv2/highgui/highgui.hpp>
+ #include <opencv2/imgproc/imgproc.hpp>
+//#include "video_recorder.h"
+#endif
+typedef pcl::ScopeTime ScopeTimeT;
+
+#include "../src/internal.h"
+
+using namespace std;
+using namespace pcl;
+using namespace pcl::gpu;
+using namespace Eigen;
+namespace pc = pcl::console;
+
+namespace pcl
+{
+ namespace gpu
+ {
+ void paint3DView (const KinfuTracker::View& rgb24, KinfuTracker::View& view, float colors_weight = 0.5f);
+ void mergePointNormal (const DeviceArray<PointXYZ>& cloud, const DeviceArray<Normal>& normals, DeviceArray<PointNormal>& output);
+ }
+
+ namespace visualization
+ {
+ //////////////////////////////////////////////////////////////////////////////////////
+ /** \brief RGB handler class for colors. Uses the data present in the "rgb" or "rgba"
+ * fields from an additional cloud as the color at each point.
+ * \author Anatoly Baksheev
+ * \ingroup visualization
+ */
+ template <typename PointT>
+ class PointCloudColorHandlerRGBCloud : public PointCloudColorHandler<PointT>
+ {
+ using PointCloudColorHandler<PointT>::capable_;
+ using PointCloudColorHandler<PointT>::cloud_;
+
+ typedef typename PointCloudColorHandler<PointT>::PointCloud::ConstPtr PointCloudConstPtr;
+ typedef typename pcl::PointCloud<RGB>::ConstPtr RgbCloudConstPtr;
+
+ public:
+ typedef boost::shared_ptr<PointCloudColorHandlerRGBCloud<PointT> > Ptr;
+ typedef boost::shared_ptr<const PointCloudColorHandlerRGBCloud<PointT> > ConstPtr;
+
+ /** \brief Constructor. */
+ PointCloudColorHandlerRGBCloud (const PointCloudConstPtr& cloud, const RgbCloudConstPtr& colors)
+ : rgb_ (colors)
+ {
+ cloud_ = cloud;
+ capable_ = true;
+ }
+
+ /** \brief Obtain the actual color for the input dataset as vtk scalars.
+ * \param[out] scalars the output scalars containing the color for the dataset
+ * \return true if the operation was successful (the handler is capable and
+ * the input cloud was given as a valid pointer), false otherwise
+ */
+ virtual bool
+ getColor (vtkSmartPointer<vtkDataArray> &scalars) const
+ {
+ if (!capable_ || !cloud_)
+ return (false);
+
+ if (!scalars)
+ scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+ scalars->SetNumberOfComponents (3);
+
+ vtkIdType nr_points = vtkIdType (cloud_->points.size ());
+ reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
+ unsigned char* colors = reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->GetPointer (0);
+
+ // Color every point
+ if (nr_points != int (rgb_->points.size ()))
+ std::fill (colors, colors + nr_points * 3, static_cast<unsigned char> (0xFF));
+ else
+ for (vtkIdType cp = 0; cp < nr_points; ++cp)
+ {
+ int idx = cp * 3;
+ colors[idx + 0] = rgb_->points[cp].r;
+ colors[idx + 1] = rgb_->points[cp].g;
+ colors[idx + 2] = rgb_->points[cp].b;
+ }
+ return (true);
+ }
+
+ private:
+ virtual std::string
+ getFieldName () const { return ("additional rgb"); }
+ virtual std::string
+ getName () const { return ("PointCloudColorHandlerRGBCloud"); }
+
+ RgbCloudConstPtr rgb_;
+ };
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+vector<string> getPcdFilesInDir(const string& directory)
+{
+ namespace fs = boost::filesystem;
+ fs::path dir(directory);
+
+ std::cout << "path: " << directory << std::endl;
+ if (directory.empty() || !fs::exists(dir) || !fs::is_directory(dir))
+ PCL_THROW_EXCEPTION (pcl::IOException, "No valid PCD directory given!\n");
+
+ vector<string> result;
+ fs::directory_iterator pos(dir);
+ fs::directory_iterator end;
+
+ for(; pos != end ; ++pos)
+ if (fs::is_regular_file(pos->status()) )
+ if (fs::extension(*pos) == ".pcd")
+ {
+#if BOOST_FILESYSTEM_VERSION == 3
+ result.push_back (pos->path ().string ());
+#else
+ result.push_back (pos->path ());
+#endif
+ cout << "added: " << result.back() << endl;
+ }
+
+ return result;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct SampledScopeTime : public StopWatch
+{
+ enum { EACH = 33 };
+ SampledScopeTime(int& time_ms) : time_ms_(time_ms) {}
+ ~SampledScopeTime()
+ {
+ static int i_ = 0;
+ static boost::posix_time::ptime starttime_ = boost::posix_time::microsec_clock::local_time();
+ time_ms_ += getTime ();
+ if (i_ % EACH == 0 && i_)
+ {
+ boost::posix_time::ptime endtime_ = boost::posix_time::microsec_clock::local_time();
+ cout << "Average frame time = " << time_ms_ / EACH << "ms ( " << 1000.f * EACH / time_ms_ << "fps )"
+ << "( real: " << 1000.f * EACH / (endtime_-starttime_).total_milliseconds() << "fps )" << endl;
+ time_ms_ = 0;
+ starttime_ = endtime_;
+ }
+ ++i_;
+ }
+private:
+ int& time_ms_;
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+setViewerPose (visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
+{
+ Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f (0, 0, 0);
+ Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f (0, 0, 1) + pos_vector;
+ Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f (0, -1, 0);
+ viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2],
+ look_at_vector[0], look_at_vector[1], look_at_vector[2],
+ up_vector[0], up_vector[1], up_vector[2]);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+Eigen::Affine3f
+getViewerPose (visualization::PCLVisualizer& viewer)
+{
+ Eigen::Affine3f pose = viewer.getViewerPose();
+ Eigen::Matrix3f rotation = pose.linear();
+
+ Matrix3f axis_reorder;
+ axis_reorder << 0, 0, 1,
+ -1, 0, 0,
+ 0, -1, 0;
+
+ rotation = rotation * axis_reorder;
+ pose.linear() = rotation;
+ return pose;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+template<typename CloudT> void
+writeCloudFile (int format, const CloudT& cloud);
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+writePolygonMeshFile (int format, const pcl::PolygonMesh& mesh);
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+template<typename MergedT, typename PointT>
+typename PointCloud<MergedT>::Ptr merge(const PointCloud<PointT>& points, const PointCloud<RGB>& colors)
+{
+ typename PointCloud<MergedT>::Ptr merged_ptr(new PointCloud<MergedT>());
+
+ pcl::copyPointCloud (points, *merged_ptr);
+ for (size_t i = 0; i < colors.size (); ++i)
+ merged_ptr->points[i].rgba = colors.points[i].rgba;
+
+ return merged_ptr;
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+boost::shared_ptr<pcl::PolygonMesh> convertToMesh(const DeviceArray<PointXYZ>& triangles)
+{
+ if (triangles.empty())
+ return boost::shared_ptr<pcl::PolygonMesh>();
+
+ pcl::PointCloud<pcl::PointXYZ> cloud;
+ cloud.width = (int)triangles.size();
+ cloud.height = 1;
+ triangles.download(cloud.points);
+
+ boost::shared_ptr<pcl::PolygonMesh> mesh_ptr( new pcl::PolygonMesh() );
+ pcl::toPCLPointCloud2(cloud, mesh_ptr->cloud);
+
+ mesh_ptr->polygons.resize (triangles.size() / 3);
+ for (size_t i = 0; i < mesh_ptr->polygons.size (); ++i)
+ {
+ pcl::Vertices v;
+ v.vertices.push_back(i*3+0);
+ v.vertices.push_back(i*3+2);
+ v.vertices.push_back(i*3+1);
+ mesh_ptr->polygons[i] = v;
+ }
+ return mesh_ptr;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct CurrentFrameCloudView
+{
+ CurrentFrameCloudView() : cloud_device_ (480, 640), cloud_viewer_ ("Frame Cloud Viewer")
+ {
+ cloud_ptr_ = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ>);
+
+ cloud_viewer_.setBackgroundColor (0, 0, 0.15);
+ cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_POINT_SIZE, 1);
+ cloud_viewer_.addCoordinateSystem (1.0, "global");
+ cloud_viewer_.initCameraParameters ();
+ cloud_viewer_.setPosition (0, 500);
+ cloud_viewer_.setSize (640, 480);
+ cloud_viewer_.setCameraClipDistances (0.01, 10.01);
+ }
+
+ void
+ show (const KinfuTracker& kinfu)
+ {
+ kinfu.getLastFrameCloud (cloud_device_);
+
+ int c;
+ cloud_device_.download (cloud_ptr_->points, c);
+ cloud_ptr_->width = cloud_device_.cols ();
+ cloud_ptr_->height = cloud_device_.rows ();
+ cloud_ptr_->is_dense = false;
+
+ cloud_viewer_.removeAllPointClouds ();
+ cloud_viewer_.addPointCloud<PointXYZ>(cloud_ptr_);
+ cloud_viewer_.spinOnce ();
+ }
+
+ void
+ setViewerPose (const Eigen::Affine3f& viewer_pose) {
+ ::setViewerPose (cloud_viewer_, viewer_pose);
+ }
+
+ PointCloud<PointXYZ>::Ptr cloud_ptr_;
+ DeviceArray2D<PointXYZ> cloud_device_;
+ visualization::PCLVisualizer cloud_viewer_;
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct ImageView
+{
+ ImageView(int viz) : viz_(viz), paint_image_ (false), accumulate_views_ (false)
+ {
+ if (viz_)
+ {
+ viewerScene_ = pcl::visualization::ImageViewer::Ptr(new pcl::visualization::ImageViewer);
+ viewerDepth_ = pcl::visualization::ImageViewer::Ptr(new pcl::visualization::ImageViewer);
+
+ viewerScene_->setWindowTitle ("View3D from ray tracing");
+ viewerScene_->setPosition (0, 0);
+ viewerDepth_->setWindowTitle ("Kinect Depth stream");
+ viewerDepth_->setPosition (640, 0);
+ //viewerColor_.setWindowTitle ("Kinect RGB stream");
+ }
+ }
+
+ void
+ showScene (KinfuTracker& kinfu, const PtrStepSz<const KinfuTracker::PixelRGB>& rgb24, bool registration, Eigen::Affine3f* pose_ptr = 0)
+ {
+ if (pose_ptr)
+ {
+ raycaster_ptr_->run(kinfu.volume(), *pose_ptr);
+ raycaster_ptr_->generateSceneView(view_device_);
+ }
+ else
+ kinfu.getImage (view_device_);
+
+ if (paint_image_ && registration && !pose_ptr)
+ {
+ colors_device_.upload (rgb24.data, rgb24.step, rgb24.rows, rgb24.cols);
+ paint3DView (colors_device_, view_device_);
+ }
+
+
+ int cols;
+ view_device_.download (view_host_, cols);
+ if (viz_)
+ viewerScene_->showRGBImage (reinterpret_cast<unsigned char*> (&view_host_[0]), view_device_.cols (), view_device_.rows ());
+
+ //viewerColor_.showRGBImage ((unsigned char*)&rgb24.data, rgb24.cols, rgb24.rows);
+
+#ifdef HAVE_OPENCV
+ if (accumulate_views_)
+ {
+ views_.push_back (cv::Mat ());
+ cv::cvtColor (cv::Mat (480, 640, CV_8UC3, (void*)&view_host_[0]), views_.back (), CV_RGB2GRAY);
+ //cv::copy(cv::Mat(480, 640, CV_8UC3, (void*)&view_host_[0]), views_.back());
+ }
+#endif
+ }
+
+ void
+ showDepth (const PtrStepSz<const unsigned short>& depth)
+ {
+ if (viz_)
+ viewerDepth_->showShortImage (depth.data, depth.cols, depth.rows, 0, 5000, true);
+ }
+
+ void
+ showGeneratedDepth (KinfuTracker& kinfu, const Eigen::Affine3f& pose)
+ {
+ raycaster_ptr_->run(kinfu.volume(), pose);
+ raycaster_ptr_->generateDepthImage(generated_depth_);
+
+ int c;
+ vector<unsigned short> data;
+ generated_depth_.download(data, c);
+
+ if (viz_)
+ viewerDepth_->showShortImage (&data[0], generated_depth_.cols(), generated_depth_.rows(), 0, 5000, true);
+ }
+
+ void
+ toggleImagePaint()
+ {
+ paint_image_ = !paint_image_;
+ cout << "Paint image: " << (paint_image_ ? "On (requires registration mode)" : "Off") << endl;
+ }
+
+ int viz_;
+ bool paint_image_;
+ bool accumulate_views_;
+
+ visualization::ImageViewer::Ptr viewerScene_;
+ visualization::ImageViewer::Ptr viewerDepth_;
+ //visualization::ImageViewer viewerColor_;
+
+ KinfuTracker::View view_device_;
+ KinfuTracker::View colors_device_;
+ vector<KinfuTracker::PixelRGB> view_host_;
+
+ RayCaster::Ptr raycaster_ptr_;
+
+ KinfuTracker::DepthMap generated_depth_;
+
+#ifdef HAVE_OPENCV
+ vector<cv::Mat> views_;
+#endif
+};
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct SceneCloudView
+{
+ enum { GPU_Connected6 = 0, CPU_Connected6 = 1, CPU_Connected26 = 2 };
+
+ SceneCloudView(int viz) : viz_(viz), extraction_mode_ (GPU_Connected6), compute_normals_ (false), valid_combined_ (false), cube_added_(false)
+ {
+ cloud_ptr_ = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ>);
+ normals_ptr_ = PointCloud<Normal>::Ptr (new PointCloud<Normal>);
+ combined_ptr_ = PointCloud<PointNormal>::Ptr (new PointCloud<PointNormal>);
+ point_colors_ptr_ = PointCloud<RGB>::Ptr (new PointCloud<RGB>);
+
+ if (viz_)
+ {
+ cloud_viewer_ = pcl::visualization::PCLVisualizer::Ptr( new pcl::visualization::PCLVisualizer("Scene Cloud Viewer") );
+
+ cloud_viewer_->setBackgroundColor (0, 0, 0);
+ cloud_viewer_->addCoordinateSystem (1.0, "global");
+ cloud_viewer_->initCameraParameters ();
+ cloud_viewer_->setPosition (0, 500);
+ cloud_viewer_->setSize (640, 480);
+ cloud_viewer_->setCameraClipDistances (0.01, 10.01);
+
+ cloud_viewer_->addText ("H: print help", 2, 15, 20, 34, 135, 246);
+ }
+ }
+
+ void
+ show (KinfuTracker& kinfu, bool integrate_colors)
+ {
+ viewer_pose_ = kinfu.getCameraPose();
+
+ ScopeTimeT time ("PointCloud Extraction");
+ cout << "\nGetting cloud... " << flush;
+
+ valid_combined_ = false;
+
+ if (extraction_mode_ != GPU_Connected6) // So use CPU
+ {
+ kinfu.volume().fetchCloudHost (*cloud_ptr_, extraction_mode_ == CPU_Connected26);
+ }
+ else
+ {
+ DeviceArray<PointXYZ> extracted = kinfu.volume().fetchCloud (cloud_buffer_device_);
+
+ if (compute_normals_)
+ {
+ kinfu.volume().fetchNormals (extracted, normals_device_);
+ pcl::gpu::mergePointNormal (extracted, normals_device_, combined_device_);
+ combined_device_.download (combined_ptr_->points);
+ combined_ptr_->width = (int)combined_ptr_->points.size ();
+ combined_ptr_->height = 1;
+
+ valid_combined_ = true;
+ }
+ else
+ {
+ extracted.download (cloud_ptr_->points);
+ cloud_ptr_->width = (int)cloud_ptr_->points.size ();
+ cloud_ptr_->height = 1;
+ }
+
+ if (integrate_colors)
+ {
+ kinfu.colorVolume().fetchColors(extracted, point_colors_device_);
+ point_colors_device_.download(point_colors_ptr_->points);
+ point_colors_ptr_->width = (int)point_colors_ptr_->points.size ();
+ point_colors_ptr_->height = 1;
+ }
+ else
+ point_colors_ptr_->points.clear();
+ }
+ size_t points_size = valid_combined_ ? combined_ptr_->points.size () : cloud_ptr_->points.size ();
+ cout << "Done. Cloud size: " << points_size / 1000 << "K" << endl;
+
+ if (viz_)
+ {
+ cloud_viewer_->removeAllPointClouds ();
+ if (valid_combined_)
+ {
+ visualization::PointCloudColorHandlerRGBCloud<PointNormal> rgb(combined_ptr_, point_colors_ptr_);
+ cloud_viewer_->addPointCloud<PointNormal> (combined_ptr_, rgb, "Cloud");
+ cloud_viewer_->addPointCloudNormals<PointNormal>(combined_ptr_, 50);
+ }
+ else
+ {
+ visualization::PointCloudColorHandlerRGBCloud<PointXYZ> rgb(cloud_ptr_, point_colors_ptr_);
+ cloud_viewer_->addPointCloud<PointXYZ> (cloud_ptr_, rgb);
+ }
+ }
+ }
+
+ void
+ toggleCube(const Eigen::Vector3f& size)
+ {
+ if (!viz_)
+ return;
+
+ if (cube_added_)
+ cloud_viewer_->removeShape("cube");
+ else
+ cloud_viewer_->addCube(size*0.5, Eigen::Quaternionf::Identity(), size(0), size(1), size(2));
+
+ cube_added_ = !cube_added_;
+ }
+
+ void
+ toggleExtractionMode ()
+ {
+ extraction_mode_ = (extraction_mode_ + 1) % 3;
+
+ switch (extraction_mode_)
+ {
+ case 0: cout << "Cloud exctraction mode: GPU, Connected-6" << endl; break;
+ case 1: cout << "Cloud exctraction mode: CPU, Connected-6 (requires a lot of memory)" << endl; break;
+ case 2: cout << "Cloud exctraction mode: CPU, Connected-26 (requires a lot of memory)" << endl; break;
+ }
+ ;
+ }
+
+ void
+ toggleNormals ()
+ {
+ compute_normals_ = !compute_normals_;
+ cout << "Compute normals: " << (compute_normals_ ? "On" : "Off") << endl;
+ }
+
+ void
+ clearClouds (bool print_message = false)
+ {
+ if (!viz_)
+ return;
+
+ cloud_viewer_->removeAllPointClouds ();
+ cloud_ptr_->points.clear ();
+ normals_ptr_->points.clear ();
+ if (print_message)
+ cout << "Clouds/Meshes were cleared" << endl;
+ }
+
+ void
+ showMesh(KinfuTracker& kinfu, bool /*integrate_colors*/)
+ {
+ if (!viz_)
+ return;
+
+ ScopeTimeT time ("Mesh Extraction");
+ cout << "\nGetting mesh... " << flush;
+
+ if (!marching_cubes_)
+ marching_cubes_ = MarchingCubes::Ptr( new MarchingCubes() );
+
+ DeviceArray<PointXYZ> triangles_device = marching_cubes_->run(kinfu.volume(), triangles_buffer_device_);
+ mesh_ptr_ = convertToMesh(triangles_device);
+
+ cloud_viewer_->removeAllPointClouds ();
+ if (mesh_ptr_)
+ cloud_viewer_->addPolygonMesh(*mesh_ptr_);
+
+ cout << "Done. Triangles number: " << triangles_device.size() / MarchingCubes::POINTS_PER_TRIANGLE / 1000 << "K" << endl;
+ }
+
+ int viz_;
+ int extraction_mode_;
+ bool compute_normals_;
+ bool valid_combined_;
+ bool cube_added_;
+
+ Eigen::Affine3f viewer_pose_;
+
+ visualization::PCLVisualizer::Ptr cloud_viewer_;
+
+ PointCloud<PointXYZ>::Ptr cloud_ptr_;
+ PointCloud<Normal>::Ptr normals_ptr_;
+
+ DeviceArray<PointXYZ> cloud_buffer_device_;
+ DeviceArray<Normal> normals_device_;
+
+ PointCloud<PointNormal>::Ptr combined_ptr_;
+ DeviceArray<PointNormal> combined_device_;
+
+ DeviceArray<RGB> point_colors_device_;
+ PointCloud<RGB>::Ptr point_colors_ptr_;
+
+ MarchingCubes::Ptr marching_cubes_;
+ DeviceArray<PointXYZ> triangles_buffer_device_;
+
+ boost::shared_ptr<pcl::PolygonMesh> mesh_ptr_;
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct KinFuApp
+{
+ enum { PCD_BIN = 1, PCD_ASCII = 2, PLY = 3, MESH_PLY = 7, MESH_VTK = 8 };
+
+ KinFuApp(pcl::Grabber& source, float vsz, int icp, int viz, boost::shared_ptr<CameraPoseProcessor> pose_processor=boost::shared_ptr<CameraPoseProcessor> () ) : exit_ (false), scan_ (false), scan_mesh_(false), scan_volume_ (false), independent_camera_ (false),
+ registration_ (false), integrate_colors_ (false), pcd_source_ (false), focal_length_(-1.f), capture_ (source), scene_cloud_view_(viz), image_view_(viz), time_ms_(0), icp_(icp), viz_(viz), pose_processor_ (pose_processor)
+ {
+ //Init Kinfu Tracker
+ Eigen::Vector3f volume_size = Vector3f::Constant (vsz/*meters*/);
+ kinfu_.volume().setSize (volume_size);
+
+ Eigen::Matrix3f R = Eigen::Matrix3f::Identity (); // * AngleAxisf( pcl::deg2rad(-30.f), Vector3f::UnitX());
+ Eigen::Vector3f t = volume_size * 0.5f - Vector3f (0, 0, volume_size (2) / 2 * 1.2f);
+
+ Eigen::Affine3f pose = Eigen::Translation3f (t) * Eigen::AngleAxisf (R);
+
+ kinfu_.setInitalCameraPose (pose);
+ kinfu_.volume().setTsdfTruncDist (0.030f/*meters*/);
+ kinfu_.setIcpCorespFilteringParams (0.1f/*meters*/, sin ( pcl::deg2rad(20.f) ));
+ //kinfu_.setDepthTruncationForICP(5.f/*meters*/);
+ kinfu_.setCameraMovementThreshold(0.001f);
+
+ if (!icp)
+ kinfu_.disableIcp();
+
+ //Init KinfuApp
+ tsdf_cloud_ptr_ = pcl::PointCloud<pcl::PointXYZI>::Ptr (new pcl::PointCloud<pcl::PointXYZI>);
+ image_view_.raycaster_ptr_ = RayCaster::Ptr( new RayCaster(kinfu_.rows (), kinfu_.cols ()) );
+
+ if (viz_)
+ {
+ scene_cloud_view_.cloud_viewer_->registerKeyboardCallback (keyboard_callback, (void*)this);
+ image_view_.viewerScene_->registerKeyboardCallback (keyboard_callback, (void*)this);
+ image_view_.viewerDepth_->registerKeyboardCallback (keyboard_callback, (void*)this);
+
+ scene_cloud_view_.toggleCube(volume_size);
+ }
+ }
+
+ ~KinFuApp()
+ {
+ if (evaluation_ptr_)
+ evaluation_ptr_->saveAllPoses(kinfu_);
+ }
+
+ void
+ initCurrentFrameView ()
+ {
+ current_frame_cloud_view_ = boost::shared_ptr<CurrentFrameCloudView>(new CurrentFrameCloudView ());
+ current_frame_cloud_view_->cloud_viewer_.registerKeyboardCallback (keyboard_callback, (void*)this);
+ current_frame_cloud_view_->setViewerPose (kinfu_.getCameraPose ());
+ }
+
+ void
+ initRegistration ()
+ {
+ registration_ = capture_.providesCallback<pcl::ONIGrabber::sig_cb_openni_image_depth_image> ();
+ cout << "Registration mode: " << (registration_ ? "On" : "Off (not supported by source)") << endl;
+ if (registration_)
+ kinfu_.setDepthIntrinsics(KINFU_DEFAULT_RGB_FOCAL_X, KINFU_DEFAULT_RGB_FOCAL_Y);
+ }
+
+ void
+ setDepthIntrinsics(std::vector<float> depth_intrinsics)
+ {
+ float fx = depth_intrinsics[0];
+ float fy = depth_intrinsics[1];
+
+ if (depth_intrinsics.size() == 4)
+ {
+ float cx = depth_intrinsics[2];
+ float cy = depth_intrinsics[3];
+ kinfu_.setDepthIntrinsics(fx, fy, cx, cy);
+ cout << "Depth intrinsics changed to fx="<< fx << " fy=" << fy << " cx=" << cx << " cy=" << cy << endl;
+ }
+ else {
+ kinfu_.setDepthIntrinsics(fx, fy);
+ cout << "Depth intrinsics changed to fx="<< fx << " fy=" << fy << endl;
+ }
+ }
+
+ void
+ toggleColorIntegration()
+ {
+ if(registration_)
+ {
+ const int max_color_integration_weight = 2;
+ kinfu_.initColorIntegration(max_color_integration_weight);
+ integrate_colors_ = true;
+ }
+ cout << "Color integration: " << (integrate_colors_ ? "On" : "Off ( requires registration mode )") << endl;
+ }
+
+ void
+ enableTruncationScaling()
+ {
+ kinfu_.volume().setTsdfTruncDist (kinfu_.volume().getSize()(0) / 100.0f);
+ }
+
+ void
+ toggleIndependentCamera()
+ {
+ independent_camera_ = !independent_camera_;
+ cout << "Camera mode: " << (independent_camera_ ? "Independent" : "Bound to Kinect pose") << endl;
+ }
+
+ void
+ toggleEvaluationMode(const string& eval_folder, const string& match_file = string())
+ {
+ evaluation_ptr_ = Evaluation::Ptr( new Evaluation(eval_folder) );
+ if (!match_file.empty())
+ evaluation_ptr_->setMatchFile(match_file);
+
+ kinfu_.setDepthIntrinsics (evaluation_ptr_->fx, evaluation_ptr_->fy, evaluation_ptr_->cx, evaluation_ptr_->cy);
+ image_view_.raycaster_ptr_ = RayCaster::Ptr( new RayCaster(kinfu_.rows (), kinfu_.cols (),
+ evaluation_ptr_->fx, evaluation_ptr_->fy, evaluation_ptr_->cx, evaluation_ptr_->cy) );
+ }
+
+ void execute(const PtrStepSz<const unsigned short>& depth, const PtrStepSz<const KinfuTracker::PixelRGB>& rgb24, bool has_data)
+ {
+ bool has_image = false;
+
+ if (has_data)
+ {
+ depth_device_.upload (depth.data, depth.step, depth.rows, depth.cols);
+ if (integrate_colors_)
+ image_view_.colors_device_.upload (rgb24.data, rgb24.step, rgb24.rows, rgb24.cols);
+
+ {
+ SampledScopeTime fps(time_ms_);
+
+ //run kinfu algorithm
+ if (integrate_colors_)
+ has_image = kinfu_ (depth_device_, image_view_.colors_device_);
+ else
+ has_image = kinfu_ (depth_device_);
+ }
+
+ // process camera pose
+ if (pose_processor_)
+ {
+ pose_processor_->processPose (kinfu_.getCameraPose ());
+ }
+
+ image_view_.showDepth (depth);
+ //image_view_.showGeneratedDepth(kinfu_, kinfu_.getCameraPose());
+ }
+
+ if (scan_)
+ {
+ scan_ = false;
+ scene_cloud_view_.show (kinfu_, integrate_colors_);
+
+ if (scan_volume_)
+ {
+ cout << "Downloading TSDF volume from device ... " << flush;
+ kinfu_.volume().downloadTsdfAndWeighs (tsdf_volume_.volumeWriteable (), tsdf_volume_.weightsWriteable ());
+ tsdf_volume_.setHeader (Eigen::Vector3i (pcl::device::VOLUME_X, pcl::device::VOLUME_Y, pcl::device::VOLUME_Z), kinfu_.volume().getSize ());
+ cout << "done [" << tsdf_volume_.size () << " voxels]" << endl << endl;
+
+ cout << "Converting volume to TSDF cloud ... " << flush;
+ tsdf_volume_.convertToTsdfCloud (tsdf_cloud_ptr_);
+ cout << "done [" << tsdf_cloud_ptr_->size () << " points]" << endl << endl;
+ }
+ else
+ cout << "[!] tsdf volume download is disabled" << endl << endl;
+ }
+
+ if (scan_mesh_)
+ {
+ scan_mesh_ = false;
+ scene_cloud_view_.showMesh(kinfu_, integrate_colors_);
+ }
+
+ if (viz_ && has_image)
+ {
+ Eigen::Affine3f viewer_pose = getViewerPose(*scene_cloud_view_.cloud_viewer_);
+ image_view_.showScene (kinfu_, rgb24, registration_, independent_camera_ ? &viewer_pose : 0);
+ }
+
+ if (current_frame_cloud_view_)
+ current_frame_cloud_view_->show (kinfu_);
+
+ if (viz_ && !independent_camera_)
+ setViewerPose (*scene_cloud_view_.cloud_viewer_, kinfu_.getCameraPose());
+ }
+
+ void source_cb1_device(const boost::shared_ptr<openni_wrapper::DepthImage>& depth_wrapper)
+ {
+ {
+ boost::mutex::scoped_try_lock lock(data_ready_mutex_);
+ if (exit_ || !lock)
+ return;
+
+ depth_.cols = depth_wrapper->getWidth();
+ depth_.rows = depth_wrapper->getHeight();
+ depth_.step = depth_.cols * depth_.elemSize();
+
+ source_depth_data_.resize(depth_.cols * depth_.rows);
+ depth_wrapper->fillDepthImageRaw(depth_.cols, depth_.rows, &source_depth_data_[0]);
+ depth_.data = &source_depth_data_[0];
+ }
+ data_ready_cond_.notify_one();
+ }
+
+ void source_cb2_device(const boost::shared_ptr<openni_wrapper::Image>& image_wrapper, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_wrapper, float)
+ {
+ {
+ boost::mutex::scoped_try_lock lock(data_ready_mutex_);
+ if (exit_ || !lock)
+ return;
+
+ depth_.cols = depth_wrapper->getWidth();
+ depth_.rows = depth_wrapper->getHeight();
+ depth_.step = depth_.cols * depth_.elemSize();
+
+ source_depth_data_.resize(depth_.cols * depth_.rows);
+ depth_wrapper->fillDepthImageRaw(depth_.cols, depth_.rows, &source_depth_data_[0]);
+ depth_.data = &source_depth_data_[0];
+
+ rgb24_.cols = image_wrapper->getWidth();
+ rgb24_.rows = image_wrapper->getHeight();
+ rgb24_.step = rgb24_.cols * rgb24_.elemSize();
+
+ source_image_data_.resize(rgb24_.cols * rgb24_.rows);
+ image_wrapper->fillRGB(rgb24_.cols, rgb24_.rows, (unsigned char*)&source_image_data_[0]);
+ rgb24_.data = &source_image_data_[0];
+ }
+ data_ready_cond_.notify_one();
+ }
+
+
+ void source_cb1_oni(const boost::shared_ptr<openni_wrapper::DepthImage>& depth_wrapper)
+ {
+ {
+ boost::mutex::scoped_lock lock(data_ready_mutex_);
+ if (exit_)
+ return;
+
+ depth_.cols = depth_wrapper->getWidth();
+ depth_.rows = depth_wrapper->getHeight();
+ depth_.step = depth_.cols * depth_.elemSize();
+
+ source_depth_data_.resize(depth_.cols * depth_.rows);
+ depth_wrapper->fillDepthImageRaw(depth_.cols, depth_.rows, &source_depth_data_[0]);
+ depth_.data = &source_depth_data_[0];
+ }
+ data_ready_cond_.notify_one();
+ }
+
+ void source_cb2_oni(const boost::shared_ptr<openni_wrapper::Image>& image_wrapper, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_wrapper, float)
+ {
+ {
+ boost::mutex::scoped_lock lock(data_ready_mutex_);
+ if (exit_)
+ return;
+
+ depth_.cols = depth_wrapper->getWidth();
+ depth_.rows = depth_wrapper->getHeight();
+ depth_.step = depth_.cols * depth_.elemSize();
+
+ source_depth_data_.resize(depth_.cols * depth_.rows);
+ depth_wrapper->fillDepthImageRaw(depth_.cols, depth_.rows, &source_depth_data_[0]);
+ depth_.data = &source_depth_data_[0];
+
+ rgb24_.cols = image_wrapper->getWidth();
+ rgb24_.rows = image_wrapper->getHeight();
+ rgb24_.step = rgb24_.cols * rgb24_.elemSize();
+
+ source_image_data_.resize(rgb24_.cols * rgb24_.rows);
+ image_wrapper->fillRGB(rgb24_.cols, rgb24_.rows, (unsigned char*)&source_image_data_[0]);
+ rgb24_.data = &source_image_data_[0];
+ }
+ data_ready_cond_.notify_one();
+ }
+
+ void
+ source_cb3 (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr & DC3)
+ {
+ {
+ boost::mutex::scoped_try_lock lock(data_ready_mutex_);
+ if (exit_ || !lock)
+ return;
+ int width = DC3->width;
+ int height = DC3->height;
+ depth_.cols = width;
+ depth_.rows = height;
+ depth_.step = depth_.cols * depth_.elemSize ();
+ source_depth_data_.resize (depth_.cols * depth_.rows);
+
+ rgb24_.cols = width;
+ rgb24_.rows = height;
+ rgb24_.step = rgb24_.cols * rgb24_.elemSize ();
+ source_image_data_.resize (rgb24_.cols * rgb24_.rows);
+
+ unsigned char *rgb = (unsigned char *) &source_image_data_[0];
+ unsigned short *depth = (unsigned short *) &source_depth_data_[0];
+
+ for (int i=0; i < width*height; i++)
+ {
+ PointXYZRGBA pt = DC3->at (i);
+ rgb[3*i +0] = pt.r;
+ rgb[3*i +1] = pt.g;
+ rgb[3*i +2] = pt.b;
+ depth[i] = pt.z/0.001;
+ }
+ rgb24_.data = &source_image_data_[0];
+ depth_.data = &source_depth_data_[0];
+ }
+ data_ready_cond_.notify_one ();
+ }
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ startMainLoop (bool triggered_capture)
+ {
+ using namespace openni_wrapper;
+ typedef boost::shared_ptr<DepthImage> DepthImagePtr;
+ typedef boost::shared_ptr<Image> ImagePtr;
+
+ boost::function<void (const ImagePtr&, const DepthImagePtr&, float constant)> func1_dev = boost::bind (&KinFuApp::source_cb2_device, this, _1, _2, _3);
+ boost::function<void (const DepthImagePtr&)> func2_dev = boost::bind (&KinFuApp::source_cb1_device, this, _1);
+
+ boost::function<void (const ImagePtr&, const DepthImagePtr&, float constant)> func1_oni = boost::bind (&KinFuApp::source_cb2_oni, this, _1, _2, _3);
+ boost::function<void (const DepthImagePtr&)> func2_oni = boost::bind (&KinFuApp::source_cb1_oni, this, _1);
+
+ bool is_oni = dynamic_cast<pcl::ONIGrabber*>(&capture_) != 0;
+ boost::function<void (const ImagePtr&, const DepthImagePtr&, float constant)> func1 = is_oni ? func1_oni : func1_dev;
+ boost::function<void (const DepthImagePtr&)> func2 = is_oni ? func2_oni : func2_dev;
+
+ boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&) > func3 = boost::bind (&KinFuApp::source_cb3, this, _1);
+
+ bool need_colors = integrate_colors_ || registration_;
+ if ( pcd_source_ && !capture_.providesCallback<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)>() )
+ {
+ std::cout << "grabber doesn't provide pcl::PointCloud<pcl::PointXYZRGBA> callback !\n";
+ }
+ boost::signals2::connection c = pcd_source_? capture_.registerCallback (func3) : need_colors ? capture_.registerCallback (func1) : capture_.registerCallback (func2);
+
+ {
+ boost::unique_lock<boost::mutex> lock(data_ready_mutex_);
+
+ if (!triggered_capture)
+ capture_.start (); // Start stream
+
+ bool scene_view_not_stopped= viz_ ? !scene_cloud_view_.cloud_viewer_->wasStopped () : true;
+ bool image_view_not_stopped= viz_ ? !image_view_.viewerScene_->wasStopped () : true;
+
+ while (!exit_ && scene_view_not_stopped && image_view_not_stopped)
+ {
+ if (triggered_capture)
+ capture_.start(); // Triggers new frame
+ bool has_data = data_ready_cond_.timed_wait (lock, boost::posix_time::millisec(100));
+
+ try { this->execute (depth_, rgb24_, has_data); }
+ catch (const std::bad_alloc& /*e*/) { cout << "Bad alloc" << endl; break; }
+ catch (const std::exception& /*e*/) { cout << "Exception" << endl; break; }
+
+ if (viz_)
+ scene_cloud_view_.cloud_viewer_->spinOnce (3);
+ }
+
+ if (!triggered_capture)
+ capture_.stop (); // Stop stream
+ }
+ c.disconnect();
+ }
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ writeCloud (int format) const
+ {
+ const SceneCloudView& view = scene_cloud_view_;
+
+ // Points to export are either in cloud_ptr_ or combined_ptr_.
+ // If none have points, we have nothing to export.
+ if (view.cloud_ptr_->points.empty () && view.combined_ptr_->points.empty ())
+ {
+ cout << "Not writing cloud: Cloud is empty" << endl;
+ }
+ else
+ {
+ if(view.point_colors_ptr_->points.empty()) // no colors
+ {
+ if (view.valid_combined_)
+ writeCloudFile (format, view.combined_ptr_);
+ else
+ writeCloudFile (format, view.cloud_ptr_);
+ }
+ else
+ {
+ if (view.valid_combined_)
+ writeCloudFile (format, merge<PointXYZRGBNormal>(*view.combined_ptr_, *view.point_colors_ptr_));
+ else
+ writeCloudFile (format, merge<PointXYZRGB>(*view.cloud_ptr_, *view.point_colors_ptr_));
+ }
+ }
+ }
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ writeMesh(int format) const
+ {
+ if (scene_cloud_view_.mesh_ptr_)
+ writePolygonMeshFile(format, *scene_cloud_view_.mesh_ptr_);
+ }
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ printHelp ()
+ {
+ cout << endl;
+ cout << "KinFu app hotkeys" << endl;
+ cout << "=================" << endl;
+ cout << " H : print this help" << endl;
+ cout << " Esc : exit" << endl;
+ cout << " T : take cloud" << endl;
+ cout << " A : take mesh" << endl;
+ cout << " M : toggle cloud exctraction mode" << endl;
+ cout << " N : toggle normals exctraction" << endl;
+ cout << " I : toggle independent camera mode" << endl;
+ cout << " B : toggle volume bounds" << endl;
+ cout << " * : toggle scene view painting ( requires registration mode )" << endl;
+ cout << " C : clear clouds" << endl;
+ cout << " 1,2,3 : save cloud to PCD(binary), PCD(ASCII), PLY(ASCII)" << endl;
+ cout << " 7,8 : save mesh to PLY, VTK" << endl;
+ cout << " X, V : TSDF volume utility" << endl;
+ cout << endl;
+ }
+
+ bool exit_;
+ bool scan_;
+ bool scan_mesh_;
+ bool scan_volume_;
+
+ bool independent_camera_;
+
+ bool registration_;
+ bool integrate_colors_;
+ bool pcd_source_;
+ float focal_length_;
+
+ pcl::Grabber& capture_;
+ KinfuTracker kinfu_;
+
+ SceneCloudView scene_cloud_view_;
+ ImageView image_view_;
+ boost::shared_ptr<CurrentFrameCloudView> current_frame_cloud_view_;
+
+ KinfuTracker::DepthMap depth_device_;
+
+ pcl::TSDFVolume<float, short> tsdf_volume_;
+ pcl::PointCloud<pcl::PointXYZI>::Ptr tsdf_cloud_ptr_;
+
+ Evaluation::Ptr evaluation_ptr_;
+
+ boost::mutex data_ready_mutex_;
+ boost::condition_variable data_ready_cond_;
+
+ std::vector<KinfuTracker::PixelRGB> source_image_data_;
+ std::vector<unsigned short> source_depth_data_;
+ PtrStepSz<const unsigned short> depth_;
+ PtrStepSz<const KinfuTracker::PixelRGB> rgb24_;
+
+ int time_ms_;
+ int icp_, viz_;
+
+ boost::shared_ptr<CameraPoseProcessor> pose_processor_;
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ static void
+ keyboard_callback (const visualization::KeyboardEvent &e, void *cookie)
+ {
+ KinFuApp* app = reinterpret_cast<KinFuApp*> (cookie);
+
+ int key = e.getKeyCode ();
+
+ if (e.keyUp ())
+ switch (key)
+ {
+ case 27: app->exit_ = true; break;
+ case (int)'t': case (int)'T': app->scan_ = true; break;
+ case (int)'a': case (int)'A': app->scan_mesh_ = true; break;
+ case (int)'h': case (int)'H': app->printHelp (); break;
+ case (int)'m': case (int)'M': app->scene_cloud_view_.toggleExtractionMode (); break;
+ case (int)'n': case (int)'N': app->scene_cloud_view_.toggleNormals (); break;
+ case (int)'c': case (int)'C': app->scene_cloud_view_.clearClouds (true); break;
+ case (int)'i': case (int)'I': app->toggleIndependentCamera (); break;
+ case (int)'b': case (int)'B': app->scene_cloud_view_.toggleCube(app->kinfu_.volume().getSize()); break;
+ case (int)'7': case (int)'8': app->writeMesh (key - (int)'0'); break;
+ case (int)'1': case (int)'2': case (int)'3': app->writeCloud (key - (int)'0'); break;
+ case '*': app->image_view_.toggleImagePaint (); break;
+
+ case (int)'x': case (int)'X':
+ app->scan_volume_ = !app->scan_volume_;
+ cout << endl << "Volume scan: " << (app->scan_volume_ ? "enabled" : "disabled") << endl << endl;
+ break;
+ case (int)'v': case (int)'V':
+ cout << "Saving TSDF volume to tsdf_volume.dat ... " << flush;
+ app->tsdf_volume_.save ("tsdf_volume.dat", true);
+ cout << "done [" << app->tsdf_volume_.size () << " voxels]" << endl;
+ cout << "Saving TSDF volume cloud to tsdf_cloud.pcd ... " << flush;
+ pcl::io::savePCDFile<pcl::PointXYZI> ("tsdf_cloud.pcd", *app->tsdf_cloud_ptr_, true);
+ cout << "done [" << app->tsdf_cloud_ptr_->size () << " points]" << endl;
+ break;
+
+ default:
+ break;
+ }
+ }
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename CloudPtr> void
+writeCloudFile (int format, const CloudPtr& cloud_prt)
+{
+ if (format == KinFuApp::PCD_BIN)
+ {
+ cout << "Saving point cloud to 'cloud_bin.pcd' (binary)... " << flush;
+ pcl::io::savePCDFile ("cloud_bin.pcd", *cloud_prt, true);
+ }
+ else
+ if (format == KinFuApp::PCD_ASCII)
+ {
+ cout << "Saving point cloud to 'cloud.pcd' (ASCII)... " << flush;
+ pcl::io::savePCDFile ("cloud.pcd", *cloud_prt, false);
+ }
+ else /* if (format == KinFuApp::PLY) */
+ {
+ cout << "Saving point cloud to 'cloud.ply' (ASCII)... " << flush;
+ pcl::io::savePLYFileASCII ("cloud.ply", *cloud_prt);
+
+ }
+ cout << "Done" << endl;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+writePolygonMeshFile (int format, const pcl::PolygonMesh& mesh)
+{
+ if (format == KinFuApp::MESH_PLY)
+ {
+ cout << "Saving mesh to to 'mesh.ply'... " << flush;
+ pcl::io::savePLYFile("mesh.ply", mesh);
+ }
+ else /* if (format == KinFuApp::MESH_VTK) */
+ {
+ cout << "Saving mesh to to 'mesh.vtk'... " << flush;
+ pcl::io::saveVTKFile("mesh.vtk", mesh);
+ }
+ cout << "Done" << endl;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+int
+print_cli_help ()
+{
+ cout << "\nKinFu parameters:" << endl;
+ cout << " --help, -h : print this message" << endl;
+ cout << " --registration, -r : try to enable registration (source needs to support this)" << endl;
+ cout << " --current-cloud, -cc : show current frame cloud" << endl;
+ cout << " --save-views, -sv : accumulate scene view and save in the end ( Requires OpenCV. Will cause 'bad_alloc' after some time )" << endl;
+ cout << " --integrate-colors, -ic : enable color integration mode (allows to get cloud with colors)" << endl;
+ cout << " --scale-truncation, -st : scale the truncation distance and raycaster based on the volume size" << endl;
+ cout << " -volume_size <size_in_meters> : define integration volume size" << endl;
+ cout << " --depth-intrinsics <fx>,<fy>[,<cx>,<cy> : set the intrinsics of the depth camera" << endl;
+ cout << " -save_pose <pose_file.csv> : write tracked camera positions to the specified file" << endl;
+ cout << "Valid depth data sources:" << endl;
+ cout << " -dev <device> (default), -oni <oni_file>, -pcd <pcd_file or directory>" << endl;
+ cout << "";
+ cout << " For RGBD benchmark (Requires OpenCV):" << endl;
+ cout << " -eval <eval_folder> [-match_file <associations_file_in_the_folder>]" << endl;
+
+ return 0;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+int
+main (int argc, char* argv[])
+{
+ if (pc::find_switch (argc, argv, "--help") || pc::find_switch (argc, argv, "-h"))
+ return print_cli_help ();
+
+ int device = 0;
+ pc::parse_argument (argc, argv, "-gpu", device);
+ pcl::gpu::setDevice (device);
+ pcl::gpu::printShortCudaDeviceInfo (device);
+
+// if (checkIfPreFermiGPU(device))
+// return cout << endl << "Kinfu is supported only for Fermi and Kepler arhitectures. It is not even compiled for pre-Fermi by default. Exiting..." << endl, 1;
+
+ boost::shared_ptr<pcl::Grabber> capture;
+
+ bool triggered_capture = false;
+ bool pcd_input = false;
+
+ std::string eval_folder, match_file, openni_device, oni_file, pcd_dir;
+ try
+ {
+ if (pc::parse_argument (argc, argv, "-dev", openni_device) > 0)
+ {
+ capture.reset (new pcl::OpenNIGrabber (openni_device));
+ }
+ else if (pc::parse_argument (argc, argv, "-oni", oni_file) > 0)
+ {
+ triggered_capture = true;
+ bool repeat = false; // Only run ONI file once
+ capture.reset (new pcl::ONIGrabber (oni_file, repeat, ! triggered_capture));
+ }
+ else if (pc::parse_argument (argc, argv, "-pcd", pcd_dir) > 0)
+ {
+ float fps_pcd = 15.0f;
+ pc::parse_argument (argc, argv, "-pcd_fps", fps_pcd);
+
+ vector<string> pcd_files = getPcdFilesInDir(pcd_dir);
+
+ // Sort the read files by name
+ sort (pcd_files.begin (), pcd_files.end ());
+ capture.reset (new pcl::PCDGrabber<pcl::PointXYZRGBA> (pcd_files, fps_pcd, false));
+ triggered_capture = true;
+ pcd_input = true;
+ }
+ else if (pc::parse_argument (argc, argv, "-eval", eval_folder) > 0)
+ {
+ //init data source latter
+ pc::parse_argument (argc, argv, "-match_file", match_file);
+ }
+ else
+ {
+ capture.reset( new pcl::OpenNIGrabber() );
+
+ //triggered_capture = true; capture.reset( new pcl::ONIGrabber("d:/onis/20111013-224932.oni", true, ! triggered_capture) );
+ //triggered_capture = true; capture.reset( new pcl::ONIGrabber("d:/onis/reg20111229-180846.oni, true, ! triggered_capture) );
+ //triggered_capture = true; capture.reset( new pcl::ONIGrabber("/media/Main/onis/20111013-224932.oni", true, ! triggered_capture) );
+ //triggered_capture = true; capture.reset( new pcl::ONIGrabber("d:/onis/20111013-224551.oni", true, ! triggered_capture) );
+ //triggered_capture = true; capture.reset( new pcl::ONIGrabber("d:/onis/20111013-224719.oni", true, ! triggered_capture) );
+ }
+ }
+ catch (const pcl::PCLException& /*e*/) { return cout << "Can't open depth source" << endl, -1; }
+
+ float volume_size = 3.f;
+ pc::parse_argument (argc, argv, "-volume_size", volume_size);
+
+ int icp = 1, visualization = 1;
+ std::vector<float> depth_intrinsics;
+ pc::parse_argument (argc, argv, "--icp", icp);
+ pc::parse_argument (argc, argv, "--viz", visualization);
+
+ std::string camera_pose_file;
+ boost::shared_ptr<CameraPoseProcessor> pose_processor;
+ if (pc::parse_argument (argc, argv, "-save_pose", camera_pose_file) && camera_pose_file.size () > 0)
+ {
+ pose_processor.reset (new CameraPoseWriter (camera_pose_file));
+ }
+
+ KinFuApp app (*capture, volume_size, icp, visualization, pose_processor);
+
+ if (pc::parse_argument (argc, argv, "-eval", eval_folder) > 0)
+ app.toggleEvaluationMode(eval_folder, match_file);
+
+ if (pc::find_switch (argc, argv, "--current-cloud") || pc::find_switch (argc, argv, "-cc"))
+ app.initCurrentFrameView ();
+
+ if (pc::find_switch (argc, argv, "--save-views") || pc::find_switch (argc, argv, "-sv"))
+ app.image_view_.accumulate_views_ = true; //will cause bad alloc after some time
+
+ if (pc::find_switch (argc, argv, "--registration") || pc::find_switch (argc, argv, "-r"))
+ {
+ if (pcd_input)
+ {
+ app.pcd_source_ = true;
+ app.registration_ = true; // since pcd provides registered rgbd
+ }
+ else
+ {
+ app.initRegistration ();
+ }
+ }
+ if (pc::find_switch (argc, argv, "--integrate-colors") || pc::find_switch (argc, argv, "-ic"))
+ app.toggleColorIntegration();
+
+ if (pc::find_switch (argc, argv, "--scale-truncation") || pc::find_switch (argc, argv, "-st"))
+ app.enableTruncationScaling();
+
+ if (pc::parse_x_arguments (argc, argv, "--depth-intrinsics", depth_intrinsics) > 0)
+ {
+ if ((depth_intrinsics.size() == 4) || (depth_intrinsics.size() == 2))
+ {
+ app.setDepthIntrinsics(depth_intrinsics);
+ }
+ else
+ {
+ pc::print_error("Depth intrinsics must be given on the form fx,fy[,cx,cy].\n");
+ return -1;
+ }
+ }
+
+ // executing
+ try { app.startMainLoop (triggered_capture); }
+ catch (const pcl::PCLException& /*e*/) { cout << "PCLException" << endl; }
+ catch (const std::bad_alloc& /*e*/) { cout << "Bad alloc" << endl; }
+ catch (const std::exception& /*e*/) { cout << "Exception" << endl; }
+
+#ifdef HAVE_OPENCV
+ for (size_t t = 0; t < app.image_view_.views_.size (); ++t)
+ {
+ if (t == 0)
+ {
+ cout << "Saving depth map of first view." << endl;
+ cv::imwrite ("./depthmap_1stview.png", app.image_view_.views_[0]);
+ cout << "Saving sequence of (" << app.image_view_.views_.size () << ") views." << endl;
+ }
+ char buf[4096];
+ sprintf (buf, "./%06d.png", (int)t);
+ cv::imwrite (buf, app.image_view_.views_[t]);
+ printf ("writing: %s\n", buf);
+ }
+#endif
+
+ return 0;
+}
--- /dev/null
+// Hacked version of kinfu_app.cpp
+// Simulates depth images
+// using pcl::simulation. these views are of a camera flying
+// around a short range object e.g. a tabletop from >3m aways
+// these are then fed into kinfu and a mesh reconstruction is made
+//
+// to run:
+// kinfu_app_sim -plyfile ~/projects/kmcl/kmcl/models/table_models/meta_model.ply
+// Maurice Fallon - mfallon <AT> mit.edu april 2012
+
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#define _CRT_SECURE_NO_DEPRECATE
+#include <iostream>
+
+// need to include GLEW net the top to avoid linking errors FOR PCL::SIMULATION:
+#include <GL/glew.h>
+
+#include <pcl/pcl_config.h>
+#ifdef OPENGL_IS_A_FRAMEWORK
+# include <OpenGL/gl.h>
+# include <OpenGL/glu.h>
+#else
+# include <GL/gl.h>
+# include <GL/glu.h>
+#endif
+#ifdef GLUT_IS_A_FRAMEWORK
+# include <GLUT/glut.h>
+#else
+# include <GL/glut.h>
+#endif
+
+#include <pcl/console/parse.h>
+#include <pcl/gpu/kinfu/kinfu.h>
+#include <pcl/gpu/kinfu/raycaster.h>
+#include <pcl/gpu/kinfu/marching_cubes.h>
+#include <pcl/gpu/containers/initialization.h>
+
+#include <pcl/common/time.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/visualization/image_viewer.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/vtk_io.h>
+
+#include "openni_capture.h"
+#include "color_handler.h"
+#include "evaluation.h"
+
+#include <pcl/common/angles.h>
+
+#include "tsdf_volume.h"
+#include "tsdf_volume.hpp"
+
+#ifdef HAVE_OPENCV
+ #include <opencv2/highgui/highgui.hpp>
+ #include <opencv2/imgproc/imgproc.hpp>
+//#include "video_recorder.h"
+#endif
+typedef pcl::ScopeTime ScopeTimeT;
+
+#include "../src/internal.h"
+
+#include <Eigen/Dense>
+#include <cmath>
+#include <iostream>
+#include <boost/shared_ptr.hpp>
+#ifdef _WIN32
+# define WIN32_LEAN_AND_MEAN
+# include <windows.h>
+#endif
+
+
+//SIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTART
+#include "pcl/common/common.h"
+#include "pcl/common/transforms.h"
+#include <pcl/console/print.h>
+#include <pcl/console/time.h>
+// define the following in order to eliminate the deprecated headers warning
+#define VTK_EXCLUDE_STRSTREAM_HEADERS
+#include <pcl/io/vtk_lib_io.h>
+//
+#include <pcl/simulation/camera.h>
+#include "pcl/simulation/model.h"
+#include "pcl/simulation/scene.h"
+#include "pcl/simulation/range_likelihood.h"
+// Writing PNG files:
+#include <opencv/cv.h>
+#include <opencv/highgui.h>
+//SIMENDSIMENDSIMENDSIMENDSIMENDSIMENDSIMEND
+
+using namespace std;
+using namespace pcl;
+using namespace pcl::gpu;
+using namespace Eigen;
+namespace pc = pcl::console;
+
+//SIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTART
+using namespace pcl::console;
+using namespace pcl::io;
+using namespace pcl::simulation;
+using namespace std;
+uint16_t t_gamma[2048];
+Scene::Ptr scene_;
+Camera::Ptr camera_;
+RangeLikelihood::Ptr range_likelihood_;
+//SIMENDSIMENDSIMENDSIMENDSIMENDSIMENDSIMEND
+
+
+namespace pcl
+{
+ namespace gpu
+ {
+ void paint3DView (const KinfuTracker::View& rgb24, KinfuTracker::View& view, float colors_weight = 0.5f);
+ void mergePointNormal (const DeviceArray<PointXYZ>& cloud, const DeviceArray<Normal>& normals, DeviceArray<PointNormal>& output);
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct SampledScopeTime : public StopWatch
+{
+ enum { EACH = 33 };
+ SampledScopeTime(int& time_ms, int i) : time_ms_(time_ms), i_(i) {}
+ ~SampledScopeTime()
+ {
+ time_ms_ += stopWatch_.getTime ();
+ if (i_ % EACH == 0 && i_)
+ {
+ cout << "Average frame time = " << time_ms_ / EACH << "ms ( " << 1000.f * EACH / time_ms_ << "fps )" << endl;
+ time_ms_ = 0;
+ }
+ }
+private:
+ StopWatch stopWatch_;
+ int& time_ms_;
+ int i_;
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+setViewerPose (visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
+{
+ Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f (0, 0, 0);
+ Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f (0, 0, 1) + pos_vector;
+ Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f (0, -1, 0);
+ viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2],
+ look_at_vector[0], look_at_vector[1], look_at_vector[2],
+ up_vector[0], up_vector[1], up_vector[2]);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+Eigen::Affine3f
+getViewerPose (visualization::PCLVisualizer& viewer)
+{
+ Eigen::Affine3f pose = viewer.getViewerPose();
+ Eigen::Matrix3f rotation = pose.linear();
+
+ Matrix3f axis_reorder;
+ axis_reorder << 0, 0, 1,
+ -1, 0, 0,
+ 0, -1, 0;
+
+ rotation = rotation * axis_reorder;
+ pose.linear() = rotation;
+ return pose;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//SIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTART
+void
+write_depth_image(const float* depth_buffer)
+{
+ int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
+ uint8_t* depth_img = new uint8_t[npixels * 3];
+
+ float min_depth = depth_buffer[0];
+ float max_depth = depth_buffer[0];
+ for (int i=1; i<npixels; i++)
+ {
+ if (depth_buffer[i] < min_depth) min_depth = depth_buffer[i];
+ if (depth_buffer[i] > max_depth) max_depth = depth_buffer[i];
+ }
+
+ for (int y = 0; y < 480; ++y)
+ {
+ for (int x = 0; x < 640; ++x)
+ {
+ int i= y*640 + x ;
+ int i_in= (480-1 -y) *640 + x ; // flip up down
+
+
+ float zn = 0.7;
+ float zf = 20.0;
+ float d = depth_buffer[i_in];
+ float z = -zf*zn/((zf-zn)*(d - zf/(zf-zn)));
+ float b = 0.075;
+ float f = 580.0;
+ uint16_t kd = static_cast<uint16_t>(1090 - b*f/z*8);
+ if (kd < 0) kd = 0;
+ else if (kd>2047) kd = 2047;
+
+ int pval = t_gamma[kd];
+ int lb = pval & 0xff;
+ switch (pval>>8) {
+ case 0:
+ depth_img[3*i+2] = 255;
+ depth_img[3*i+1] = 255-lb;
+ depth_img[3*i+0] = 255-lb;
+ break;
+ case 1:
+ depth_img[3*i+2] = 255;
+ depth_img[3*i+1] = lb;
+ depth_img[3*i+0] = 0;
+ break;
+ case 2:
+ depth_img[3*i+2] = 255-lb;
+ depth_img[3*i+1] = 255;
+ depth_img[3*i+0] = 0;
+ break;
+ case 3:
+ depth_img[3*i+2] = 0;
+ depth_img[3*i+1] = 255;
+ depth_img[3*i+0] = lb;
+ break;
+ case 4:
+ depth_img[3*i+2] = 0;
+ depth_img[3*i+1] = 255-lb;
+ depth_img[3*i+0] = 255;
+ break;
+ case 5:
+ depth_img[3*i+2] = 0;
+ depth_img[3*i+1] = 0;
+ depth_img[3*i+0] = 255-lb;
+ break;
+ default:
+ depth_img[3*i+2] = 0;
+ depth_img[3*i+1] = 0;
+ depth_img[3*i+0] = 0;
+ break;
+ }
+ }
+ }
+
+ // Write to file:
+ IplImage *cv_ipl = cvCreateImage( cvSize(640 ,480), 8, 3);
+ cv::Mat cv_mat(cv_ipl);
+ cv_mat.data = depth_img;
+
+ std::stringstream ss;
+ ss <<"depth_image.png" ;
+ cv::imwrite(ss.str() , cv_mat);
+
+ delete [] depth_img;
+}
+
+
+void
+write_rgb_image(const uint8_t* rgb_buffer)
+{
+ int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
+ uint8_t* rgb_img = new uint8_t[npixels * 3];
+
+ for (int y = 0; y < 480; ++y)
+ {
+ for (int x = 0; x < 640; ++x)
+ {
+ int px= y*640 + x ;
+ int px_in= (480-1 -y) *640 + x ; // flip up down
+ rgb_img [3* (px) +0] = rgb_buffer[3*px_in+0];
+ rgb_img [3* (px) +1] = rgb_buffer[3*px_in+1];
+ rgb_img [3* (px) +2] = rgb_buffer[3*px_in+2];
+ }
+ }
+
+ // Write to file:
+ IplImage *cv_ipl = cvCreateImage( cvSize(640 ,480), 8, 3);
+ cv::Mat cv_mat(cv_ipl);
+ cv_mat.data = rgb_img ;
+// cv::imwrite("rgb_image.png", cv_mat);
+
+ std::stringstream ss;
+ ss <<"rgb_image.png" ;
+ cv::imwrite(ss.str() , cv_mat);
+
+ delete [] rgb_img;
+}
+
+
+void
+depthBufferToMM(const float* depth_buffer,unsigned short* depth_img)
+{
+ int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
+ // unsigned short * depth_img = new unsigned short[npixels ];
+ for (int y = 0; y < 480; ++y)
+ {
+ for (int x = 0; x < 640; ++x)
+ {
+ int i= y*640 + x ;
+ int i_in= (480-1 -y) *640 + x ; // flip up down
+ float zn = 0.7;
+ float zf = 20.0;
+ float d = depth_buffer[i_in];
+ unsigned short z_new = (unsigned short) floor( 1000*( -zf*zn/((zf-zn)*(d - zf/(zf-zn)))));
+
+ if (z_new < 0) z_new = 0;
+// else if (z_new>65535) z_new = 65535;
+ else if (z_new>5000) z_new = 0;
+
+ // if ( z_new < 18000){
+// cout << z_new << " " << d << " " << x << "\n";
+// }
+ depth_img[i] = z_new;
+ }
+ }
+}
+
+
+void
+write_depth_image_uint(unsigned short* depth_img)
+{
+ // Write to file:
+ IplImage *cv_ipl = cvCreateImage( cvSize(640 ,480), IPL_DEPTH_16U, 1);
+ cv::Mat cv_mat(cv_ipl);
+ cv_mat.data =(uchar *) depth_img;
+ cv::imwrite("depth_image_uint.png", cv_mat);
+}
+
+
+// display_tic_toc: a helper function which accepts a set of
+// timestamps and displays the elapsed time between them as
+// a fraction and time used [for profiling]
+void
+display_tic_toc (vector<double> &tic_toc,const string &fun_name)
+{
+ size_t tic_toc_size = tic_toc.size ();
+
+ double percent_tic_toc_last = 0;
+ double dtime = tic_toc[tic_toc_size-1] - tic_toc[0];
+ cout << "fraction_" << fun_name << ",";
+ for (size_t i = 0; i < tic_toc_size; i++)
+ {
+ double percent_tic_toc = (tic_toc[i] - tic_toc[0])/(tic_toc[tic_toc_size-1] - tic_toc[0]);
+ cout << percent_tic_toc - percent_tic_toc_last << ", ";
+ percent_tic_toc_last = percent_tic_toc;
+ }
+ cout << "\ntime_" << fun_name << ",";
+ double time_tic_toc_last = 0;
+ for (size_t i = 0; i < tic_toc_size; i++)
+ {
+ double percent_tic_toc = (tic_toc[i] - tic_toc[0])/(tic_toc[tic_toc_size-1] - tic_toc[0]);
+ cout << percent_tic_toc*dtime - time_tic_toc_last << ", ";
+ time_tic_toc_last = percent_tic_toc*dtime;
+ }
+ cout << "\ntotal_time_" << fun_name << " " << dtime << "\n";
+}
+
+void
+capture (Eigen::Isometry3d pose_in,unsigned short* depth_buffer_mm,const uint8_t* color_buffer)//, string point_cloud_fname)
+{
+ // No reference image - but this is kept for compatability with range_test_v2:
+ float* reference = new float[range_likelihood_->getRowHeight() * range_likelihood_->getColWidth()];
+ //const float* depth_buffer = range_likelihood_->getDepthBuffer();
+ // Copy one image from our last as a reference.
+ /*
+ for (int i=0, n=0; i<range_likelihood_->getRowHeight(); ++i)
+ {
+ for (int j=0; j<range_likelihood_->getColWidth(); ++j)
+ {
+ reference[n++] = 0;//depth_buffer[i*range_likelihood_->getWidth() + j];
+ }
+ }
+ */
+
+ std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
+ std::vector<float> scores;
+ int n = 1;
+ poses.push_back (pose_in);
+ // HACK: mfallon modified computeLikelihoods to only call render() (which is currently private)
+ // need to make render public and use it.
+ // For simulation it is used alone from the reset of range_likelihood_
+ range_likelihood_->computeLikelihoods (reference, poses, scores);
+
+ color_buffer =range_likelihood_->getColorBuffer();
+ const float* db_ptr= range_likelihood_->getDepthBuffer ();
+ range_likelihood_->addNoise ();
+ depthBufferToMM (db_ptr,depth_buffer_mm);
+
+ // Writing these images is a smaller computation draw relative to KinFu:
+ write_depth_image (db_ptr);
+ //write_depth_image_uint(depth_buffer_mm);
+ write_rgb_image (color_buffer);
+
+/*
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out (new pcl::PointCloud<pcl::PointXYZRGB>);
+ bool write_cloud=true;
+ if (write_cloud)
+ {
+ // Read Color Buffer from the GPU before creating PointCloud:
+ // By default the buffers are not read back from the GPU
+ range_likelihood_->getColorBuffer ();
+ range_likelihood_->getDepthBuffer ();
+
+ // Add noise directly to the CPU depth buffer
+ range_likelihood_->addNoise ();
+
+ // Optional argument to save point cloud in global frame:
+ // Save camera relative:
+ //range_likelihood_->getPointCloud(pc_out);
+ // Save in global frame - applying the camera frame:
+ //range_likelihood_->getPointCloud(pc_out,true,camera_->pose());
+ // Save in local frame
+ range_likelihood_->getPointCloud (pc_out,false,camera_->pose ());
+ // TODO: what to do when there are more than one simulated view?
+ std::cout << pc_out->points.size() << " points written to file\n";
+
+ pcl::PCDWriter writer;
+ //writer.write (point_cloud_fname, *pc_out, false); /// ASCII
+ writer.writeBinary (point_cloud_fname, *pc_out);
+ //cout << "finished writing file\n";
+ }
+ */
+
+ delete [] reference;
+}
+
+// Read in a 3D model
+void
+load_PolygonMesh_model (std::string polygon_file)
+{
+ pcl::PolygonMesh mesh; // (new pcl::PolygonMesh);
+ //pcl::io::loadPolygonFile("/home/mfallon/data/models/dalet/Darlek_modified_works.obj",mesh);
+ if (!pcl::io::loadPolygonFile (polygon_file, mesh)){
+ std::cout << "No ply file found, exiting" << std::endl;
+ exit(-1);
+ }
+ pcl::PolygonMesh::Ptr cloud (new pcl::PolygonMesh (mesh));
+
+ TriangleMeshModel::Ptr model = TriangleMeshModel::Ptr (new TriangleMeshModel (cloud));
+ scene_->add (model);
+
+ std::cout << "Just read " << polygon_file << std::endl;
+ std::cout << mesh.polygons.size () << " polygons and "
+ << mesh.cloud.data.size () << " triangles\n";
+}
+
+// A 'halo' camera - a circular ring of poses all pointing at a center point
+// @param: focus_center: the center points
+// @param: halo_r: radius of the ring
+// @param: halo_dz: elevation of the camera above/below focus_center's z value
+// @param: n_poses: number of generated poses
+void
+generate_halo(
+ std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > &poses,
+ Eigen::Vector3d focus_center,double halo_r,double halo_dz,int n_poses)
+{
+ for (double t=0;t < (2*M_PI);t =t + (2*M_PI)/ ((double) n_poses) ){
+ double x = halo_r*cos(t);
+ double y = halo_r*sin(t);
+ double z = halo_dz;
+ double pitch =atan2( halo_dz,halo_r);
+ double yaw = atan2(-y,-x);
+
+ Eigen::Isometry3d pose;
+ pose.setIdentity();
+ Eigen::Matrix3d m;
+ m = AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
+ * AngleAxisd(pitch, Eigen::Vector3d::UnitY())
+ * AngleAxisd(0, Eigen::Vector3d::UnitZ());
+
+ pose *=m;
+ Vector3d v(x,y,z);
+ v += focus_center;
+ pose.translation() = v;
+ poses.push_back(pose);
+ }
+ return ;
+}
+//SIMENDSIMENDSIMENDSIMENDSIMENDSIMENDSIMENDSIMENDSIMENDSIMENDSIMEND
+
+
+template<typename CloudT> void
+writeCloudFile (int format, const CloudT& cloud);
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+writePolygonMeshFile (int format, const pcl::PolygonMesh& mesh);
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+template<typename MergedT, typename PointT>
+typename PointCloud<MergedT>::Ptr merge(const PointCloud<PointT>& points, const PointCloud<RGB>& colors)
+{
+ typename PointCloud<MergedT>::Ptr merged_ptr(new PointCloud<MergedT>());
+
+ pcl::copyPointCloud (points, *merged_ptr);
+ //pcl::copyPointCloud (colors, *merged_ptr); why error?
+ //pcl::concatenateFields (points, colors, *merged_ptr); why error?
+
+ for (size_t i = 0; i < colors.size (); ++i)
+ merged_ptr->points[i].rgba = colors.points[i].rgba;
+
+ return merged_ptr;
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+boost::shared_ptr<pcl::PolygonMesh> convertToMesh(const DeviceArray<PointXYZ>& triangles)
+{
+ if (triangles.empty())
+ return boost::shared_ptr<pcl::PolygonMesh>();
+
+ pcl::PointCloud<pcl::PointXYZ> cloud;
+ cloud.width = (int)triangles.size();
+ cloud.height = 1;
+ triangles.download(cloud.points);
+
+ boost::shared_ptr<pcl::PolygonMesh> mesh_ptr( new pcl::PolygonMesh() );
+ pcl::toPCLPointCloud2(cloud, mesh_ptr->cloud);
+
+ mesh_ptr->polygons.resize (triangles.size() / 3);
+ for (size_t i = 0; i < mesh_ptr->polygons.size (); ++i)
+ {
+ pcl::Vertices v;
+ v.vertices.push_back(i*3+0);
+ v.vertices.push_back(i*3+2);
+ v.vertices.push_back(i*3+1);
+ mesh_ptr->polygons[i] = v;
+ }
+ return mesh_ptr;
+
+ cout << mesh_ptr->polygons.size () << " plys\n";
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct CurrentFrameCloudView
+{
+ CurrentFrameCloudView() : cloud_device_ (480, 640), cloud_viewer_ ("Frame Cloud Viewer")
+ {
+ cloud_ptr_ = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ>);
+
+ cloud_viewer_.setBackgroundColor (0, 0, 0.15);
+ cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_POINT_SIZE, 1);
+ cloud_viewer_.addCoordinateSystem (1.0);
+ cloud_viewer_.initCameraParameters ();
+ cloud_viewer_.setPosition (0, 500);
+ cloud_viewer_.setSize (640, 480);
+ cloud_viewer_.setCameraClipDistances (0.01, 10.01);
+ }
+
+ void
+ show (const KinfuTracker& kinfu)
+ {
+ kinfu.getLastFrameCloud (cloud_device_);
+
+ int c;
+ cloud_device_.download (cloud_ptr_->points, c);
+ cloud_ptr_->width = cloud_device_.cols ();
+ cloud_ptr_->height = cloud_device_.rows ();
+ cloud_ptr_->is_dense = false;
+
+ cloud_viewer_.removeAllPointClouds ();
+ cloud_viewer_.addPointCloud<PointXYZ>(cloud_ptr_);
+ cloud_viewer_.spinOnce ();
+ }
+
+ void
+ setViewerPose (const Eigen::Affine3f& viewer_pose) {
+ ::setViewerPose (cloud_viewer_, viewer_pose);
+ }
+
+ PointCloud<PointXYZ>::Ptr cloud_ptr_;
+ DeviceArray2D<PointXYZ> cloud_device_;
+ visualization::PCLVisualizer cloud_viewer_;
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct ImageView
+{
+ ImageView() : paint_image_ (false), accumulate_views_ (false)
+ {
+ viewerScene_.setWindowTitle ("View3D from ray tracing");
+ viewerScene_.setPosition (0, 0);
+ viewerDepth_.setWindowTitle ("Kinect Depth stream");
+ viewerDepth_.setPosition (640, 0);
+ //viewerColor_.setWindowTitle ("Kinect RGB stream");
+ }
+
+ void
+ showScene (KinfuTracker& kinfu, const PtrStepSz<const KinfuTracker::PixelRGB>& rgb24, bool registration, Eigen::Affine3f* pose_ptr = 0)
+ {
+ if (pose_ptr)
+ {
+ raycaster_ptr_->run(kinfu.volume(), *pose_ptr);
+ raycaster_ptr_->generateSceneView(view_device_);
+ }
+ else
+ kinfu.getImage (view_device_);
+
+ if (paint_image_ && registration && !pose_ptr)
+ {
+ colors_device_.upload (rgb24.data, rgb24.step, rgb24.rows, rgb24.cols);
+ paint3DView (colors_device_, view_device_);
+ }
+
+ int cols;
+ view_device_.download (view_host_, cols);
+ viewerScene_.showRGBImage (reinterpret_cast<unsigned char*> (&view_host_[0]), view_device_.cols (), view_device_.rows ());
+
+ //viewerColor_.showRGBImage ((unsigned char*)&rgb24.data, rgb24.cols, rgb24.rows);
+
+#ifdef HAVE_OPENCV
+ if (accumulate_views_)
+ {
+ views_.push_back (cv::Mat ());
+ cv::cvtColor (cv::Mat (480, 640, CV_8UC3, (void*)&view_host_[0]), views_.back (), CV_RGB2GRAY);
+ //cv::copy(cv::Mat(480, 640, CV_8UC3, (void*)&view_host_[0]), views_.back());
+ }
+#endif
+ }
+
+ void
+ showDepth (const PtrStepSz<const unsigned short>& depth)
+ {
+ viewerDepth_.showShortImage (depth.data, depth.cols, depth.rows, 0, 5000, true);
+ }
+
+ void
+ showGeneratedDepth (KinfuTracker& kinfu, const Eigen::Affine3f& pose)
+ {
+ raycaster_ptr_->run(kinfu.volume(), pose);
+ raycaster_ptr_->generateDepthImage(generated_depth_);
+
+ int c;
+ vector<unsigned short> data;
+ generated_depth_.download(data, c);
+
+ viewerDepth_.showShortImage (&data[0], generated_depth_.cols(), generated_depth_.rows(), 0, 5000, true);
+ }
+
+ void
+ toggleImagePaint()
+ {
+ paint_image_ = !paint_image_;
+ cout << "Paint image: " << (paint_image_ ? "On (requires registration mode)" : "Off") << endl;
+ }
+
+ bool paint_image_;
+ bool accumulate_views_;
+
+ visualization::ImageViewer viewerScene_;
+ visualization::ImageViewer viewerDepth_;
+ //visualization::ImageViewer viewerColor_;
+
+ KinfuTracker::View view_device_;
+ KinfuTracker::View colors_device_;
+ vector<KinfuTracker::PixelRGB> view_host_;
+
+ RayCaster::Ptr raycaster_ptr_;
+
+ KinfuTracker::DepthMap generated_depth_;
+
+#ifdef HAVE_OPENCV
+ vector<cv::Mat> views_;
+#endif
+};
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct SceneCloudView
+{
+ enum { GPU_Connected6 = 0, CPU_Connected6 = 1, CPU_Connected26 = 2 };
+
+ SceneCloudView() : extraction_mode_ (GPU_Connected6), compute_normals_ (false), valid_combined_ (false), cube_added_(false), cloud_viewer_ ("Scene Cloud Viewer")
+ {
+ cloud_ptr_ = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ>);
+ normals_ptr_ = PointCloud<Normal>::Ptr (new PointCloud<Normal>);
+ combined_ptr_ = PointCloud<PointNormal>::Ptr (new PointCloud<PointNormal>);
+ point_colors_ptr_ = PointCloud<RGB>::Ptr (new PointCloud<RGB>);
+
+ cloud_viewer_.setBackgroundColor (0, 0, 0);
+ cloud_viewer_.addCoordinateSystem (1.0);
+ cloud_viewer_.initCameraParameters ();
+ cloud_viewer_.setPosition (0, 500);
+ cloud_viewer_.setSize (640, 480);
+ cloud_viewer_.setCameraClipDistances (0.01, 10.01);
+
+ cloud_viewer_.addText ("H: print help", 2, 15, 20, 34, 135, 246);
+ }
+
+ void
+ show (KinfuTracker& kinfu, bool integrate_colors)
+ {
+ viewer_pose_ = kinfu.getCameraPose();
+
+ ScopeTimeT time ("PointCloud Extraction");
+ cout << "\nGetting cloud... " << flush;
+
+ valid_combined_ = false;
+
+ if (extraction_mode_ != GPU_Connected6) // So use CPU
+ {
+ kinfu.volume().fetchCloudHost (*cloud_ptr_, extraction_mode_ == CPU_Connected26);
+ }
+ else
+ {
+ DeviceArray<PointXYZ> extracted = kinfu.volume().fetchCloud (cloud_buffer_device_);
+
+ if (compute_normals_)
+ {
+ kinfu.volume().fetchNormals (extracted, normals_device_);
+ pcl::gpu::mergePointNormal (extracted, normals_device_, combined_device_);
+ combined_device_.download (combined_ptr_->points);
+ combined_ptr_->width = (int)combined_ptr_->points.size ();
+ combined_ptr_->height = 1;
+
+ valid_combined_ = true;
+ }
+ else
+ {
+ extracted.download (cloud_ptr_->points);
+ cloud_ptr_->width = (int)cloud_ptr_->points.size ();
+ cloud_ptr_->height = 1;
+ }
+
+ if (integrate_colors)
+ {
+ kinfu.colorVolume().fetchColors(extracted, point_colors_device_);
+ point_colors_device_.download(point_colors_ptr_->points);
+ point_colors_ptr_->width = (int)point_colors_ptr_->points.size ();
+ point_colors_ptr_->height = 1;
+ }
+ else
+ point_colors_ptr_->points.clear();
+ }
+ size_t points_size = valid_combined_ ? combined_ptr_->points.size () : cloud_ptr_->points.size ();
+ cout << "Done. Cloud size: " << points_size / 1000 << "K" << endl;
+
+ cloud_viewer_.removeAllPointClouds ();
+ if (valid_combined_)
+ {
+ visualization::PointCloudColorHandlerRGBHack<PointNormal> rgb(combined_ptr_, point_colors_ptr_);
+ cloud_viewer_.addPointCloud<PointNormal> (combined_ptr_, rgb, "Cloud");
+ cloud_viewer_.addPointCloudNormals<PointNormal>(combined_ptr_, 50);
+ }
+ else
+ {
+ visualization::PointCloudColorHandlerRGBHack<PointXYZ> rgb(cloud_ptr_, point_colors_ptr_);
+ cloud_viewer_.addPointCloud<PointXYZ> (cloud_ptr_, rgb);
+ }
+ }
+
+ void
+ toggleCube(const Eigen::Vector3f& size)
+ {
+ if (cube_added_)
+ cloud_viewer_.removeShape("cube");
+ else
+ cloud_viewer_.addCube(size*0.5, Eigen::Quaternionf::Identity(), size(0), size(1), size(2));
+
+ cube_added_ = !cube_added_;
+ }
+
+ void
+ toggleExtractionMode ()
+ {
+ extraction_mode_ = (extraction_mode_ + 1) % 3;
+
+ switch (extraction_mode_)
+ {
+ case 0: cout << "Cloud exctraction mode: GPU, Connected-6" << endl; break;
+ case 1: cout << "Cloud exctraction mode: CPU, Connected-6 (requires a lot of memory)" << endl; break;
+ case 2: cout << "Cloud exctraction mode: CPU, Connected-26 (requires a lot of memory)" << endl; break;
+ }
+ ;
+ }
+
+ void
+ toggleNormals ()
+ {
+ compute_normals_ = !compute_normals_;
+ cout << "Compute normals: " << (compute_normals_ ? "On" : "Off") << endl;
+ }
+
+ void
+ clearClouds (bool print_message = false)
+ {
+ cloud_viewer_.removeAllPointClouds ();
+ cloud_ptr_->points.clear ();
+ normals_ptr_->points.clear ();
+ if (print_message)
+ cout << "Clouds/Meshes were cleared" << endl;
+ }
+
+ void
+ showMesh(KinfuTracker& kinfu, bool /*integrate_colors*/)
+ {
+ ScopeTimeT time ("Mesh Extraction");
+ cout << "\nGetting mesh... " << flush;
+
+ if (!marching_cubes_)
+ marching_cubes_ = MarchingCubes::Ptr( new MarchingCubes() );
+
+ DeviceArray<PointXYZ> triangles_device = marching_cubes_->run(kinfu.volume(), triangles_buffer_device_);
+ mesh_ptr_ = convertToMesh(triangles_device);
+
+ cloud_viewer_.removeAllPointClouds ();
+ if (mesh_ptr_){
+ cloud_viewer_.addPolygonMesh(*mesh_ptr_);
+ cout << "mesh ptr exist\n";
+ }else{
+ cout << "mesh ptr no exist\n";
+ }
+
+ cout << "Done. Triangles number: " << triangles_device.size() / MarchingCubes::POINTS_PER_TRIANGLE / 1000 << "K" << endl;
+ }
+
+ int extraction_mode_;
+ bool compute_normals_;
+ bool valid_combined_;
+ bool cube_added_;
+
+ Eigen::Affine3f viewer_pose_;
+
+ visualization::PCLVisualizer cloud_viewer_;
+
+ PointCloud<PointXYZ>::Ptr cloud_ptr_;
+ PointCloud<Normal>::Ptr normals_ptr_;
+
+ DeviceArray<PointXYZ> cloud_buffer_device_;
+ DeviceArray<Normal> normals_device_;
+
+ PointCloud<PointNormal>::Ptr combined_ptr_;
+ DeviceArray<PointNormal> combined_device_;
+
+ DeviceArray<RGB> point_colors_device_;
+ PointCloud<RGB>::Ptr point_colors_ptr_;
+
+ MarchingCubes::Ptr marching_cubes_;
+ DeviceArray<PointXYZ> triangles_buffer_device_;
+
+ boost::shared_ptr<pcl::PolygonMesh> mesh_ptr_;
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct KinFuApp
+{
+ enum { PCD_BIN = 1, PCD_ASCII = 2, PLY = 3, MESH_PLY = 7, MESH_VTK = 8 };
+
+ KinFuApp(CaptureOpenNI& source, float vsz) : exit_ (false), scan_ (false), scan_mesh_(false), scan_volume_ (false), independent_camera_ (false),
+ registration_ (false), integrate_colors_ (false), capture_ (source)
+ {
+ //Init Kinfu Tracker
+ Eigen::Vector3f volume_size = Vector3f::Constant (vsz/*meters*/);
+
+ float f = capture_.depth_focal_length_VGA;
+ kinfu_.setDepthIntrinsics (f, f);
+ kinfu_.volume().setSize (volume_size);
+
+ Eigen::Matrix3f R = Eigen::Matrix3f::Identity (); // * AngleAxisf( pcl::deg2rad(-30.f), Vector3f::UnitX());
+ Eigen::Vector3f t = volume_size * 0.5f - Vector3f (0, 0, volume_size (2) / 2 * 1.2f);
+
+ Eigen::Affine3f pose = Eigen::Translation3f (t) * Eigen::AngleAxisf (R);
+
+ kinfu_.setInitalCameraPose (pose);
+ kinfu_.volume().setTsdfTruncDist (0.030f/*meters*/);
+ kinfu_.setIcpCorespFilteringParams (0.1f/*meters*/, sin ( pcl::deg2rad(20.f) ));
+ //kinfu_.setDepthTruncationForICP(5.f/*meters*/);
+ kinfu_.setCameraMovementThreshold(0.001f);
+
+ //Init KinfuApp
+ tsdf_cloud_ptr_ = pcl::PointCloud<pcl::PointXYZI>::Ptr (new pcl::PointCloud<pcl::PointXYZI>);
+ image_view_.raycaster_ptr_ = RayCaster::Ptr( new RayCaster(kinfu_.rows (), kinfu_.cols (), f, f) );
+
+ scene_cloud_view_.cloud_viewer_.registerKeyboardCallback (keyboard_callback, (void*)this);
+ image_view_.viewerScene_.registerKeyboardCallback (keyboard_callback, (void*)this);
+ image_view_.viewerDepth_.registerKeyboardCallback (keyboard_callback, (void*)this);
+
+ float diag = sqrt ((float)kinfu_.cols () * kinfu_.cols () + kinfu_.rows () * kinfu_.rows ());
+ scene_cloud_view_.cloud_viewer_.setCameraFieldOfView (2 * atan (diag / (2 * f)) * 1.5);
+
+ scene_cloud_view_.toggleCube(volume_size);
+ }
+
+ ~KinFuApp()
+ {
+ if (evaluation_ptr_)
+ evaluation_ptr_->saveAllPoses(kinfu_);
+ }
+
+ void
+ initCurrentFrameView ()
+ {
+ current_frame_cloud_view_ = boost::shared_ptr<CurrentFrameCloudView>(new CurrentFrameCloudView ());
+ current_frame_cloud_view_->cloud_viewer_.registerKeyboardCallback (keyboard_callback, (void*)this);
+ current_frame_cloud_view_->setViewerPose (kinfu_.getCameraPose ());
+ }
+
+ void
+ tryRegistrationInit ()
+ {
+ registration_ = capture_.setRegistration (true);
+ cout << "Registration mode: " << (registration_ ? "On" : "Off (not supported by source)") << endl;
+ }
+
+ void
+ toggleColorIntegration(bool force = false)
+ {
+ if (registration_ || force)
+ {
+ const int max_color_integration_weight = 2;
+ kinfu_.initColorIntegration(max_color_integration_weight);
+ integrate_colors_ = true;
+ }
+ cout << "Color integration: " << (integrate_colors_ ? "On" : "Off (not supported by source)") << endl;
+ }
+
+ void
+ toggleIndependentCamera()
+ {
+ independent_camera_ = !independent_camera_;
+ cout << "Camera mode: " << (independent_camera_ ? "Independent" : "Bound to Kinect pose") << endl;
+ }
+
+ void
+ toggleEvaluationMode(const string& eval_folder, const string& match_file = string())
+ {
+ evaluation_ptr_ = Evaluation::Ptr( new Evaluation(eval_folder) );
+ if (!match_file.empty())
+ evaluation_ptr_->setMatchFile(match_file);
+
+ kinfu_.setDepthIntrinsics (evaluation_ptr_->fx, evaluation_ptr_->fy, evaluation_ptr_->cx, evaluation_ptr_->cy);
+ image_view_.raycaster_ptr_ = RayCaster::Ptr( new RayCaster(kinfu_.rows (), kinfu_.cols (),
+ evaluation_ptr_->fx, evaluation_ptr_->fy, evaluation_ptr_->cx, evaluation_ptr_->cy) );
+ }
+
+ void
+ execute (int argc, char** argv, std::string plyfile)
+ {
+ PtrStepSz<const unsigned short> depth;
+ PtrStepSz<const KinfuTracker::PixelRGB> rgb24;
+ int time_ms = 0;
+ bool has_image = false;
+
+ // Create simulation environment:
+ int width = 640;
+ int height = 480;
+ for (int i=0; i<2048; i++)
+ {
+ float v = i/2048.0;
+ v = powf(v, 3)* 6;
+ t_gamma[i] = v*6*256;
+ }
+
+ glutInit (&argc, argv);
+ glutInitDisplayMode (GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);// was GLUT_RGBA
+ glutInitWindowPosition (10, 10);
+ glutInitWindowSize (10, 10);
+ //glutInitWindowSize (window_width_, window_height_);
+ glutCreateWindow ("OpenGL range likelihood");
+
+ GLenum err = glewInit ();
+ if (GLEW_OK != err)
+ {
+ std::cerr << "Error: " << glewGetErrorString (err) << std::endl;
+ exit (-1);
+ }
+
+ std::cout << "Status: Using GLEW " << glewGetString (GLEW_VERSION) << std::endl;
+
+ if (glewIsSupported ("GL_VERSION_2_0"))
+ std::cout << "OpenGL 2.0 supported" << std::endl;
+ else
+ {
+ std::cerr << "Error: OpenGL 2.0 not supported" << std::endl;
+ exit(1);
+ }
+ std::cout << "GL_MAX_VIEWPORTS: " << GL_MAX_VIEWPORTS << std::endl;
+
+ camera_ = Camera::Ptr (new Camera ());
+ scene_ = Scene::Ptr (new Scene ());
+ range_likelihood_ = RangeLikelihood::Ptr (new RangeLikelihood (1, 1, height, width, scene_));
+
+ // Actually corresponds to default parameters:
+ range_likelihood_->setCameraIntrinsicsParameters (640,480, 576.09757860,
+ 576.09757860, 321.06398107, 242.97676897);
+ range_likelihood_->setComputeOnCPU (false);
+ range_likelihood_->setSumOnCPU (true);
+ range_likelihood_->setUseColor (true);
+
+ camera_->set(0.471703, 1.59862, 3.10937, 0, 0.418879, -12.2129);
+ camera_->set_pitch(0.418879); // not sure why this is here:
+
+ cout << "About to read: "<< plyfile << endl;
+ load_PolygonMesh_model (plyfile);
+
+ // Generate a series of poses:
+ std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
+ Eigen::Vector3d focus_center(0,0,1.3);
+ // double halo_r = 4.0;
+ double halo_r = 1.5;
+ double halo_dz = 1.5; // was 2;
+ // 20 is too quick when adding noise:
+ // 50 is ok though
+ int n_poses=50;
+ int n_pose_stop = 10;
+ // above means make a circle of 50 poses, stop after the 10th i.e. 1/5 of a halo ring:
+ generate_halo(poses,focus_center,halo_r,halo_dz,n_poses);
+
+ unsigned short * disparity_buf_ = new unsigned short[width*height ];
+ const KinfuTracker::PixelRGB* color_buf_;
+ const uint8_t* color_buf_uint;
+
+ // loop though and create the mesh:
+ for (int i = 0; !exit_; ++i)
+ {
+ vector<double> tic_toc;
+ tic_toc.push_back(getTime());
+ double tic_main = getTime();
+
+ Eigen::Vector3d t(poses[i].translation());
+ Eigen::Quaterniond r(poses[i].rotation());
+ std::stringstream ss;
+ ss << t[0]<<", "<<t[1]<<", "<<t[2]<<" | "
+ <<r.w()<<", "<<r.x()<<", "<<r.y()<<", "<<r.z() ;
+ std::cout << i << ": " << ss.str() << " pose_simulatedposition\n";
+
+ capture (poses[i],disparity_buf_, color_buf_uint);//,ss.str());
+ color_buf_ = (const KinfuTracker::PixelRGB*) color_buf_uint;
+ PtrStepSz<const unsigned short> depth_sim = PtrStepSz<const unsigned short>(height, width, disparity_buf_, 2*width);
+ //cout << depth_sim.rows << " by " << depth_sim.cols << " | s: " << depth_sim.step << "\n";
+ // RGB-KinFu currently disabled for now - problems with color in KinFu apparently
+ // but this constructor might not be right either: not sure about step size
+ integrate_colors_=false;
+ PtrStepSz<const KinfuTracker::PixelRGB> rgb24_sim = PtrStepSz<const KinfuTracker::PixelRGB>(height, width, color_buf_, width);
+ tic_toc.push_back (getTime ());
+
+ if (1==0){ // live capture - probably doesnt work anymore, left in here for comparison:
+ bool has_frame = evaluation_ptr_ ? evaluation_ptr_->grab(i, depth) : capture_.grab (depth, rgb24);
+ if (!has_frame)
+ {
+ cout << "Can't grab" << endl;
+ break;
+ }
+
+ depth_device_.upload (depth.data, depth.step, depth.rows, depth.cols);
+ if (integrate_colors_)
+ image_view_.colors_device_.upload (rgb24.data, rgb24.step, rgb24.rows, rgb24.cols);
+
+ {
+ SampledScopeTime fps(time_ms, i);
+
+ //run kinfu algorithm
+ if (integrate_colors_)
+ has_image = kinfu_ (depth_device_, image_view_.colors_device_);
+ else
+ has_image = kinfu_ (depth_device_);
+ }
+ }else{ //simulate:
+
+ cout << " color: " << integrate_colors_ << "\n"; // integrate_colors_ seems to be zero
+ depth_device_.upload (depth_sim.data, depth_sim.step, depth_sim.rows, depth_sim.cols);
+ if (integrate_colors_){
+ image_view_.colors_device_.upload (rgb24_sim.data, rgb24_sim.step, rgb24_sim.rows, rgb24_sim.cols);
+ }
+
+ tic_toc.push_back (getTime ());
+
+ {
+ SampledScopeTime fps(time_ms, i);
+ //run kinfu algorithm
+ if (integrate_colors_)
+ has_image = kinfu_ (depth_device_, image_view_.colors_device_);
+ else
+ has_image = kinfu_ (depth_device_);
+ }
+
+ }
+
+ tic_toc.push_back (getTime ());
+
+ Eigen::Affine3f k_aff = kinfu_.getCameraPose();
+ Eigen::Matrix3f k_m;
+ k_m =k_aff.rotation();
+ Eigen::Quaternionf k_r;
+ k_r = Eigen::Quaternionf(k_m);
+ std::stringstream ss_k;
+ ss_k << k_aff(0,3) <<", "<< k_aff(1,3)<<", "<< k_aff(2,3)<<" | "
+ <<k_r.w()<<", "<<k_r.x()<<", "<<k_r.y()<<", "<<k_r.z() ;
+ std::cout << i << ": " << ss_k.str() << " pose_kinect\n";
+
+ // Everything below this is Visualization or I/O:
+ if (i >n_pose_stop){
+ int pause;
+ cout << "Enter a key to write Mesh file\n";
+ cin >> pause;
+
+ scene_cloud_view_.showMesh(kinfu_, integrate_colors_);
+ writeMesh(KinFuApp::MESH_VTK);
+ // writeMesh(KinFuApp::MESH_PLY);
+
+ if (scan_)
+ {
+ scan_ = false;
+ scene_cloud_view_.show (kinfu_, integrate_colors_);
+
+ if (scan_volume_)
+ {
+ // download tsdf volume
+ {
+ ScopeTimeT time ("tsdf volume download");
+ cout << "Downloading TSDF volume from device ... " << flush;
+ kinfu_.volume().downloadTsdfAndWeighs (tsdf_volume_.volumeWriteable (), tsdf_volume_.weightsWriteable ());
+ tsdf_volume_.setHeader (Eigen::Vector3i (pcl::device::VOLUME_X, pcl::device::VOLUME_Y, pcl::device::VOLUME_Z), kinfu_.volume().getSize ());
+ cout << "done [" << tsdf_volume_.size () << " voxels]" << endl << endl;
+ }
+ {
+ ScopeTimeT time ("converting");
+ cout << "Converting volume to TSDF cloud ... " << flush;
+ tsdf_volume_.convertToTsdfCloud (tsdf_cloud_ptr_);
+ cout << "done [" << tsdf_cloud_ptr_->size () << " points]" << endl << endl;
+ }
+ }
+ else
+ cout << "[!] tsdf volume download is disabled" << endl << endl;
+ }
+
+ if (scan_mesh_)
+ {
+ scan_mesh_ = false;
+ scene_cloud_view_.showMesh(kinfu_, integrate_colors_);
+ }
+
+ if (has_image)
+ {
+ Eigen::Affine3f viewer_pose = getViewerPose(scene_cloud_view_.cloud_viewer_);
+// image_view_.showScene (kinfu_, rgb24, registration_, independent_camera_ ? &viewer_pose : 0);
+ image_view_.showScene (kinfu_, rgb24_sim, registration_, independent_camera_ ? &viewer_pose : 0);
+ }
+
+ if (current_frame_cloud_view_)
+ current_frame_cloud_view_->show (kinfu_);
+
+ image_view_.showDepth (depth_sim);
+ //image_view_.showDepth (depth);
+ // image_view_.showGeneratedDepth(kinfu_, kinfu_.getCameraPose());
+
+ if (!independent_camera_)
+ setViewerPose (scene_cloud_view_.cloud_viewer_, kinfu_.getCameraPose());
+
+ scene_cloud_view_.cloud_viewer_.spinOnce (3);
+
+ // As of April 2012, entering a key will end this program...
+ cout << "Paused after view\n";
+ cin >> pause;
+ }
+ double elapsed = (getTime() -tic_main);
+ cout << elapsed << " sec elapsed [" << (1/elapsed) << "]\n";
+ tic_toc.push_back (getTime ());
+ display_tic_toc (tic_toc, "kinfu_app_sim");
+ }
+ }
+
+ void
+ writeCloud (int format) const
+ {
+ const SceneCloudView& view = scene_cloud_view_;
+
+ if (!view.cloud_ptr_->points.empty ())
+ {
+ if(view.point_colors_ptr_->points.empty()) // no colors
+ {
+ if (view.valid_combined_)
+ writeCloudFile (format, view.combined_ptr_);
+ else
+ writeCloudFile (format, view.cloud_ptr_);
+ }
+ else
+ {
+ if (view.valid_combined_)
+ writeCloudFile (format, merge<PointXYZRGBNormal>(*view.combined_ptr_, *view.point_colors_ptr_));
+ else
+ writeCloudFile (format, merge<PointXYZRGB>(*view.cloud_ptr_, *view.point_colors_ptr_));
+ }
+ }
+ }
+
+ void
+ writeMesh(int format) const
+ {
+ if (scene_cloud_view_.mesh_ptr_) {
+ writePolygonMeshFile(format, *scene_cloud_view_.mesh_ptr_);
+ }
+ }
+
+ void
+ printHelp ()
+ {
+ cout << endl;
+ cout << "KinFu app hotkeys" << endl;
+ cout << "=================" << endl;
+ cout << " H : print this help" << endl;
+ cout << " Esc : exit" << endl;
+ cout << " T : take cloud" << endl;
+ cout << " A : take mesh" << endl;
+ cout << " M : toggle cloud exctraction mode" << endl;
+ cout << " N : toggle normals exctraction" << endl;
+ cout << " I : toggle independent camera mode" << endl;
+ cout << " B : toggle volume bounds" << endl;
+ cout << " * : toggle scene view painting ( requires registration mode )" << endl;
+ cout << " C : clear clouds" << endl;
+ cout << " 1,2,3 : save cloud to PCD(binary), PCD(ASCII), PLY(ASCII)" << endl;
+ cout << " 7,8 : save mesh to PLY, VTK" << endl;
+ cout << " X, V : TSDF volume utility" << endl;
+ cout << endl;
+ }
+
+ bool exit_;
+ bool scan_;
+ bool scan_mesh_;
+ bool scan_volume_;
+
+ bool independent_camera_;
+
+ bool registration_;
+ bool integrate_colors_;
+
+ CaptureOpenNI& capture_;
+ KinfuTracker kinfu_;
+
+ SceneCloudView scene_cloud_view_;
+ ImageView image_view_;
+ boost::shared_ptr<CurrentFrameCloudView> current_frame_cloud_view_;
+
+ KinfuTracker::DepthMap depth_device_;
+
+ pcl::TSDFVolume<float, short> tsdf_volume_;
+ pcl::PointCloud<pcl::PointXYZI>::Ptr tsdf_cloud_ptr_;
+
+ Evaluation::Ptr evaluation_ptr_;
+
+ static void
+ keyboard_callback (const visualization::KeyboardEvent &e, void *cookie)
+ {
+ KinFuApp* app = reinterpret_cast<KinFuApp*> (cookie);
+
+ int key = e.getKeyCode ();
+
+ if (e.keyUp ())
+ switch (key)
+ {
+ case 27: app->exit_ = true; break;
+ case (int)'t': case (int)'T': app->scan_ = true; break;
+ case (int)'a': case (int)'A': app->scan_mesh_ = true; break;
+ case (int)'h': case (int)'H': app->printHelp (); break;
+ case (int)'m': case (int)'M': app->scene_cloud_view_.toggleExtractionMode (); break;
+ case (int)'n': case (int)'N': app->scene_cloud_view_.toggleNormals (); break;
+ case (int)'c': case (int)'C': app->scene_cloud_view_.clearClouds (true); break;
+ case (int)'i': case (int)'I': app->toggleIndependentCamera (); break;
+ case (int)'b': case (int)'B': app->scene_cloud_view_.toggleCube(app->kinfu_.volume().getSize()); break;
+ case (int)'7': case (int)'8': app->writeMesh (key - (int)'0'); break;
+ case (int)'1': case (int)'2': case (int)'3': app->writeCloud (key - (int)'0'); break;
+ case '*': app->image_view_.toggleImagePaint (); break;
+
+ case (int)'x': case (int)'X':
+ app->scan_volume_ = !app->scan_volume_;
+ cout << endl << "Volume scan: " << (app->scan_volume_ ? "enabled" : "disabled") << endl << endl;
+ break;
+ case (int)'v': case (int)'V':
+ cout << "Saving TSDF volume to tsdf_volume.dat ... " << flush;
+ app->tsdf_volume_.save ("tsdf_volume.dat", true);
+ cout << "done [" << app->tsdf_volume_.size () << " voxels]" << endl;
+ cout << "Saving TSDF volume cloud to tsdf_cloud.pcd ... " << flush;
+ pcl::io::savePCDFile<pcl::PointXYZI> ("tsdf_cloud.pcd", *app->tsdf_cloud_ptr_, true);
+ cout << "done [" << app->tsdf_cloud_ptr_->size () << " points]" << endl;
+ break;
+
+ default:
+ break;
+ }
+ }
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+template<typename CloudPtr> void
+writeCloudFile (int format, const CloudPtr& cloud_prt)
+{
+ if (format == KinFuApp::PCD_BIN)
+ {
+ cout << "Saving point cloud to 'cloud_bin.pcd' (binary)... " << flush;
+ pcl::io::savePCDFile ("cloud_bin.pcd", *cloud_prt, true);
+ }
+ else
+ if (format == KinFuApp::PCD_ASCII)
+ {
+ cout << "Saving point cloud to 'cloud.pcd' (ASCII)... " << flush;
+ pcl::io::savePCDFile ("cloud.pcd", *cloud_prt, false);
+ }
+ else /* if (format == KinFuApp::PLY) */
+ {
+ cout << "Saving point cloud to 'cloud.ply' (ASCII)... " << flush;
+ pcl::io::savePLYFileASCII ("cloud.ply", *cloud_prt);
+
+ }
+ cout << "Done" << endl;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+writePolygonMeshFile (int format, const pcl::PolygonMesh& mesh)
+{
+ cout << "writePolygonMeshFile mf" << endl;
+
+ if (format == KinFuApp::MESH_PLY)
+ {
+ cout << "Saving mesh to to 'mesh.ply'... " << flush;
+ pcl::io::savePLYFile("mesh.ply", mesh);
+ }
+ else /* if (format == KinFuApp::MESH_VTK) */
+ {
+ cout << "Saving mesh to to 'mesh.vtk'... " << flush;
+ pcl::io::saveVTKFile("mesh.vtk", mesh);
+ }
+ cout << "Done" << endl;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+int
+print_cli_help ()
+{
+ cout << "\nKinfu app concole parameters help:" << endl;
+ cout << " --help, -h : print this message" << endl;
+ cout << " --registration, -r : try to enable registration ( requires source to support this )" << endl;
+ cout << " --current-cloud, -cc : show current frame cloud" << endl;
+ cout << " --save-views, -sv : accumulate scene view and save in the end ( Requires OpenCV. Will cause 'bad_alloc' after some time )" << endl;
+ cout << " --registration, -r : enable registration mode" << endl;
+ cout << " --integrate-colors, -ic : enable color integration mode ( allows to get cloud with colors )" << endl;
+ cout << " -volume_suze <size_in_meters> : define integration volume size" << endl;
+ cout << " -dev <deivce>, -oni <oni_file> : select depth source. Default will be selected if not specified" << endl;
+ cout << "";
+ cout << " For RGBD benchmark (Requires OpenCV):" << endl;
+ cout << " -eval <eval_folder> [-match_file <associations_file_in_the_folder>]" << endl;
+ cout << " For Simuation (Requires pcl::simulation):" << endl;
+ cout << " -plyfile : path to ply file for simulation testing " << endl;
+
+ return 0;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+int
+main (int argc, char* argv[])
+{
+ if (pc::find_switch (argc, argv, "--help") || pc::find_switch (argc, argv, "-h"))
+ return print_cli_help ();
+
+ int device = 0;
+ pc::parse_argument (argc, argv, "-gpu", device);
+ pcl::gpu::setDevice (device);
+ pcl::gpu::printShortCudaDeviceInfo (device);
+
+ if(checkIfPreFermiGPU(device))
+ return cout << endl << "Kinfu is not supported for pre-Fermi GPU architectures, and not built for them by default. Exiting..." << endl, 1;
+
+ CaptureOpenNI capture;
+
+ int openni_device = 0;
+ std::string oni_file, eval_folder, match_file;
+ if (pc::parse_argument (argc, argv, "-dev", openni_device) > 0)
+ {
+ capture.open (openni_device);
+ }
+ else
+ if (pc::parse_argument (argc, argv, "-oni", oni_file) > 0)
+ {
+ capture.open (oni_file);
+ }
+ else
+ if (pc::parse_argument (argc, argv, "-eval", eval_folder) > 0)
+ {
+ //init data source latter
+ pc::parse_argument (argc, argv, "-match_file", match_file);
+ }
+ else
+ {
+ //capture.open (openni_device);
+// capture.open ("/home/mfallon/gcc-4.4/kinfu/skeletonrec.oni");
+// capture.open ("/home/mfallon/gcc-4.4/kinfu/MultipleHands.oni");
+ //capture.open("d:/onis/20111013-224932.oni");
+ //capture.open("d:/onis/reg20111229-180846.oni");
+ //capture.open("d:/onis/white1.oni");
+ //capture.open("/media/Main/onis/20111013-224932.oni");
+ //capture.open("20111013-225218.oni");
+ //capture.open("d:/onis/20111013-224551.oni");
+ //capture.open("d:/onis/20111013-224719.oni");
+ }
+
+ //SIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIM
+ // read model for simulation mode:
+ std::string plyfile;
+ pc::parse_argument (argc, argv, "-plyfile", plyfile);
+ //SIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIM
+
+ float volume_size = 3.f;
+ pc::parse_argument (argc, argv, "-volume_size", volume_size);
+
+ KinFuApp app (capture, volume_size);
+
+ if (pc::parse_argument (argc, argv, "-eval", eval_folder) > 0)
+ app.toggleEvaluationMode(eval_folder, match_file);
+
+ if (pc::find_switch (argc, argv, "--current-cloud") || pc::find_switch (argc, argv, "-cc"))
+ app.initCurrentFrameView ();
+
+ if (pc::find_switch (argc, argv, "--save-views") || pc::find_switch (argc, argv, "-sv"))
+ app.image_view_.accumulate_views_ = true; //will cause bad alloc after some time
+
+ if (pc::find_switch (argc, argv, "--registration") || pc::find_switch (argc, argv, "-r"))
+ app.tryRegistrationInit();
+
+ bool force = pc::find_switch (argc, argv, "-icf");
+ if (force || pc::find_switch (argc, argv, "--integrate-colors") || pc::find_switch (argc, argv, "-ic"))
+ app.toggleColorIntegration(force);
+
+
+ // executing
+ try { app.execute (argc, argv,plyfile); }
+ catch (const std::bad_alloc& /*e*/) { cout << "Bad alloc" << endl; }
+ catch (const std::exception& /*e*/) { cout << "Exception" << endl; }
+
+#ifdef HAVE_OPENCV
+ for (size_t t = 0; t < app.image_view_.views_.size (); ++t)
+ {
+ if (t == 0)
+ {
+ cout << "Saving depth map of first view." << endl;
+ cv::imwrite ("./depthmap_1stview.png", app.image_view_.views_[0]);
+ cout << "Saving sequence of (" << app.image_view_.views_.size () << ") views." << endl;
+ }
+ char buf[4096];
+ sprintf (buf, "./%06d.png", (int)t);
+ cv::imwrite (buf, app.image_view_.views_[t]);
+ printf ("writing: %s\n", buf);
+ }
+#endif
+ return 0;
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#pragma once
+
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/containers/kernel_containers.h>
+
+#include <boost/shared_ptr.hpp>
+#include <string>
+
+#include <pcl/gpu/kinfu/kinfu.h>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ class CaptureOpenNI
+ {
+public:
+ typedef KinfuTracker::PixelRGB RGB;
+
+ enum { PROP_OPENNI_REGISTRATION_ON = 104 };
+
+
+ CaptureOpenNI();
+ CaptureOpenNI(int device);
+ CaptureOpenNI(const std::string& oni_filename);
+
+ void open(int device);
+ void open(const std::string& oni_filename);
+ void release();
+
+ ~CaptureOpenNI();
+
+ bool grab (PtrStepSz<const unsigned short>& depth, PtrStepSz<const RGB>& rgb24);
+
+ //parameters taken from camera/oni
+ float depth_focal_length_VGA;
+ float baseline; // mm
+ int shadow_value;
+ int no_sample_value;
+ double pixelSize; //mm
+
+ unsigned short max_depth; //mm
+
+ bool setRegistration (bool value = false);
+private:
+ struct Impl;
+ boost::shared_ptr<Impl> impl_;
+ void getParams ();
+
+ };
+ }
+};
\ No newline at end of file
--- /dev/null
+% Copyright (c) 2014-, Open Perception, Inc.
+% All rights reserved.
+%
+% Redistribution and use in source and binary forms, with or without
+% modification, are permitted provided that the following conditions
+% are met:
+%
+% * Redistributions of source code must retain the above copyright
+% notice, this list of conditions and the following disclaimer.
+% * Redistributions in binary form must reproduce the above
+% copyright notice, this list of conditions and the following
+% disclaimer in the documentation and/or other materials provided
+% with the distribution.
+% * Neither the name of the copyright holder(s) nor the names of its
+% contributors may be used to endorse or promote products derived
+% from this software without specific prior written permission.
+%
+% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+% "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+% LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+% FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+% COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+% INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+% BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+% LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+% CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+% LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+% ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+% POSSIBILITY OF SUCH DAMAGE.
+% Author: Marco Paladini <marco.paladini@ocado.com>
+
+% sample octave script to load camera poses from file and plot them
+% example usage: run 'pcl_kinfu_app -save_pose camera.csv' to save
+% camera poses in a 'camera.csv' file
+% run octave and cd into the directory where this script resides
+% and call plot_camera_poses('<path-to-camera.csv-file>')
+
+function plot_camera_poses(filename)
+poses=load(filename);
+%% show data on a 2D graph
+h=figure();
+plot(poses,'*-');
+legend('x','y','z','qw','qx','qy','qz');
+
+%% show data as 3D axis
+h=figure();
+for n=1:size(poses,1)
+ t=poses(n,1:3);
+ q=poses(n,4:7);
+ r=q2rot(q);
+ coord(h,r,t);
+end
+octave_axis_equal(h);
+
+%% prevent Octave from quitting if called from the command line
+input('Press enter to continue');
+end
+
+function coord(h,r,t)
+figure(h);
+hold on;
+c={'r','g','b'};
+p=0.1*[1 0 0;0 1 0;0 0 1];
+for n=1:3
+ a=r*p(n,:)';
+ plot3([t(1),t(1)+a(1)], [t(2),t(2)+a(2)], [t(3),t(3)+a(3)], 'color', c{n});
+end
+end
+
+function R=q2rot(q)
+% conversion code from http://en.wikipedia.org/wiki/Rotation_matrix%Quaternion
+Nq = q(1)^2 + q(2)^2 + q(3)^2 + q(4)^2;
+if Nq>0; s=2/Nq; else s=0; end
+X = q(2)*s; Y = q(3)*s; Z = q(4)*s;
+wX = q(1)*X; wY = q(1)*Y; wZ = q(1)*Z;
+xX = q(2)*X; xY = q(2)*Y; xZ = q(2)*Z;
+yY = q(3)*Y; yZ = q(3)*Z; zZ = q(4)*Z;
+R=[ 1.0-(yY+zZ) xY-wZ xZ+wY ;
+ xY+wZ 1.0-(xX+zZ) yZ-wX ;
+ xZ-wY yZ+wX 1.0-(xX+yY) ];
+end
+
+function octave_axis_equal(h)
+% workaround for axis auto not working in 3d
+% tanks http://octave.1599824.n4.nabble.com/axis-equal-help-tp1636701p1636702.html
+figure(h);
+xl = get (gca, 'xlim');
+yl = get (gca, 'ylim');
+zl = get (gca, 'zlim');
+span = max ([diff(xl), diff(yl), diff(zl)]);
+xlim (mean (xl) + span*[-0.5, 0.5])
+ylim (mean (yl) + span*[-0.5, 0.5])
+zlim (mean (zl) + span*[-0.5, 0.5])
+end
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Bernhard Zeisl, (myname.mysurname@inf.ethz.ch)
+ */
+
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/common/file_io.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/visualization/point_cloud_handlers.h>
+
+#include <pcl/gpu/kinfu/kinfu.h>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/containers/initialization.h>
+#include "openni_capture.h"
+
+#include "../src/internal.h"
+
+#include "tsdf_volume.h"
+#include "tsdf_volume.hpp"
+
+using namespace std;
+namespace pc = pcl::console;
+
+typedef pcl::PointXYZ PointT;
+typedef float VoxelT;
+typedef short WeightT;
+
+string cloud_file = "cloud.pcd";
+string volume_file = "tsdf_volume.dat";
+
+double min_trunc_dist = 30.0f;
+
+bool quit = false, save = false;
+bool extract_cloud_volume = false;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+/// DEVICE_VOLUME
+
+/** \brief Class for storing and handling the TSDF Volume on the GPU */
+class DeviceVolume
+{
+public:
+
+ typedef boost::shared_ptr<DeviceVolume> Ptr;
+ typedef boost::shared_ptr<const DeviceVolume> ConstPtr;
+
+ /** \brief Constructor
+ * param[in] volume_size size of the volume in mm
+ * param[in] volume_res volume grid resolution (typically device::VOLUME_X x device::VOLUME_Y x device::VOLUME_Z)
+ */
+ DeviceVolume (const Eigen::Vector3f &volume_size, const Eigen::Vector3i &volume_res)
+ : volume_size_ (volume_size)
+ {
+ // initialize GPU
+ device_volume_.create (volume_res[1] * volume_res[2], volume_res[0]); // (device::VOLUME_Y * device::VOLUME_Z, device::VOLUME_X)
+ pcl::device::initVolume (device_volume_);
+
+ // truncation distance
+ Eigen::Vector3f voxel_size = volume_size.array() / volume_res.array().cast<float>();
+ trunc_dist_ = max ((float)min_trunc_dist, 2.1f * max (voxel_size[0], max (voxel_size[1], voxel_size[2])));
+ };
+
+ /** \brief Creates the TSDF volume on the GPU
+ * param[in] depth depth readings from the sensor
+ * param[in] intr camaera intrinsics
+ */
+ void
+ createFromDepth (const pcl::device::PtrStepSz<const unsigned short> &depth, const pcl::device::Intr &intr);
+
+ /** \brief Downloads the volume from the GPU
+ * param[out] volume volume structure where the data is written to (size needs to be appropriately set beforehand (is checked))
+ */
+ bool
+ getVolume (pcl::TSDFVolume<VoxelT, WeightT>::Ptr &volume);
+
+ /** \brief Generates and returns a point cloud form the implicit surface in the TSDF volume
+ * param[out] cloud point cloud containing the surface
+ */
+ bool
+ getCloud (pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);
+
+private:
+
+ template<class D, class Matx> D&
+ device_cast (Matx& matx)
+ {
+ return (*reinterpret_cast<D*>(matx.data ()));
+ };
+
+ pcl::gpu::DeviceArray2D<int> device_volume_;
+
+ Eigen::Vector3f volume_size_;
+
+ float trunc_dist_;
+
+};
+
+
+void
+DeviceVolume::createFromDepth (const pcl::device::PtrStepSz<const unsigned short> &depth, const pcl::device::Intr &intr)
+{
+ using namespace pcl;
+
+ typedef Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Matrix3frm;
+
+ const int rows = 480;
+ const int cols = 640;
+
+ // scale depth values
+ gpu::DeviceArray2D<float> device_depth_scaled;
+ device_depth_scaled.create (rows, cols);
+
+ // upload depth map on GPU
+ pcl::gpu::KinfuTracker::DepthMap device_depth;
+ device_depth.upload (depth.data, depth.step, depth.rows, depth.cols);
+
+ // initial camera rotation and translation
+ Matrix3frm init_Rcam = Eigen::Matrix3f::Identity ();
+ Eigen::Vector3f init_tcam = volume_size_ * 0.5f - Eigen::Vector3f (0, 0, volume_size_(2)/2 * 1.2f);
+
+ Matrix3frm init_Rcam_inv = init_Rcam.inverse ();
+ device::Mat33& device_Rcam_inv = device_cast<device::Mat33> (init_Rcam_inv);
+ float3& device_tcam = device_cast<float3> (init_tcam);
+
+ // integrate depth values into volume
+ float3 device_volume_size = device_cast<float3> (volume_size_);
+ device::integrateTsdfVolume (device_depth, intr, device_volume_size, device_Rcam_inv, device_tcam, trunc_dist_,
+ device_volume_, device_depth_scaled);
+}
+
+
+bool
+DeviceVolume::getVolume (pcl::TSDFVolume<VoxelT, WeightT>::Ptr &volume)
+{
+ int volume_size = device_volume_.rows() * device_volume_.cols();
+
+ if ((size_t)volume_size != volume->size())
+ {
+ pc::print_error ("Device volume size (%d) and tsdf volume size (%d) don't match. ABORTING!\n", volume_size, volume->size());
+ return false;
+ }
+
+ vector<VoxelT>& volume_vec = volume->volumeWriteable();
+ vector<WeightT>& weights_vec = volume->weightsWriteable();
+
+ device_volume_.download (&volume_vec[0], device_volume_.cols() * sizeof(int));
+
+ #pragma omp parallel for
+ for(int i = 0; i < (int) volume->size(); ++i)
+ {
+ short2 *elem = (short2*)&volume_vec[i];
+ volume_vec[i] = (float)(elem->x)/pcl::device::DIVISOR;
+ weights_vec[i] = (short)(elem->y);
+ }
+
+ return true;
+}
+
+
+bool
+DeviceVolume::getCloud (pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud)
+{
+ const int DEFAULT_VOLUME_CLOUD_BUFFER_SIZE = 10 * 1000 * 1000;
+
+ // point buffer on the device
+ pcl::gpu::DeviceArray<pcl::PointXYZ> device_cloud_buffer (DEFAULT_VOLUME_CLOUD_BUFFER_SIZE);
+
+ // do the extraction
+ float3 device_volume_size = device_cast<float3> (volume_size_);
+ /*size_t size =*/ pcl::device::extractCloud (device_volume_, device_volume_size, device_cloud_buffer);
+
+ // write into point cloud structure
+ device_cloud_buffer.download (cloud->points);
+ cloud->width = (int)cloud->points.size ();
+ cloud->height = 1;
+
+ return true;
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+/// HELPER FUNCTIONS
+
+/** \brief Converts depth and RGB sensor readings into a point cloud
+ * param[in] depth depth data from sensor
+ * param[in] rgb24 color data from sensor
+ * param[in] intr camera intrinsics
+ * param[out] cloud the generated point cloud
+ * \note: position in mm is converted to m
+ * \note: RGB reading not working!
+ */
+//TODO implement correct color reading (how does rgb24 look like?)
+bool
+convertDepthRGBToCloud (const pcl::device::PtrStepSz<const unsigned short> &depth, const pcl::device::PtrStepSz<const pcl::gpu::KinfuTracker::PixelRGB> &rgb24, const pcl::device::Intr &intr,
+ pcl::PointCloud<PointT>::Ptr &cloud)
+{
+ // resize point cloud if it doesn't fit
+ if (depth.rows != (int)cloud->height || depth.cols != (int)cloud->width)
+ cloud = pcl::PointCloud<PointT>::Ptr (new pcl::PointCloud<PointT> (depth.cols, depth.rows));
+
+ // std::cout << "step = " << rgb24.step << std::endl;
+ // std::cout << "elem size = " << rgb24.elem_size << std::endl;
+
+ // iterate over all depth and rgb values
+ for (int y = 0; y < depth.rows; ++y)
+ {
+ // get pointers to the values in one row
+ const unsigned short *depth_row_ptr = depth.ptr(y);
+ // const pcl::gpu::KinfuTracker::RGB *rgb24_row_ptr = rgb24.ptr(y);
+ // const char* rgb24_row_ptr = (const char*) rgb24.ptr(y);
+
+ // iterate over row and store values
+ for (int x = 0; x < depth.cols; ++x)
+ {
+ float u = (x - intr.cx) / intr.fx;
+ float v = (y - intr.cy) / intr.fy;
+
+ PointT &point = cloud->at (x, y);
+
+ point.z = depth_row_ptr[x] / 1000.0f;
+ point.x = u * point.z;
+ point.y = v * point.z;
+
+/* uint8_t r = *(rgb24_row_ptr + 0);
+ uint8_t g = *(rgb24_row_ptr + 1);
+ uint8_t b = *(rgb24_row_ptr + 2);
+ uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
+ point.rgb = *reinterpret_cast<float*>(&rgb);
+
+ point.r = *((const char*)rgb24.data + y*rgb24.step + x*rgb24.elem_size);
+ point.g = *((const char*)rgb24.data + y*rgb24.step + x*rgb24.elem_size + 1);
+ point.b = *((const char*)rgb24.data + y*rgb24.step + x*rgb24.elem_size + 2);
+*/
+ }
+ }
+
+ cloud->is_dense = false;
+
+ return true;
+}
+
+/** \brief Captures data from a sensor and generates a point cloud from it
+ * param[in] capture capturing device object
+ * param[out] depth the depth reading
+ * param[out] rgb24 the color reading
+ * param[out] intr camera intrinsics for this reading
+ * param[out] cloud point cloud generated from the readings
+ */
+bool
+captureCloud (pcl::gpu::CaptureOpenNI &capture,
+ pcl::device::PtrStepSz<const unsigned short> &depth, pcl::device::PtrStepSz<const pcl::gpu::KinfuTracker::PixelRGB> &rgb24,
+ pcl::device::Intr &intr, pcl::PointCloud<PointT>::Ptr &cloud)
+{
+ // capture frame
+ if (!capture.grab (depth, rgb24))
+ {
+ pc::print_error ("Can't capture via sensor.\n");
+ return false;
+ }
+
+ // get intrinsics from capture
+ float f = capture.depth_focal_length_VGA;
+ intr = pcl::device::Intr (f, f, depth.cols/2, depth.rows/2);
+
+ // generate point cloud
+ cloud = pcl::PointCloud<PointT>::Ptr (new pcl::PointCloud<PointT> (depth.cols, depth.rows));
+ if (!convertDepthRGBToCloud (depth, rgb24, intr, cloud))
+ {
+ pc::print_error ("Conversion depth --> cloud was not successful!\n");
+ return false;
+ }
+
+ return true;
+}
+
+/** \brief callback function for the PCLvisualizer */
+void
+keyboard_callback (const pcl::visualization::KeyboardEvent &event, void *cookie)
+{
+ if (event.keyDown())
+ {
+ switch (event.getKeyCode())
+ {
+ case 27:
+ case (int)'q': case (int)'Q':
+ case (int)'e': case (int)'E':
+ cout << "Exiting program" << endl;
+ quit = true;
+ break;
+ case (int)'s': case (int)'S':
+ cout << "Saving volume and cloud" << endl;
+ save = true;
+ break;
+ default:
+ break;
+ }
+ }
+}
+
+/** \brief prints usage information for the executable */
+void
+printUsage (char* argv[])
+{
+ pc::print_error ("usage: %s <options>\n\n", argv[0]);
+
+ pc::print_info (" where options are:\n");
+ pc::print_info (" -cf = cloud filename (default: ");
+ pc::print_value ("%s)", cloud_file.c_str()); pc::print_info (")\n");
+ pc::print_info (" -vf = volume filename (default: ");
+ pc::print_value ("%s", volume_file.c_str()); pc::print_info (")\n");
+ pc::print_info (" -ec = extract cloud from generated volume (default: ");
+ pc::print_value ("0 (false)", volume_file.c_str()); pc::print_info (")\n");
+ pc::print_info (" -td = minimal truncation distance (default: ");
+ pc::print_value ("%f", min_trunc_dist); pc::print_info (")\n");
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+/// MAIN
+
+/** \brief main loop for the program */
+int
+main (int argc, char* argv[])
+{
+ pc::print_info ("Records a 2.5 point cloud (organized = depth map) as TSDF volume. For more information, use: %s -h\n", argv[0]);
+
+ /***
+ * PARSE COMMAND LINE
+ */
+
+ // check for help
+ if (pc::find_argument (argc, argv, "-h") > 0)
+ {
+ pc::print_warn ("Showing help: \n\n");
+ printUsage (argv);
+ return (EXIT_SUCCESS);
+ }
+
+ // parse input cloud file
+ pc::parse_argument (argc, argv, "-cf", cloud_file);
+
+ // pase output volume file
+ pc::parse_argument (argc, argv, "-vf", volume_file);
+
+ // parse options to extract and save cloud from volume
+ pc::parse_argument (argc, argv, "-ec", extract_cloud_volume);
+
+ // parse minimual truncation distance
+ pc::parse_argument (argc, argv, "-td", min_trunc_dist);
+
+ /***
+ * SET UP AND VISUALIZATION
+ */
+
+ pc::print_info (" -------------------- START OF ALGORITHM --------------------\n");
+
+ pcl::gpu::setDevice (0);
+ pcl::gpu::printShortCudaDeviceInfo (0);
+
+ pcl::gpu::CaptureOpenNI capture (0); // first OpenNI device;
+ pcl::device::PtrStepSz<const unsigned short> depth;
+ pcl::device::PtrStepSz<const pcl::gpu::KinfuTracker::PixelRGB> rgb24;
+
+ pcl::PointCloud<PointT>::Ptr cloud; // (new pcl::PointCloud<PointT>);
+ pcl::device::Intr intr;
+
+ // capture first frame
+ if (!captureCloud (capture, depth, rgb24, intr, cloud))
+ return EXIT_FAILURE;
+
+ // start visualizer
+ pcl::visualization::PCLVisualizer visualizer;
+ // pcl::visualization::PointCloudColorHandlerRGBField<PointT> color_handler (cloud);
+ // pcl::visualization::PointCloudColorHandlerCustom<PointT> color_handler (cloud, 0.5, 0.5, 0.5);
+ visualizer.addPointCloud<PointT> (cloud); //, color_handler, "cloud");
+ visualizer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1);
+ visualizer.addCoordinateSystem (1, "global");
+ visualizer.initCameraParameters();
+ visualizer.registerKeyboardCallback (keyboard_callback);
+ visualizer.spinOnce();
+
+ /***
+ * CAPTURING DATA AND GENERATING CLOUD
+ */
+
+ pc::print_highlight ("Capturing data ... \n");
+
+ while (!quit && !save)
+ {
+ // capture data and convert to point cloud
+ if (!captureCloud (capture, depth, rgb24, intr, cloud))
+ return EXIT_FAILURE;
+
+ // update visualization
+ visualizer.updatePointCloud<PointT> (cloud); //, color_handler, "cloud");
+ visualizer.spinOnce();
+ }
+
+ if (quit)
+ return (EXIT_SUCCESS);
+
+
+ /***
+ * GENERATE VOLUME
+ */
+
+ // create volume object
+ pcl::TSDFVolume<VoxelT, WeightT>::Ptr volume (new pcl::TSDFVolume<VoxelT, WeightT>);
+ Eigen::Vector3i resolution (pcl::device::VOLUME_X, pcl::device::VOLUME_Y, pcl::device::VOLUME_Z);
+ Eigen::Vector3f volume_size = Eigen::Vector3f::Constant (3000);
+ volume->resize (resolution, volume_size);
+
+ DeviceVolume::Ptr device_volume (new DeviceVolume (volume->volumeSize(), volume->gridResolution()));
+
+
+ // integrate depth in device volume
+ pc::print_highlight ("Converting depth map to volume ... "); cout << flush;
+ device_volume->createFromDepth (depth, intr);
+
+ // get volume from device
+ if (!device_volume->getVolume (volume))
+ {
+ pc::print_error ("Coudln't get volume from device!\n");
+ return (EXIT_FAILURE);
+ }
+ pc::print_info ("done [%d voxels]\n", volume->size());
+
+
+ // generating TSDF cloud
+ pc::print_highlight ("Generating tsdf volume cloud ... "); cout << flush;
+ pcl::PointCloud<pcl::PointXYZI>::Ptr tsdf_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ volume->convertToTsdfCloud (tsdf_cloud);
+ pc::print_info ("done [%d points]\n", tsdf_cloud->size());
+
+
+ // get cloud from volume
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_volume (new pcl::PointCloud<pcl::PointXYZ>);
+ if (extract_cloud_volume)
+ {
+ pc::print_highlight ("Generating cloud from volume ... "); cout << flush;
+ if (!device_volume->getCloud (cloud_volume))
+ {
+ pc::print_error ("Cloudn't get cloud from device volume!\n");
+ return (EXIT_FAILURE);
+ }
+ pc::print_info ("done [%d points]\n", cloud_volume->size());
+ }
+
+
+ /***
+ * STORE RESULTS
+ */
+
+ pc::print_highlight ("Storing results:\n");
+
+ // point cloud
+ pc::print_info ("Saving captured cloud to "); pc::print_value ("%s", cloud_file.c_str()); pc::print_info (" ... ");
+ if (pcl::io::savePCDFile (cloud_file, *cloud, true) < 0)
+ {
+ cout << endl;
+ pc::print_error ("Cloudn't save the point cloud to file %s.\n", cloud_file.c_str());
+ }
+ else
+ pc::print_info ("done [%d points].\n", cloud->size());
+
+ // volume
+ if (!volume->save (volume_file, true))
+ pc::print_error ("Cloudn't save the volume to file %s.\n", volume_file.c_str());
+
+ // TSDF point cloud
+ string tsdf_cloud_file (pcl::getFilenameWithoutExtension(volume_file) + "_cloud.pcd");
+ pc::print_info ("Saving volume cloud to "); pc::print_value ("%s", tsdf_cloud_file.c_str()); pc::print_info (" ... ");
+ if (pcl::io::savePCDFile (tsdf_cloud_file, *tsdf_cloud, true) < 0)
+ {
+ cout << endl;
+ pc::print_error ("Cloudn't save the volume point cloud to file %s.\n", tsdf_cloud_file.c_str());
+ }
+ else
+ pc::print_info ("done [%d points].\n", tsdf_cloud->size());
+
+ // point cloud from volume
+ if (extract_cloud_volume)
+ {
+ string cloud_volume_file (pcl::getFilenameWithoutExtension(cloud_file) + "_from_volume.pcd");
+ pc::print_info ("Saving cloud from volume to "); pc::print_value ("%s", cloud_volume_file.c_str()); pc::print_info (" ... ");
+ if (pcl::io::savePCDFile (cloud_volume_file, *cloud_volume, true) < 0)
+ {
+ cout << endl;
+ pc::print_error ("Cloudn't save the point cloud to file %s.\n", cloud_volume_file.c_str());
+ }
+ else
+ pc::print_info ("done [%d points].\n", cloud_volume->size());
+ }
+
+ pc::print_info (" -------------------- END OF ALGORITHM --------------------\n");
+
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: tsdf_volume.h 6459 2012-07-18 07:50:37Z dpb $
+ */
+
+
+#ifndef TSDF_VOLUME_H_
+#define TSDF_VOLUME_H_
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/console/print.h>
+
+
+#define DEFAULT_GRID_RES_X 512 // pcl::device::VOLUME_X ( and _Y, _Z)
+#define DEFAULT_GRID_RES_Y 512
+#define DEFAULT_GRID_RES_Z 512
+
+#define DEFAULT_VOLUME_SIZE_X 3000
+#define DEFAULT_VOLUME_SIZE_Y 3000
+#define DEFAULT_VOLUME_SIZE_Z 3000
+
+
+namespace pcl
+{
+ template <typename VoxelT, typename WeightT>
+ class TSDFVolume
+ {
+ public:
+
+ typedef boost::shared_ptr<TSDFVolume<VoxelT, WeightT> > Ptr;
+ typedef boost::shared_ptr<const TSDFVolume<VoxelT, WeightT> > ConstPtr;
+
+ // typedef Eigen::Matrix<VoxelT, Eigen::Dynamic, 1> VoxelTVec;
+ typedef Eigen::VectorXf VoxelTVec;
+
+ /** \brief Structure storing voxel grid resolution, volume size (in mm) and element_size of stored data */
+ struct Header
+ {
+ Eigen::Vector3i resolution;
+ Eigen::Vector3f volume_size;
+ int volume_element_size, weights_element_size;
+
+ Header ()
+ : resolution (0,0,0),
+ volume_size (0,0,0),
+ volume_element_size (sizeof(VoxelT)),
+ weights_element_size (sizeof(WeightT))
+ {};
+
+ Header (const Eigen::Vector3i &res, const Eigen::Vector3f &size)
+ : resolution (res),
+ volume_size (size),
+ volume_element_size (sizeof(VoxelT)),
+ weights_element_size (sizeof(WeightT))
+ {};
+
+ inline size_t
+ getVolumeSize () const { return resolution[0] * resolution[1] * resolution[2]; };
+
+ friend inline std::ostream&
+ operator << (std::ostream& os, const Header& h)
+ {
+ os << "(resolution = " << h.resolution.transpose() << ", volume size = " << h.volume_size.transpose() << ")";
+ return (os);
+ }
+
+public:
+EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ };
+
+ #define DEFAULT_TRANCATION_DISTANCE 30.0f
+
+ /** \brief Camera intrinsics structure
+ */
+ struct Intr
+ {
+ float fx, fy, cx, cy;
+ Intr () {};
+ Intr (float fx_, float fy_, float cx_, float cy_)
+ : fx(fx_), fy(fy_), cx(cx_), cy(cy_) {};
+
+ Intr operator()(int level_index) const
+ {
+ int div = 1 << level_index;
+ return (Intr (fx / div, fy / div, cx / div, cy / div));
+ }
+
+ friend inline std::ostream&
+ operator << (std::ostream& os, const Intr& intr)
+ {
+ os << "([f = " << intr.fx << ", " << intr.fy << "] [cp = " << intr.cx << ", " << intr.cy << "])";
+ return (os);
+ }
+
+ };
+
+
+ ////////////////////////////////////////////////////////////////////////////////////////
+ // Constructors
+
+ /** \brief Default constructor */
+ TSDFVolume ()
+ : volume_ (new std::vector<VoxelT>),
+ weights_ (new std::vector<WeightT>)
+ {};
+
+ /** \brief Constructor loading data from file */
+ TSDFVolume (const std::string &filename)
+ : volume_ (new std::vector<VoxelT>),
+ weights_ (new std::vector<WeightT>)
+ {
+ if (load (filename))
+ std::cout << "done [" << size() << "]" << std::endl;
+ else
+ std::cout << "error!" << std::endl;
+ };
+
+ /** \brief Set the header directly. Useful if directly writing into volume and weights */
+ inline void
+ setHeader (const Eigen::Vector3i &resolution, const Eigen::Vector3f &volume_size) {
+ header_ = Header (resolution, volume_size);
+ if (volume_->size() != this->size())
+ pcl::console::print_warn ("[TSDFVolume::setHeader] Header volume size (%d) doesn't fit underlying data size (%d)", volume_->size(), size());
+ };
+
+ /** \brief Resizes the internal storage and updates the header accordingly */
+ inline void
+ resize (Eigen::Vector3i &grid_resolution, const Eigen::Vector3f& volume_size = Eigen::Vector3f (DEFAULT_VOLUME_SIZE_X, DEFAULT_VOLUME_SIZE_Y, DEFAULT_VOLUME_SIZE_Z)) {
+ int lin_size = grid_resolution[0] * grid_resolution[1] * grid_resolution[2];
+ volume_->resize (lin_size);
+ weights_->resize (lin_size);
+ setHeader (grid_resolution, volume_size);
+ };
+
+ /** \brief Resize internal storage and header to default sizes defined in tsdf_volume.h */
+ inline void
+ resizeDefaultSize () {
+ resize (Eigen::Vector3i (DEFAULT_GRID_RES_X, DEFAULT_GRID_RES_Y, DEFAULT_GRID_RES_Z),
+ Eigen::Vector3f (DEFAULT_VOLUME_SIZE_X, DEFAULT_VOLUME_SIZE_Y, DEFAULT_VOLUME_SIZE_Z));
+ };
+
+ ////////////////////////////////////////////////////////////////////////////////////////
+ // Storage and element access
+
+ /** \brief Loads volume from file */
+ bool
+ load (const std::string &filename, bool binary = true);
+
+ /** \brief Saves volume to file */
+ bool
+ save (const std::string &filename = "tsdf_volume.dat", bool binary = true) const;
+
+ /** \brief Returns overall number of voxels in grid */
+ inline size_t
+ size () const { return header_.getVolumeSize(); };
+
+ /** \brief Returns the volume size in mm */
+ inline const Eigen::Vector3f &
+ volumeSize () const { return header_.volume_size; };
+
+ /** \brief Returns the size of one voxel in mm */
+ inline Eigen::Vector3f
+ voxelSize () const {
+ Eigen::Array3f res = header_.resolution.array().template cast<float>();
+ return header_.volume_size.array() / res;
+ };
+
+ /** \brief Returns the voxel grid resolution */
+ inline const Eigen::Vector3i &
+ gridResolution() const { return header_.resolution; };
+
+ /** \brief Returns constant reference to header */
+ inline const Header &
+ header () const { return header_; };
+
+ /** \brief Returns constant reference to the volume std::vector */
+ inline const std::vector<VoxelT> &
+ volume () const { return *volume_; };
+
+ /** \brief Returns writebale(!) reference to volume */
+ inline std::vector<VoxelT> &
+ volumeWriteable () const { return *volume_; };
+
+ /** \brief Returns constant reference to the weights std::vector */
+ inline const std::vector<WeightT> &
+ weights () const { return *weights_; };
+
+ /** \brief Returns writebale(!) reference to volume */
+ inline std::vector<WeightT> &
+ weightsWriteable () const { return *weights_; };
+
+ ////////////////////////////////////////////////////////////////////////////////////////
+ // Functionality
+
+ /** \brief Converts volume to cloud of TSDF values*/
+ void
+ convertToTsdfCloud (pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) const;
+
+ /** \brief Converts the volume to a surface representation via a point cloud */
+ // void
+ // convertToCloud (pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) const;
+
+ /** \brief Crate Volume from Point Cloud */
+ // template <typename PointT> void
+ // createFromCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, const Intr &intr);
+
+ /** \brief Retunrs the 3D voxel coordinate */
+ template <typename PointT> void
+ getVoxelCoord (const PointT &point, Eigen::Vector3i &voxel_coord) const;
+
+ /** \brief Retunrs the 3D voxel coordinate and point offset wrt. to the voxel center (in mm) */
+ template <typename PointT> void
+ getVoxelCoordAndOffset (const PointT &point, Eigen::Vector3i &voxel_coord, Eigen::Vector3f &offset) const;
+
+ /** extracts voxels in neighborhood of given voxel */
+ bool
+ extractNeighborhood (const Eigen::Vector3i &voxel_coord, int neighborhood_size, VoxelTVec &neighborhood) const;
+
+ /** adds voxel values in local neighborhood */
+ bool
+ addNeighborhood (const Eigen::Vector3i &voxel_coord, int neighborhood_size, const VoxelTVec &neighborhood, WeightT voxel_weight);
+
+ /** averages voxel values by the weight value */
+ void
+ averageValues ();
+
+ /** \brief Returns and index for linear access of the volume and weights */
+ inline int
+ getLinearVoxelIndex (const Eigen::Array3i &indices) const {
+ return indices(0) + indices(1) * header_.resolution[0] + indices(2) * header_.resolution[0] * header_.resolution[1];
+ }
+
+ /** \brief Returns a vector of linear indices for voxel coordinates given in 3xn matrix */
+ inline Eigen::VectorXi
+ getLinearVoxelIndinces (const Eigen::Matrix<int, 3, Eigen::Dynamic> &indices_matrix) const {
+ return (Eigen::RowVector3i (1, header_.resolution[0], header_.resolution[0] * header_.resolution[1]) * indices_matrix).transpose();
+ }
+
+ private:
+
+ ////////////////////////////////////////////////////////////////////////////////////////
+ // Private functions and members
+
+ // void
+ // scaleDepth (const Eigen::MatrixXf &depth, Eigen::MatrixXf &depth_scaled, const Intr &intr) const;
+
+ // void
+ // integrateVolume (const Eigen::MatrixXf &depth_scaled, float tranc_dist, const Eigen::Matrix3f &R_inv, const Eigen::Vector3f &t, const Intr &intr);
+
+ typedef boost::shared_ptr<std::vector<VoxelT> > VolumePtr;
+ typedef boost::shared_ptr<std::vector<WeightT> > WeightsPtr;
+
+ Header header_;
+ VolumePtr volume_;
+ WeightsPtr weights_;
+public:
+EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ };
+
+}
+
+#endif /* TSDF_VOLUME_H_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ */
+
+#ifndef TSDF_VOLUME_HPP_
+#define TSDF_VOLUME_HPP_
+
+#include "tsdf_volume.h"
+
+#include <fstream>
+
+
+template <typename VoxelT, typename WeightT> bool
+pcl::TSDFVolume<VoxelT, WeightT>::load (const std::string &filename, bool binary)
+{
+ pcl::console::print_info ("Loading TSDF volume from "); pcl::console::print_value ("%s ... ", filename.c_str());
+ std::cout << std::flush;
+
+ std::ifstream file (filename.c_str());
+
+ if (file.is_open())
+ {
+ if (binary)
+ {
+ // read HEADER
+ file.read ((char*) &header_, sizeof (Header));
+ /* file.read (&header_.resolution, sizeof(Eigen::Array3i));
+ file.read (&header_.volume_size, sizeof(Eigen::Vector3f));
+ file.read (&header_.volume_element_size, sizeof(int));
+ file.read (&header_.weights_element_size, sizeof(int)); */
+
+ // check if element size fits to data
+ if (header_.volume_element_size != sizeof(VoxelT))
+ {
+ pcl::console::print_error ("[TSDFVolume::load] Error: Given volume element size (%d) doesn't fit data (%d)", sizeof(VoxelT), header_.volume_element_size);
+ return false;
+ }
+ if ( header_.weights_element_size != sizeof(WeightT))
+ {
+ pcl::console::print_error ("[TSDFVolume::load] Error: Given weights element size (%d) doesn't fit data (%d)", sizeof(WeightT), header_.weights_element_size);
+ return false;
+ }
+
+ // read DATA
+ int num_elements = header_.getVolumeSize();
+ volume_->resize (num_elements);
+ weights_->resize (num_elements);
+ file.read ((char*) &(*volume_)[0], num_elements * sizeof(VoxelT));
+ file.read ((char*) &(*weights_)[0], num_elements * sizeof(WeightT));
+ }
+ else
+ {
+ pcl::console::print_error ("[TSDFVolume::load] Error: ASCII loading not implemented.\n");
+ }
+
+ file.close ();
+ }
+ else
+ {
+ pcl::console::print_error ("[TSDFVolume::load] Error: Cloudn't read file %s.\n", filename.c_str());
+ return false;
+ }
+
+ const Eigen::Vector3i &res = this->gridResolution();
+ pcl::console::print_info ("done [%d voxels, res %dx%dx%d]\n", this->size(), res[0], res[1], res[2]);
+
+ return true;
+}
+
+
+template <typename VoxelT, typename WeightT> bool
+pcl::TSDFVolume<VoxelT, WeightT>::save (const std::string &filename, bool binary) const
+{
+ pcl::console::print_info ("Saving TSDF volume to "); pcl::console::print_value ("%s ... ", filename.c_str());
+ std::cout << std::flush;
+
+ std::ofstream file (filename.c_str(), binary ? std::ios_base::binary : std::ios_base::out);
+
+ if (file.is_open())
+ {
+ if (binary)
+ {
+ // HEADER
+ // write resolution and size of volume
+ file.write ((char*) &header_, sizeof (Header));
+ /* file.write ((char*) &header_.resolution, sizeof(Eigen::Vector3i));
+ file.write ((char*) &header_.volume_size, sizeof(Eigen::Vector3f));
+ // write element size
+ int volume_element_size = sizeof(VolumeT);
+ file.write ((char*) &volume_element_size, sizeof(int));
+ int weights_element_size = sizeof(WeightT);
+ file.write ((char*) &weights_element_size, sizeof(int)); */
+
+ // DATA
+ // write data
+ file.write ((char*) &(volume_->at(0)), volume_->size()*sizeof(VoxelT));
+ file.write ((char*) &(weights_->at(0)), weights_->size()*sizeof(WeightT));
+ }
+ else
+ {
+ // write resolution and size of volume and element size
+ file << header_.resolution(0) << " " << header_.resolution(1) << " " << header_.resolution(2) << std::endl;
+ file << header_.volume_size(0) << " " << header_.volume_size(1) << " " << header_.volume_size(2) << std::endl;
+ file << sizeof (VoxelT) << " " << sizeof(WeightT) << std::endl;
+
+ // write data
+ for (typename std::vector<VoxelT>::const_iterator iter = volume_->begin(); iter != volume_->end(); ++iter)
+ file << *iter << std::endl;
+ }
+
+ file.close();
+ }
+ else
+ {
+ pcl::console::print_error ("[saveTsdfVolume] Error: Couldn't open file %s.\n", filename.c_str());
+ return false;
+ }
+
+ pcl::console::print_info ("done [%d voxels]\n", this->size());
+
+ return true;
+}
+
+
+template <typename VoxelT, typename WeightT> void
+pcl::TSDFVolume<VoxelT, WeightT>::convertToTsdfCloud (pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) const
+{
+ int sx = header_.resolution(0);
+ int sy = header_.resolution(1);
+ int sz = header_.resolution(2);
+
+ const int step = 2;
+ const int cloud_size = header_.getVolumeSize() / (step*step*step);
+
+ cloud->clear();
+ cloud->reserve (std::min (cloud_size/10, 500000));
+
+ int volume_idx = 0, cloud_idx = 0;
+//#pragma omp parallel for // if used, increment over idx not possible! use index calculation
+ for (int z = 0; z < sz; z+=step)
+ for (int y = 0; y < sy; y+=step)
+ for (int x = 0; x < sx; x+=step, ++cloud_idx)
+ {
+ volume_idx = sx*sy*z + sx*y + x;
+ // pcl::PointXYZI &point = cloud->points[cloud_idx];
+
+ if (weights_->at(volume_idx) == 0 || volume_->at(volume_idx) > 0.98 )
+ continue;
+
+ pcl::PointXYZI point;
+ point.x = x; point.y = y; point.z = z;//*64;
+ point.intensity = volume_->at(volume_idx);
+ cloud->push_back (point);
+ }
+
+ // cloud->width = cloud_size;
+ // cloud->height = 1;
+}
+
+
+template <typename VoxelT, typename WeightT> template <typename PointT> void
+pcl::TSDFVolume<VoxelT, WeightT>::getVoxelCoord (const PointT &point, Eigen::Vector3i &coord) const
+{
+ static Eigen::Array3f voxel_size = voxelSize().array();
+
+ // point coordinates in world coordinate frame and voxel coordinates
+ Eigen::Array3f point_coord (point.x, point.y, point.z);
+ Eigen::Array3f voxel_coord = (point_coord / voxel_size) - 0.5f; // 0.5f offset due to voxel center vs grid
+ coord(0) = round(voxel_coord(0));
+ coord(1) = round(voxel_coord(1));
+ coord(2) = round(voxel_coord(2));
+}
+
+
+/** \brief Retunrs the 3D voxel coordinate and point offset wrt. to the voxel center (in mm) */
+template <typename VoxelT, typename WeightT> template <typename PointT> void
+pcl::TSDFVolume<VoxelT, WeightT>::getVoxelCoordAndOffset (const PointT &point,
+ Eigen::Vector3i &coord, Eigen::Vector3f &offset) const
+{
+ static Eigen::Array3f voxel_size = voxelSize().array();
+
+ // point coordinates in world coordinate frame and voxel coordinates
+ Eigen::Array3f point_coord (point.x, point.y, point.z);
+ Eigen::Array3f voxel_coord = (point_coord / voxel_size) - 0.5f; // 0.5f offset due to voxel center vs grid
+ coord(0) = round(voxel_coord(0));
+ coord(1) = round(voxel_coord(1));
+ coord(2) = round(voxel_coord(2));
+
+ // offset of point wrt. to voxel center
+ offset = (voxel_coord - coord.cast<float>().array() * voxel_size).matrix();
+}
+
+
+template <typename VoxelT, typename WeightT> bool
+pcl::TSDFVolume<VoxelT, WeightT>::extractNeighborhood (const Eigen::Vector3i &voxel_coord, int neighborhood_size,
+ VoxelTVec &neighborhood) const
+{
+ // point_index is at the center of a cube of scale_ x scale_ x scale_ voxels
+ int shift = (neighborhood_size - 1) / 2;
+ Eigen::Vector3i min_index = voxel_coord.array() - shift;
+ Eigen::Vector3i max_index = voxel_coord.array() + shift;
+
+ // check that index is within range
+ if (getLinearVoxelIndex(min_index) < 0 || getLinearVoxelIndex(max_index) >= (int)size())
+ {
+ pcl::console::print_info ("[extractNeighborhood] Skipping voxel with coord (%d, %d, %d).\n", voxel_coord(0), voxel_coord(1), voxel_coord(2));
+ return false;
+ }
+
+ static const int descriptor_size = neighborhood_size*neighborhood_size*neighborhood_size;
+ neighborhood.resize (descriptor_size);
+
+ const Eigen::RowVector3i offset_vector (1, neighborhood_size, neighborhood_size*neighborhood_size);
+
+ // loop over all voxels in 3D neighborhood
+ #pragma omp parallel for
+ for (int z = min_index(2); z <= max_index(2); ++z)
+ {
+ for (int y = min_index(1); y <= max_index(1); ++y)
+ {
+ for (int x = min_index(0); x <= max_index(0); ++x)
+ {
+ // linear voxel index in volume and index in descriptor vector
+ Eigen::Vector3i point (x,y,z);
+ int volume_idx = getLinearVoxelIndex (point);
+ int descr_idx = offset_vector * (point - min_index);
+
+/* std::cout << "linear index " << volume_idx << std::endl;
+ std::cout << "weight " << weights_->at (volume_idx) << std::endl;
+ std::cout << "volume " << volume_->at (volume_idx) << std::endl;
+ std::cout << "descr " << neighborhood.rows() << "x" << neighborhood.cols() << ", val = " << neighborhood << std::endl;
+ std::cout << "descr index = " << descr_idx << std::endl;
+*/
+ // get the TSDF value and store as descriptor entry
+ if (weights_->at (volume_idx) != 0)
+ neighborhood (descr_idx) = volume_->at (volume_idx);
+ else
+ neighborhood (descr_idx) = -1.0; // if never visited we assume inside of object (outside captured and thus filled with positive values)
+ }
+ }
+ }
+
+ return true;
+}
+
+
+template <typename VoxelT, typename WeightT> bool
+pcl::TSDFVolume<VoxelT, WeightT>::addNeighborhood (const Eigen::Vector3i &voxel_coord, int neighborhood_size,
+ const VoxelTVec &neighborhood, WeightT voxel_weight)
+{
+ // point_index is at the center of a cube of scale_ x scale_ x scale_ voxels
+ int shift = (neighborhood_size - 1) / 2;
+ Eigen::Vector3i min_index = voxel_coord.array() - shift;
+ Eigen::Vector3i max_index = voxel_coord.array() + shift;
+
+ // check that index is within range
+ if (getLinearVoxelIndex(min_index) < 0 || getLinearVoxelIndex(max_index) >= (int)size())
+ {
+ pcl::console::print_info ("[addNeighborhood] Skipping voxel with coord (%d, %d, %d).\n", voxel_coord(0), voxel_coord(1), voxel_coord(2));
+ return false;
+ }
+
+ // static const int descriptor_size = neighborhood_size*neighborhood_size*neighborhood_size;
+ const Eigen::RowVector3i offset_vector (1, neighborhood_size, neighborhood_size*neighborhood_size);
+
+ Eigen::Vector3i index = min_index;
+ // loop over all voxels in 3D neighborhood
+ #pragma omp parallel for
+ for (int z = min_index(2); z <= max_index(2); ++z)
+ {
+ for (int y = min_index(1); y <= max_index(1); ++y)
+ {
+ for (int x = min_index(0); x <= max_index(0); ++x)
+ {
+ // linear voxel index in volume and index in descriptor vector
+ Eigen::Vector3i point (x,y,z);
+ int volume_idx = getLinearVoxelIndex (point);
+ int descr_idx = offset_vector * (point - min_index);
+
+ // add the descriptor entry to the volume
+ VoxelT &voxel = volume_->at (volume_idx);
+ WeightT &weight = weights_->at (volume_idx);
+
+ // TODO check that this simple lock works correctly!!
+ #pragma omp atomic
+ voxel += neighborhood (descr_idx);
+
+ #pragma omp atomic
+ weight += voxel_weight;
+ }
+ }
+ }
+
+ return true;
+}
+
+
+template <typename VoxelT, typename WeightT> void
+pcl::TSDFVolume<VoxelT, WeightT>::averageValues ()
+{
+ #pragma omp parallel for
+ for (size_t i = 0; i < volume_->size(); ++i)
+ {
+ WeightT &w = weights_->at(i);
+ if (w > 0.0)
+ {
+ volume_->at(i) /= w;
+ w = 1.0;
+ }
+ }
+}
+
+
+/*template <typename VoxelT, typename WeightT> template <typename PointT> void
+pcl::TSDFVolume<VoxelT, WeightT>::createFromCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, const Intr &intr)
+{
+ // get depth map from cloud
+ float bad_point = std::numeric_limits<float>::quiet_NaN ();
+ // Eigen::MatrixXf depth = Eigen::MatrixXf::Constant(cloud_->height, cloud_->width, bad_point);
+ Eigen::MatrixXf depth (cloud->height, cloud->width);
+
+ // determine max and min value
+ float min = 3000.0, max = 0.0;
+ for (int x = 0; x < cloud->width; ++x)
+ for (int y = 0; y < cloud->height; ++y)
+ {
+ depth(y,x) = cloud->at(x,y).z;
+ if (!isnan(depth(y,x)))
+ {
+ if (depth(y,x) > max) max = depth(y,x);
+ if (depth(y,x) < min) min = depth(y,x);
+ }
+ }
+
+ std::cout << " depth size = " << depth.rows() << "x" << depth.cols() << ", min/max = " << min << "/" << max << std::endl;
+
+
+ // BOOST_FOREACH (const PointT &p, cloud->points)
+ typename pcl::PointCloud<PointT>::const_iterator iter = cloud->begin();
+ for (; iter != cloud_>end(); ++iter)
+ {
+ const PointT &p = *iter;
+
+ std::cout << "orig point = " << p << std::endl;
+
+ Eigen::Array2f point (intr.fx * p.x + intr.cx * p.z,
+ intr.fx * p.y + intr.cy * p.z);
+ Eigen::Array2i pixel (round(point(0))/p.z, round(point(1))/p.z);
+
+ std::cout << "point = " << point.transpose() << std::endl;
+ std::cout << "pixel = " << pixel.transpose() << std::endl;
+
+ depth (pixel(1), pixel(0)) = p.z;
+ }
+
+ std::cout << " scaling depth map" << std::endl;
+ // scale depth map
+ Eigen::MatrixXf depth_scaled;
+ // scaleDepth (depth, depth_scaled, intr);
+ // TODO find out what this should do! projection on unit sphere?!
+ depth_scaled = depth;
+
+ // generate volume
+ // std::cout << " generating volume" << std::endl;
+ // resizeDefaultSize();
+ Eigen::Vector3f volume_size = volumeSize();
+ Eigen::Vector3f voxel_size = voxelSize();
+
+ float tranc_dist = std::max (DEFAULT_TRANCATION_DISTANCE, 2.1f * voxel_size.maxCoeff());
+
+ Eigen::Matrix3f R_inv_init = Eigen::Matrix3f::Identity();
+ Eigen::Vector3f t_init = volume_size * 0.5f - Eigen::Vector3f (0, 0, volume_size(2)/2.0f * 1.2f);
+ // std::cout << "initial pose: R_inv = " << R_inv_init << ", t_init = " << t_init.transpose() << std::endl;
+
+ std::cout << " integrating values" << std::endl;
+ integrateVolume (depth_scaled, tranc_dist, R_inv_init, t_init, intr);
+}*/
+
+
+/*template <typename VoxelT, typename WeightT> void
+pcl::TSDFVolume<VoxelT, WeightT>::scaleDepth (const Eigen::MatrixXf &depth, Eigen::MatrixXf &depth_scaled, const Intr &intr) const
+{
+ // function ported from KinFu GPU code
+ depth_scaled.resizeLike (depth);
+
+ float min = 3000.0, max = 0.0;
+ // loop over depth image
+ for (int x = 0; x < depth.cols(); ++x)
+ for (int y = 0; y < depth.rows(); ++y)
+ {
+ int Dp = depth(y,x);
+
+ float xl = (x - intr.cx) / intr.fx;
+ float yl = (y - intr.cy) / intr.fy;
+ float lambda = sqrtf (xl * xl + yl * yl + 1);
+
+ depth_scaled(y,x) = Dp * lambda;
+
+ if (!isnan(depth_scaled(y,x)))
+ {
+ if (depth_scaled(y,x) > max) max = depth_scaled(y,x);
+ if (depth_scaled(y,x) < min) min = depth_scaled(y,x);
+ }
+ }
+
+ std::cout << "depth_scaled size = " << depth_scaled.rows() << "x" << depth_scaled.cols() << ", min/max = " << min << "/" << max << std::endl;
+}*/
+
+
+/*template <typename VoxelT, typename WeightT> void
+pcl::TSDFVolume<VoxelT, WeightT>::integrateVolume (const Eigen::MatrixXf &depth_scaled,
+ float tranc_dist,
+ const Eigen::Matrix3f &R_inv,
+ const Eigen::Vector3f &t,
+ const Intr &intr)
+{
+ Eigen::Array3f voxel_size = voxelSize();
+ Eigen::Array3i volume_res = gridResolution();
+ Eigen::Array3f intr_arr (intr.fx, intr.fy, 1.0f);
+ Eigen::Array3i voxel_coord (0,0,0);
+
+ // loop over grid in X and Y dimension
+ #pragma omp parallel for
+ // for (voxel_coord(0) = 0; voxel_coord(0) < volume_res(0); ++voxel_coord(0))
+ for (int i = 0; i < volume_res(0); ++i)
+ {
+ voxel_coord(0) = i;
+
+ // std::stringstream ss;
+ // ss << voxel_coord(0) << "/" << volume_res(0) << " ";
+ // std::cout << ss.str();
+ std::cout << ". " << std::flush;
+
+ for (voxel_coord(1) = 0; voxel_coord(1) < volume_res(1); ++voxel_coord(1))
+ {
+ voxel_coord(2) = 0;
+ // points at depth 0, shifted by t
+ Eigen::Vector3f v_g = (voxel_coord.cast<float>() + 0.5f) * voxel_size - t.array();
+ float v_g_part_norm = v_g(0)*v_g(0) + v_g(1)*v_g(1);
+
+ // rays in 3d
+ Eigen::Vector3f v = (R_inv * v_g).array() * intr_arr;
+
+ float z_scaled = 0;
+
+ Eigen::Array3f R_inv_z_scaled = R_inv.col(2).array() * voxel_size(2) * intr_arr;
+
+ float tranc_dist_inv = 1.0f / tranc_dist;
+
+ // loop over depth values
+ for (voxel_coord(2) = 0; voxel_coord(2) < volume_res(2); ++voxel_coord(2),
+ v_g(2) += voxel_size(2),
+ z_scaled += voxel_size(2),
+ v(0) += R_inv_z_scaled(0),
+ v(1) += R_inv_z_scaled(1))
+ {
+ float inv_z = 1.0f / (v(2) + R_inv(2,2) * z_scaled);
+
+ // std::cout << "z = " << voxel_coord(2) << ", inv_z = " << inv_z << std::endl;
+
+ if (inv_z < 0)
+ continue;
+
+ // project to camera
+ Eigen::Array2i img_coord (round(v(0) * inv_z + intr.cx),
+ round(v(1) * inv_z + intr.cy));
+
+ // std::cout << "img_coord = " << img_coord.transpose() << std::endl;
+
+ if (img_coord(0) >= 0 && img_coord(1) >= 0 && img_coord(0) < depth_scaled.cols() && img_coord(1) < depth_scaled.rows()) //6
+ {
+ float Dp_scaled = depth_scaled(img_coord(1), img_coord(0));
+
+ // signed distance function
+ float sdf = Dp_scaled - sqrtf (v_g(2) * v_g(2) + v_g_part_norm);
+
+ if (Dp_scaled != 0 && sdf >= -tranc_dist)
+ {
+ // get truncated distance function value
+ float tsdf = fmin (1.0f, sdf * tranc_dist_inv);
+
+ // add values to volume
+ int idx = getLinearVoxelIndex (voxel_coord);
+ VoxelT &tsdf_val = volume_->at(idx);
+ short &weight = weights_->at(idx);
+ tsdf_val = tsdf_val * weight + tsdf;
+ weight += 1;
+ }
+ }
+ } // loop over depths
+ }
+ }
+ std::cout << std::endl;
+}*/
+
+#define PCL_INSTANTIATE_TSDFVolume(VT,WT) template class PCL_EXPORTS pcl::reconstruction::TSDFVolume<VT,WT>;
+
+#endif /* TSDF_VOLUME_HPP_ */
--- /dev/null
+set(SUBSYS_NAME gpu_kinfu_large_scale)
+set(SUBSYS_PATH gpu/kinfu_large_scale)
+set(SUBSYS_DESC "Kinect Fusion implementation, with volume shifting")
+set(SUBSYS_DEPS common visualization io gpu_containers gpu_utils geometry search octree filters kdtree features surface)
+
+set(build FALSE)
+
+# OpenNI found?
+if(NOT WITH_OPENNI)
+ set(DEFAULT FALSE)
+ set(REASON "OpenNI was not found or was disabled by the user.")
+else()
+ set(DEFAULT TRUE)
+ set(REASON)
+endif()
+
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}")
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS})
+PCL_SET_SUBSYS_INCLUDE_DIR("${SUBSYS_NAME}" "${SUBSYS_PATH}")
+mark_as_advanced("BUILD_${SUBSYS_NAME}")
+
+if (build)
+ REMOVE_VTK_DEFINITIONS()
+ FILE(GLOB incs include/pcl/gpu/kinfu_large_scale/*.h*)
+ FILE(GLOB impl_incs include/pcl/gpu/kinfu_large_scale/impl/*.h*)
+ FILE(GLOB srcs src/*.cpp src/*.h*)
+ FILE(GLOB cuda src/cuda/*.cu src/cuda/*.h*)
+ #FILE(GLOB tsdf src/cuda/tsdf_volume.cu)
+ #FILE(GLOB est src/cuda/estimate_tranform.cu)
+
+ source_group("Source Files\\cuda" FILES ${cuda} )
+ source_group("Source Files" FILES ${srcs} )
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include" "${CMAKE_CURRENT_SOURCE_DIR}/src" ${CUDA_INCLUDE_DIRS})
+
+ if (UNIX OR APPLE)
+ set (CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "-Xcompiler;-fPIC;")
+ endif()
+
+ if(NOT UNIX OR APPLE)
+ add_definitions(-DPCLAPI_EXPORTS)
+ endif()
+
+ #set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "-gencode;arch=compute_11,code=compute_11;-gencode;arch=compute_12,code=compute_12")
+
+ if(TRUE)
+ #list(REMOVE_ITEM cuda ${est})
+ #CUDA_COMPILE(est_objs ${est})
+
+ set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "--ftz=true;--prec-div=false;--prec-sqrt=false")
+ CUDA_COMPILE(cuda_objs ${cuda})
+
+ #LIST(APPEND cuda ${est})
+ #LIST(APPEND cuda_objs ${est_objs})
+
+ else()
+ list(REMOVE_ITEM cuda ${tsdf})
+ CUDA_COMPILE(cuda_objs ${cuda})
+ set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "--ftz=true;--prec-div=false;--prec-sqrt=false")
+ CUDA_COMPILE(tsdf_obj ${tsdf})
+
+ LIST(APPEND cuda ${tsdf})
+ LIST(APPEND cuda_objs ${tsdf_obj})
+ endif()
+
+ PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs} ${cuda} ${cuda_objs})
+ target_link_libraries("${LIB_NAME}" pcl_common pcl_io pcl_gpu_utils pcl_gpu_containers pcl_gpu_octree pcl_octree pcl_filters)
+
+ set(EXT_DEPS "")
+ #set(EXT_DEPS CUDA)
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "")
+
+ # Install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}" ${incs})
+
+ add_subdirectory(test)
+ add_subdirectory(tools)
+endif()
+
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_KINFU_COLOR_VOLUME_H_
+#define PCL_KINFU_COLOR_VOLUME_H_
+
+#include <pcl/pcl_macros.h>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <Eigen/Core>
+
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace kinfuLS
+ {
+ class TsdfVolume;
+
+ /** \brief ColorVolume class
+ * \author Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+ class PCL_EXPORTS ColorVolume
+ {
+ public:
+ typedef PointXYZ PointType;
+ typedef boost::shared_ptr<ColorVolume> Ptr;
+
+ /** \brief Constructor
+ * \param[in] tsdf tsdf volume to get parameters from
+ * \param[in] max_weight max weight for running average. Can be less than 255. Negative means default.
+ */
+ ColorVolume(const TsdfVolume& tsdf, int max_weight = -1);
+
+ /** \brief Desctructor */
+ ~ColorVolume();
+
+ /** \brief Resets color volume to uninitialized state */
+ void
+ reset();
+
+ /** \brief Returns running average length */
+ int
+ getMaxWeight() const;
+
+ /** \brief Returns container with color volume in GPU memory */
+ DeviceArray2D<int>
+ data() const;
+
+ /** \brief Computes colors from color volume
+ * \param[in] cloud Points for which colors are to be computed.
+ * \param[out] colors output array for colors
+ */
+ void
+ fetchColors (const DeviceArray<PointType>& cloud, DeviceArray<RGB>& colors) const;
+
+ private:
+ /** \brief Volume resolution */
+ Eigen::Vector3i resolution_;
+
+ /** \brief Volume size in meters */
+ Eigen::Vector3f volume_size_;
+
+ /** \brief Length of running average */
+ int max_weight_;
+
+ /** \brief color volume data */
+ DeviceArray2D<int> color_volume_;
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+ }
+ }
+}
+
+#endif /* PCL_KINFU_COLOR_VOLUME_H_ */
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+
+#ifndef PCL_CYCLICAL_BUFFER_IMPL_H_
+#define PCL_CYCLICAL_BUFFER_IMPL_H_
+
+#include <pcl/point_types.h>
+#include <pcl/gpu/kinfu_large_scale/tsdf_volume.h>
+#include <pcl/gpu/kinfu_large_scale/tsdf_buffer.h>
+#include <Eigen/Core>
+//#include <boost/graph/buffer_concepts.hpp>
+#include <cuda_runtime.h>
+#include <pcl/gpu/kinfu_large_scale/point_intensity.h>
+
+#include <pcl/gpu/kinfu_large_scale/world_model.h>
+
+
+#include <pcl/io/pcd_io.h>
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace kinfuLS
+ {
+
+ /** \brief CyclicalBuffer implements a cyclical TSDF buffer.
+ * The class offers a simple interface, by handling shifts and maintaining the world autonomously.
+ * \author Raphael Favier, Francisco Heredia
+ */
+ class PCL_EXPORTS CyclicalBuffer
+ {
+ public:
+
+
+ /** \brief Constructor for a cubic CyclicalBuffer.
+ * \param[in] distance_threshold distance between cube center and target point at which we decide to shift.
+ * \param[in] cube_size physical size (in meters) of the volume (here, a cube) represented by the TSDF buffer.
+ * \param[in] nb_voxels_per_axis number of voxels per axis of the volume represented by the TSDF buffer.
+ */
+ CyclicalBuffer (const double distance_threshold, const double cube_size = 3.f, const int nb_voxels_per_axis = 512)
+ {
+ distance_threshold_ = distance_threshold;
+ buffer_.volume_size.x = cube_size;
+ buffer_.volume_size.y = cube_size;
+ buffer_.volume_size.z = cube_size;
+ buffer_.voxels_size.x = nb_voxels_per_axis;
+ buffer_.voxels_size.y = nb_voxels_per_axis;
+ buffer_.voxels_size.z = nb_voxels_per_axis;
+ }
+
+
+ /** \brief Constructor for a non-cubic CyclicalBuffer.
+ * \param[in] distance_threshold distance between cube center and target point at which we decide to shift.
+ * \param[in] volume_size_x physical size (in meters) of the volume, X axis.
+ * \param[in] volume_size_y physical size (in meters) of the volume, Y axis.
+ * \param[in] volume_size_z physical size (in meters) of the volume, Z axis.
+ * \param[in] nb_voxels_x number of voxels for X axis of the volume represented by the TSDF buffer.
+ * \param[in] nb_voxels_y number of voxels for Y axis of the volume represented by the TSDF buffer.
+ * \param[in] nb_voxels_z number of voxels for Z axis of the volume represented by the TSDF buffer.
+ */
+ CyclicalBuffer (const double distance_threshold, const double volume_size_x, const double volume_size_y, const double volume_size_z, const int nb_voxels_x, const int nb_voxels_y, const int nb_voxels_z)
+ {
+ distance_threshold_ = distance_threshold;
+ buffer_.volume_size.x = volume_size_x;
+ buffer_.volume_size.y = volume_size_y;
+ buffer_.volume_size.z = volume_size_z;
+ buffer_.voxels_size.x = nb_voxels_x;
+ buffer_.voxels_size.y = nb_voxels_y;
+ buffer_.voxels_size.z = nb_voxels_z;
+ }
+
+ /** \brief Check if shifting needs to be performed, returns true if so.
+ Shifting is considered needed if the target point is farther than distance_treshold_.
+ The target point is located at distance_camera_point on the local Z axis of the camera.
+ * \param[in] volume pointer to the TSDFVolume living in GPU
+ * \param[in] cam_pose global pose of the camera in the world
+ * \param[in] distance_camera_target distance from the camera's origin to the target point
+ * \param[in] perform_shift if set to false, shifting is not performed. The function will return true if shifting is needed.
+ * \param[in] last_shift if set to true, the whole cube will be shifted. This is used to push the whole cube to the world model.
+ * \param[in] force_shift if set to true, shifting is forced.
+ * \return true is the cube needs to be or has been shifted.
+ */
+ bool checkForShift (const TsdfVolume::Ptr volume, const Eigen::Affine3f &cam_pose, const double distance_camera_target, const bool perform_shift = true, const bool last_shift = false, const bool force_shift = false);
+
+ /** \brief Perform shifting operations:
+ Compute offsets.
+ Extract current slice from TSDF buffer.
+ Extract existing data from world.
+ Clear shifted slice in TSDF buffer.
+ Push existing data into TSDF buffer.
+ Update rolling buffer
+ Update world model.
+ * \param[in] volume pointer to the TSDFVolume living in GPU
+ * \param[in] target_point target point around which the new cube will be centered
+ * \param[in] last_shift if set to true, the whole cube will be shifted. This is used to push the whole cube to the world model.
+ */
+ void performShift (const TsdfVolume::Ptr volume, const pcl::PointXYZ &target_point, const bool last_shift = false);
+
+ /** \brief Sets the distance threshold between cube's center and target point that triggers a shift.
+ * \param[in] threshold the distance in meters at which to trigger shift.
+ */
+ void setDistanceThreshold (const double threshold)
+ {
+ distance_threshold_ = threshold;
+ // PCL_INFO ("Shifting threshold set to %f meters.\n", distance_threshold_);
+ }
+
+ /** \brief Returns the distance threshold between cube's center and target point that triggers a shift. */
+ float getDistanceThreshold () { return (distance_threshold_); }
+
+ /** \brief get a pointer to the tsdf_buffer structure.
+ * \return a pointer to the tsdf_buffer used by cyclical buffer object.
+ */
+ tsdf_buffer* getBuffer () { return (&buffer_); }
+
+ /** \brief Set the physical size represented by the default TSDF volume.
+ * \param[in] size_x size of the volume on X axis, in meters.
+ * \param[in] size_y size of the volume on Y axis, in meters.
+ * \param[in] size_z size of the volume on Z axis, in meters.
+ */
+ void setVolumeSize (const double size_x, const double size_y, const double size_z)
+ {
+ buffer_.volume_size.x = size_x;
+ buffer_.volume_size.y = size_y;
+ buffer_.volume_size.z = size_z;
+ }
+
+ /** \brief Set the physical size represented by the default TSDF volume.
+ * \param[in] size size of the volume on all axis, in meters.
+ */
+ void setVolumeSize (const double size)
+ {
+ buffer_.volume_size.x = size;
+ buffer_.volume_size.y = size;
+ buffer_.volume_size.z = size;
+ }
+
+ /** \brief Computes and set the origin of the new cube (relative to the world), centered around a the target point.
+ * \param[in] target_point the target point around which the new cube will be centered.
+ * \param[out] shiftX shift on X axis (in indices).
+ * \param[out] shiftY shift on Y axis (in indices).
+ * \param[out] shiftZ shift on Z axis (in indices).
+ */
+ void computeAndSetNewCubeMetricOrigin (const pcl::PointXYZ &target_point, int &shiftX, int &shiftY, int &shiftZ);
+
+ /** \brief Initializes memory pointers of the cyclical buffer (start, end, current origin)
+ * \param[in] tsdf_volume pointer to the TSDF volume managed by this cyclical buffer
+ */
+ void initBuffer (TsdfVolume::Ptr tsdf_volume)
+ {
+ PtrStep<short2> localVolume = tsdf_volume->data();
+
+ buffer_.tsdf_memory_start = &(localVolume.ptr (0)[0]);
+ buffer_.tsdf_memory_end = &(localVolume.ptr (buffer_.voxels_size.y * (buffer_.voxels_size.z - 1) + (buffer_.voxels_size.y - 1) )[buffer_.voxels_size.x - 1]);
+ buffer_.tsdf_rolling_buff_origin = buffer_.tsdf_memory_start;
+ }
+
+ /** \brief Reset buffer structure
+ * \param[in] tsdf_volume pointer to the TSDF volume managed by this cyclical buffer
+ */
+ void resetBuffer (TsdfVolume::Ptr tsdf_volume)
+ {
+ buffer_.origin_GRID.x = 0; buffer_.origin_GRID.y = 0; buffer_.origin_GRID.z = 0;
+ buffer_.origin_GRID_global.x = 0.f; buffer_.origin_GRID_global.y = 0.f; buffer_.origin_GRID_global.z = 0.f;
+ buffer_.origin_metric.x = 0.f; buffer_.origin_metric.y = 0.f; buffer_.origin_metric.z = 0.f;
+ initBuffer (tsdf_volume);
+ }
+
+ /** \brief Return a pointer to the world model
+ */
+ pcl::kinfuLS::WorldModel<pcl::PointXYZI>*
+ getWorldModel ()
+ {
+ return (&world_model_);
+ }
+
+
+ private:
+
+ /** \brief buffer used to extract XYZ values from GPU */
+ DeviceArray<PointXYZ> cloud_buffer_device_xyz_;
+
+ /** \brief buffer used to extract Intensity values from GPU */
+ DeviceArray<float> cloud_buffer_device_intensities_;
+
+ /** \brief distance threshold (cube's center to target point) to trigger shift */
+ double distance_threshold_;
+
+ /** \brief world model object that maintains the known world */
+ pcl::kinfuLS::WorldModel<pcl::PointXYZI> world_model_;
+
+ /** \brief structure that contains all TSDF buffer's addresses */
+ tsdf_buffer buffer_;
+
+ /** \brief updates cyclical buffer origins given offsets on X, Y and Z
+ * \param[in] tsdf_volume pointer to the TSDF volume managed by this cyclical buffer
+ * \param[in] offset_x offset in indices on axis X
+ * \param[in] offset_y offset in indices on axis Y
+ * \param[in] offset_z offset in indices on axis Z
+ */
+ void shiftOrigin (TsdfVolume::Ptr tsdf_volume, const int offset_x, const int offset_y, const int offset_z)
+ {
+ // shift rolling origin (making sure they keep in [0 - NbVoxels[ )
+ buffer_.origin_GRID.x += offset_x;
+ if(buffer_.origin_GRID.x >= buffer_.voxels_size.x)
+ buffer_.origin_GRID.x -= buffer_.voxels_size.x;
+ else if(buffer_.origin_GRID.x < 0)
+ buffer_.origin_GRID.x += buffer_.voxels_size.x;
+
+ buffer_.origin_GRID.y += offset_y;
+ if(buffer_.origin_GRID.y >= buffer_.voxels_size.y)
+ buffer_.origin_GRID.y -= buffer_.voxels_size.y;
+ else if(buffer_.origin_GRID.y < 0)
+ buffer_.origin_GRID.y += buffer_.voxels_size.y;
+
+ buffer_.origin_GRID.z += offset_z;
+ if(buffer_.origin_GRID.z >= buffer_.voxels_size.z)
+ buffer_.origin_GRID.z -= buffer_.voxels_size.z;
+ else if(buffer_.origin_GRID.z < 0)
+ buffer_.origin_GRID.z += buffer_.voxels_size.z;
+
+ // update memory pointers
+ PtrStep<short2> localVolume = tsdf_volume->data();
+ buffer_.tsdf_memory_start = &(localVolume.ptr (0)[0]);
+ buffer_.tsdf_memory_end = &(localVolume.ptr (buffer_.voxels_size.y * (buffer_.voxels_size.z - 1) + (buffer_.voxels_size.y - 1) )[buffer_.voxels_size.x - 1]);
+ buffer_.tsdf_rolling_buff_origin = &(localVolume.ptr (buffer_.voxels_size.y * (buffer_.origin_GRID.z) + (buffer_.origin_GRID.y) )[buffer_.origin_GRID.x]);
+
+ // update global origin
+ buffer_.origin_GRID_global.x += offset_x;
+ buffer_.origin_GRID_global.y += offset_y;
+ buffer_.origin_GRID_global.z += offset_z;
+ }
+
+ };
+ }
+ }
+}
+
+#endif // PCL_CYCLICAL_BUFFER_IMPL_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_KINFU_DEVICE_H_
+#define PCL_KINFU_DEVICE_H_
+
+#include <pcl/gpu/containers/device_array.h>
+#include <iostream> // used by operator << in Struct Intr
+#include <pcl/gpu/kinfu_large_scale/tsdf_buffer.h>
+
+//using namespace pcl::gpu;
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Types
+ typedef unsigned short ushort;
+ typedef DeviceArray2D<float> MapArr;
+ typedef DeviceArray2D<ushort> DepthMap;
+ typedef float4 PointType;
+
+ //TSDF fixed point divisor (if old format is enabled)
+ const int DIVISOR = 32767; // SHRT_MAX;
+
+ //RGB images resolution
+ const float HEIGHT = 480.0f;
+ const float WIDTH = 640.0f;
+
+ //Should be multiple of 32
+ enum { VOLUME_X = 512, VOLUME_Y = 512, VOLUME_Z = 512 };
+
+
+ //Temporary constant (until we make it automatic) that holds the Kinect's focal length
+ const float FOCAL_LENGTH = 575.816f;
+
+ const float VOLUME_SIZE = 3.0f; // physical size represented by the TSDF volume. In meters
+ const float DISTANCE_THRESHOLD = 1.5f; // when the camera target point is farther than DISTANCE_THRESHOLD from the current cube's center, shifting occurs. In meters
+ const int SNAPSHOT_RATE = 45; // every 45 frames an RGB snapshot will be saved. -et parameter is needed when calling Kinfu Large Scale in command line.
+
+
+ /** \brief Camera intrinsics structure
+ */
+ struct Intr
+ {
+ float fx, fy, cx, cy;
+ Intr () {}
+ Intr (float fx_, float fy_, float cx_, float cy_) : fx (fx_), fy (fy_), cx (cx_), cy (cy_) {}
+
+ Intr operator () (int level_index) const
+ {
+ int div = 1 << level_index;
+ return (Intr (fx / div, fy / div, cx / div, cy / div));
+ }
+
+ friend inline std::ostream&
+ operator << (std::ostream& os, const Intr& intr)
+ {
+ os << "([f = " << intr.fx << ", " << intr.fy << "] [cp = " << intr.cx << ", " << intr.cy << "])";
+ return (os);
+ }
+ };
+
+ /** \brief 3x3 Matrix for device code
+ */
+ struct Mat33
+ {
+ float3 data[3];
+ };
+ }
+ }
+}
+
+#endif /* PCL_KINFU_DEVICE_H_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Raphael Favier, Technical University Eindhoven, (r.mysurname <aT> tue.nl)
+ *
+ */
+
+#include <iostream>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace kinfuLS
+ {
+ inline float
+ dot(const float3& v1, const float3& v2)
+ {
+ return v1.x * v2.x + v1.y*v2.y + v1.z*v2.z;
+ }
+
+ inline float3&
+ operator+=(float3& vec, const float& v)
+ {
+ vec.x += v; vec.y += v; vec.z += v; return vec;
+ }
+
+ inline float3&
+ operator+=(float3& vec, const float3& v)
+ {
+ vec.x += v.x; vec.y += v.y; vec.z += v.z; return vec;
+ }
+
+ inline float3
+ operator+(const float3& v1, const float3& v2)
+ {
+ return make_float3 (v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
+ }
+
+ inline float3&
+ operator*=(float3& vec, const float& v)
+ {
+ vec.x *= v; vec.y *= v; vec.z *= v; return vec;
+ }
+
+ inline float3&
+ operator-=(float3& vec, const float& v)
+ {
+ vec.x -= v; vec.y -= v; vec.z -= v; return vec;
+ }
+
+ inline float3&
+ operator-=(float3& vec, const float3& v)
+ {
+ vec.x -= v.x; vec.y -= v.y; vec.z -= v.z; return vec;
+ }
+
+ inline float3
+ operator-(const float3& v1, const float3& v2)
+ {
+ return make_float3 (v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
+ }
+
+ inline float3
+ operator-(const float3& v1)
+ {
+ return make_float3 (-v1.x, -v1.y, -v1.z);
+ }
+
+ inline float3
+ operator-(float3& v1)
+ {
+ v1.x = -v1.x; v1.y = -v1.y; v1.z = -v1.z; return v1;
+ }
+
+ inline float3
+ operator*(const float3& v1, const float& v)
+ {
+ return make_float3 (v1.x * v, v1.y * v, v1.z * v);
+ }
+
+ inline float
+ norm(const float3& v)
+ {
+ return sqrt (dot (v, v));
+ }
+
+ inline std::ostream&
+ operator << (std::ostream& os, const float3& v1)
+ {
+ os << "[" << v1.x << ", " << v1.y << ", " << v1.z<< "]";
+ return (os);
+ }
+
+ /*inline float3
+ normalized(const float3& v)
+ {
+ return v * rsqrt(dot(v, v));
+ }*/
+
+ inline float3
+ cross(const float3& v1, const float3& v2)
+ {
+ return make_float3 (v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x);
+ }
+ }
+ }
+}
\ No newline at end of file
--- /dev/null
+ /*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_STANDALONE_MARCHING_CUBES_IMPL_HPP_
+#define PCL_STANDALONE_MARCHING_CUBES_IMPL_HPP_
+
+#include <pcl/gpu/kinfu_large_scale/standalone_marching_cubes.h>
+
+///////////////////////////////////////////////////////////////////////////////
+template <typename PointT>
+pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::StandaloneMarchingCubes (int new_voxels_x, int new_voxels_y, int new_voxels_z, float new_volume_size)
+{
+ voxels_x_ = new_voxels_x;
+ voxels_y_ = new_voxels_y;
+ voxels_z_ = new_voxels_z;
+ volume_size_ = new_volume_size;
+
+ ///Creating GPU TSDF Volume instance
+ const Eigen::Vector3f volume_size = Eigen::Vector3f::Constant (volume_size_);
+ // std::cout << "VOLUME SIZE IS " << volume_size_ << std::endl;
+ const Eigen::Vector3i volume_resolution (voxels_x_, voxels_y_, voxels_z_);
+ tsdf_volume_gpu_ = TsdfVolume::Ptr ( new TsdfVolume (volume_resolution) );
+ tsdf_volume_gpu_->setSize (volume_size);
+
+ ///Creating CPU TSDF Volume instance
+ int tsdf_total_size = voxels_x_ * voxels_y_ * voxels_z_;
+ tsdf_volume_cpu_= std::vector<int> (tsdf_total_size,0);
+
+ mesh_counter_ = 0;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+template <typename PointT> typename pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::MeshPtr
+pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::getMeshFromTSDFCloud (const PointCloud &cloud)
+{
+
+ //Clearing TSDF GPU and cPU
+ const Eigen::Vector3f volume_size = Eigen::Vector3f::Constant (volume_size_);
+ std::cout << "VOLUME SIZE IS " << volume_size_ << std::endl;
+ const Eigen::Vector3i volume_resolution (voxels_x_, voxels_y_, voxels_z_);
+
+ //Clear values in TSDF Volume GPU
+ tsdf_volume_gpu_->reset (); // This one uses the same tsdf volume but clears it before loading new values. This one is our friend.
+
+ //Clear values in TSDF Volume CPU
+ fill (tsdf_volume_cpu_.begin (), tsdf_volume_cpu_.end (), 0);
+
+ //Loading values to GPU
+ loadTsdfCloudToGPU (cloud);
+
+ //Creating and returning mesh
+ return ( runMarchingCubes () );
+
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+//template <typename PointT> std::vector< typename pcl::gpu::StandaloneMarchingCubes<PointT>::MeshPtr >
+template <typename PointT> void
+pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::getMeshesFromTSDFVector (const std::vector<PointCloudPtr> &tsdf_clouds, const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &tsdf_offsets)
+{
+ std::vector< MeshPtr > meshes_vector;
+
+ int max_iterations = std::min( tsdf_clouds.size (), tsdf_offsets.size () ); //Safety check
+ PCL_INFO ("There are %d cubes to be processed \n", max_iterations);
+ float cell_size = volume_size_ / voxels_x_;
+
+ int mesh_counter = 0;
+
+ for(int i = 0; i < max_iterations; ++i)
+ {
+ PCL_INFO ("Processing cube number %d\n", i);
+
+ //Making cloud local
+ Eigen::Affine3f cloud_transform;
+
+ float originX = (tsdf_offsets[i]).x();
+ float originY = (tsdf_offsets[i]).y();
+ float originZ = (tsdf_offsets[i]).z();
+
+ cloud_transform.linear ().setIdentity ();
+ cloud_transform.translation ()[0] = -originX;
+ cloud_transform.translation ()[1] = -originY;
+ cloud_transform.translation ()[2] = -originZ;
+
+ transformPointCloud (*tsdf_clouds[i], *tsdf_clouds[i], cloud_transform);
+
+ //Get mesh
+ MeshPtr tmp = getMeshFromTSDFCloud (*tsdf_clouds[i]);
+
+ if(tmp != 0)
+ {
+ meshes_vector.push_back (tmp);
+ mesh_counter++;
+ }
+ else
+ {
+ PCL_INFO ("This cloud returned no faces, we skip it!\n");
+ continue;
+ }
+
+ //Making cloud global
+ cloud_transform.translation ()[0] = originX * cell_size;
+ cloud_transform.translation ()[1] = originY * cell_size;
+ cloud_transform.translation ()[2] = originZ * cell_size;
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp_ptr (new pcl::PointCloud<pcl::PointXYZ>);
+ fromPCLPointCloud2 ( (meshes_vector.back () )->cloud, *cloud_tmp_ptr);
+
+ transformPointCloud (*cloud_tmp_ptr, *cloud_tmp_ptr, cloud_transform);
+
+ toPCLPointCloud2 (*cloud_tmp_ptr, (meshes_vector.back () )->cloud);
+
+ std::stringstream name;
+ name << "mesh_" << mesh_counter << ".ply";
+ PCL_INFO ("Saving mesh...%d \n", mesh_counter);
+ pcl::io::savePLYFile (name.str (), *(meshes_vector.back ()));
+
+ }
+ return;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+template <typename PointT> pcl::gpu::kinfuLS::TsdfVolume::Ptr
+pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::tsdfVolumeGPU ()
+{
+ return (tsdf_volume_gpu_);
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+template <typename PointT> std::vector<int>& //todo
+pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::tsdfVolumeCPU ()
+{
+ return (tsdf_volume_cpu_);
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+template <typename PointT> void
+pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::loadTsdfCloudToGPU (const PointCloud &cloud)
+{
+ //Converting Values
+ convertTsdfVectors (cloud, tsdf_volume_cpu_);
+
+ //Uploading data to GPU
+ int cubeColumns = voxels_x_;
+ tsdf_volume_gpu_->data ().upload (tsdf_volume_cpu_, cubeColumns);
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+template <typename PointT> void
+pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::convertTsdfVectors (const PointCloud &cloud, std::vector<int> &output)
+{
+ const int DIVISOR = 32767; // SHRT_MAX;
+
+ ///For every point in the cloud
+#pragma omp parallel for
+
+ for(int i = 0; i < (int) cloud.points.size (); ++i)
+ {
+ int x = cloud.points[i].x;
+ int y = cloud.points[i].y;
+ int z = cloud.points[i].z;
+
+ if(x > 0 && x < voxels_x_ && y > 0 && y < voxels_y_ && z > 0 && z < voxels_z_)
+ {
+ ///Calculate the index to write to
+ int dst_index = x + voxels_x_ * y + voxels_y_ * voxels_x_ * z;
+
+ short2& elem = *reinterpret_cast<short2*> (&output[dst_index]);
+ elem.x = static_cast<short> (cloud.points[i].intensity * DIVISOR);
+ elem.y = static_cast<short> (1);
+ }
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+template <typename PointT> typename pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::MeshPtr
+pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::convertTrianglesToMesh (const pcl::gpu::DeviceArray<pcl::PointXYZ>& triangles)
+{
+ if (triangles.empty () )
+ {
+ return MeshPtr ();
+ }
+
+ pcl::PointCloud<pcl::PointXYZ> cloud;
+ cloud.width = (int)triangles.size ();
+ cloud.height = 1;
+ triangles.download (cloud.points);
+
+ boost::shared_ptr<pcl::PolygonMesh> mesh_ptr ( new pcl::PolygonMesh () );
+
+ pcl::toPCLPointCloud2 (cloud, mesh_ptr->cloud);
+
+ mesh_ptr->polygons.resize (triangles.size () / 3);
+ for (size_t i = 0; i < mesh_ptr->polygons.size (); ++i)
+ {
+ pcl::Vertices v;
+ v.vertices.push_back (i*3+0);
+ v.vertices.push_back (i*3+2);
+ v.vertices.push_back (i*3+1);
+ mesh_ptr->polygons[i] = v;
+ }
+ return (mesh_ptr);
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+template <typename PointT> typename pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::MeshPtr
+pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::runMarchingCubes ()
+{
+ //Preparing the pointers and variables
+ const TsdfVolume::Ptr tsdf_volume_const_ = tsdf_volume_gpu_;
+ pcl::gpu::DeviceArray<pcl::PointXYZ> triangles_buffer_device_;
+
+ //Creating Marching cubes instance
+ MarchingCubes::Ptr marching_cubes_ = MarchingCubes::Ptr ( new MarchingCubes() );
+
+ //Running marching cubes
+ pcl::gpu::DeviceArray<pcl::PointXYZ> triangles_device = marching_cubes_->run (*tsdf_volume_const_, triangles_buffer_device_);
+
+ //Creating mesh
+ boost::shared_ptr<pcl::PolygonMesh> mesh_ptr_ = convertTrianglesToMesh (triangles_device);
+
+ if(mesh_ptr_ != 0)
+ {
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp_ptr (new pcl::PointCloud<pcl::PointXYZ>);
+ fromPCLPointCloud2 ( mesh_ptr_->cloud, *cloud_tmp_ptr);
+ }
+ return (mesh_ptr_);
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+#endif // PCL_STANDALONE_MARCHING_CUBES_IMPL_HPP_
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Raphael Favier, Technical University Eindhoven, (r.mysurname <aT> tue.nl)
+ */
+
+#ifndef PCL_WORLD_MODEL_IMPL_HPP_
+#define PCL_WORLD_MODEL_IMPL_HPP_
+
+#include <pcl/gpu/kinfu_large_scale/world_model.h>
+
+template <typename PointT>
+void
+pcl::kinfuLS::WorldModel<PointT>::addSlice ( PointCloudPtr new_cloud)
+{
+ PCL_DEBUG ("Adding new cloud. Current world contains %d points.\n", world_->points.size ());
+
+ PCL_DEBUG ("New slice contains %d points.\n", new_cloud->points.size ());
+
+ *world_ += *new_cloud;
+
+ PCL_DEBUG ("World now contains %d points.\n", world_->points.size ());
+}
+
+
+template <typename PointT>
+void
+pcl::kinfuLS::WorldModel<PointT>::getExistingData(const double previous_origin_x, const double previous_origin_y, const double previous_origin_z, const double offset_x, const double offset_y, const double offset_z, const double volume_x, const double volume_y, const double volume_z, pcl::PointCloud<PointT> &existing_slice)
+{
+ double newOriginX = previous_origin_x + offset_x;
+ double newOriginY = previous_origin_y + offset_y;
+ double newOriginZ = previous_origin_z + offset_z;
+ double newLimitX = newOriginX + volume_x;
+ double newLimitY = newOriginY + volume_y;
+ double newLimitZ = newOriginZ + volume_z;
+
+ // filter points in the space of the new cube
+ PointCloudPtr newCube (new pcl::PointCloud<PointT>);
+ // condition
+ ConditionAndPtr range_condAND (new pcl::ConditionAnd<PointT> ());
+ range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, newOriginX)));
+ range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, newLimitX)));
+ range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, newOriginY)));
+ range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, newLimitY)));
+ range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, newOriginZ)));
+ range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, newLimitZ)));
+
+ // build the filter
+ pcl::ConditionalRemoval<PointT> condremAND (true);
+ condremAND.setCondition (range_condAND);
+ condremAND.setInputCloud (world_);
+ condremAND.setKeepOrganized (false);
+
+ // apply filter
+ condremAND.filter (*newCube);
+
+ // filter points that belong to the new slice
+ ConditionOrPtr range_condOR (new pcl::ConditionOr<PointT> ());
+
+ if(offset_x >= 0)
+ range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, previous_origin_x + volume_x - 1.0 )));
+ else
+ range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, previous_origin_x )));
+
+ if(offset_y >= 0)
+ range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, previous_origin_y + volume_y - 1.0 )));
+ else
+ range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, previous_origin_y )));
+
+ if(offset_z >= 0)
+ range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, previous_origin_z + volume_z - 1.0 )));
+ else
+ range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, previous_origin_z )));
+
+ // build the filter
+ pcl::ConditionalRemoval<PointT> condrem (true);
+ condrem.setCondition (range_condOR);
+ condrem.setInputCloud (newCube);
+ condrem.setKeepOrganized (false);
+ // apply filter
+ condrem.filter (existing_slice);
+
+ if(existing_slice.points.size () != 0)
+ {
+ //transform the slice in new cube coordinates
+ Eigen::Affine3f transformation;
+ transformation.translation ()[0] = newOriginX;
+ transformation.translation ()[1] = newOriginY;
+ transformation.translation ()[2] = newOriginZ;
+
+ transformation.linear ().setIdentity ();
+
+ transformPointCloud (existing_slice, existing_slice, transformation.inverse ());
+
+ }
+}
+
+
+template <typename PointT>
+void
+pcl::kinfuLS::WorldModel<PointT>::getWorldAsCubes (const double size, std::vector<typename WorldModel<PointT>::PointCloudPtr> &cubes, std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &transforms, double overlap)
+{
+
+ if(world_->points.size () == 0)
+ {
+ PCL_INFO("The world is empty, returning nothing\n");
+ return;
+ }
+
+ PCL_INFO ("Getting world as cubes. World contains %d points.\n", world_->points.size ());
+
+ // remove nans from world cloud
+ world_->is_dense = false;
+ std::vector<int> indices;
+ pcl::removeNaNFromPointCloud ( *world_, *world_, indices);
+
+ PCL_INFO ("World contains %d points after nan removal.\n", world_->points.size ());
+
+
+ // check cube size value
+ double cubeSide = size;
+ if (cubeSide <= 0.0f)
+ {
+ PCL_ERROR ("Size of the cube must be positive and non null (%f given). Setting it to 3.0 meters.\n", cubeSide);
+ cubeSide = 512.0f;
+ }
+
+ std::cout << "cube size is set to " << cubeSide << std::endl;
+
+ // check overlap value
+ double step_increment = 1.0f - overlap;
+ if (overlap < 0.0)
+ {
+ PCL_ERROR ("Overlap ratio must be positive or null (%f given). Setting it to 0.0 procent.\n", overlap);
+ step_increment = 1.0f;
+ }
+ if (overlap > 1.0)
+ {
+ PCL_ERROR ("Overlap ratio must be less or equal to 1.0 (%f given). Setting it to 10 procent.\n", overlap);
+ step_increment = 0.1f;
+ }
+
+
+ // get world's bounding values on XYZ
+ PointT min, max;
+ pcl::getMinMax3D(*world_, min, max);
+
+ PCL_INFO ("Bounding box for the world: \n\t [%f - %f] \n\t [%f - %f] \n\t [%f - %f] \n", min.x, max.x, min.y, max.y, min.z, max.z);
+
+ PointT origin = min;
+
+ // clear returned vectors
+ cubes.clear();
+ transforms.clear();
+
+ // iterate with box filter
+ while (origin.x < max.x)
+ {
+ origin.y = min.y;
+ while (origin.y < max.y)
+ {
+ origin.z = min.z;
+ while (origin.z < max.z)
+ {
+ // extract cube here
+ PCL_INFO ("Extracting cube at: [%f, %f, %f].\n", origin.x, origin.y, origin.z);
+
+ // pointcloud for current cube.
+ PointCloudPtr box (new pcl::PointCloud<PointT>);
+
+
+ // set conditional filter
+ ConditionAndPtr range_cond (new pcl::ConditionAnd<PointT> ());
+ range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, origin.x)));
+ range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, origin.x + cubeSide)));
+ range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, origin.y)));
+ range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, origin.y + cubeSide)));
+ range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, origin.z)));
+ range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, origin.z + cubeSide)));
+
+ // build the filter
+ pcl::ConditionalRemoval<PointT> condrem;
+ condrem.setCondition (range_cond);
+ condrem.setInputCloud (world_);
+ condrem.setKeepOrganized(false);
+ // apply filter
+ condrem.filter (*box);
+
+ // also push transform along with points.
+ if(box->points.size() > 0)
+ {
+ Eigen::Vector3f transform;
+ transform[0] = origin.x, transform[1] = origin.y, transform[2] = origin.z;
+ transforms.push_back(transform);
+ cubes.push_back(box);
+ }
+ else
+ {
+ PCL_INFO ("Extracted cube was empty, skiping this one.\n");
+ }
+ origin.z += cubeSide * step_increment;
+ }
+ origin.y += cubeSide * step_increment;
+ }
+ origin.x += cubeSide * step_increment;
+ }
+
+
+ /* for(int c = 0 ; c < cubes.size() ; ++c)
+ {
+ std::stringstream name;
+ name << "cloud" << c+1 << ".pcd";
+ pcl::io::savePCDFileASCII(name.str(), *(cubes[c]));
+
+ }*/
+
+ std::cout << "returning " << cubes.size() << " cubes" << std::endl;
+
+}
+
+template <typename PointT>
+inline void
+pcl::kinfuLS::WorldModel<PointT>::setIndicesAsNans (PointCloudPtr cloud, IndicesConstPtr indices)
+{
+ std::vector<pcl::PCLPointField> fields;
+ pcl::for_each_type<FieldList> (pcl::detail::FieldAdder<PointT> (fields));
+ float my_nan = std::numeric_limits<float>::quiet_NaN ();
+
+ for (int rii = 0; rii < static_cast<int> (indices->size ()); ++rii) // rii = removed indices iterator
+ {
+ uint8_t* pt_data = reinterpret_cast<uint8_t*> (&cloud->points[(*indices)[rii]]);
+ for (int fi = 0; fi < static_cast<int> (fields.size ()); ++fi) // fi = field iterator
+ memcpy (pt_data + fields[fi].offset, &my_nan, sizeof (float));
+ }
+}
+
+
+template <typename PointT>
+void
+pcl::kinfuLS::WorldModel<PointT>::setSliceAsNans (const double origin_x, const double origin_y, const double origin_z, const double offset_x, const double offset_y, const double offset_z, const int size_x, const int size_y, const int size_z)
+{
+ // PCL_DEBUG ("IN SETSLICE AS NANS\n");
+
+ PointCloudPtr slice (new pcl::PointCloud<PointT>);
+
+ // prepare filter limits on all dimensions
+ double previous_origin_x = origin_x;
+ double previous_limit_x = origin_x + size_x - 1;
+ double new_origin_x = origin_x + offset_x;
+ double new_limit_x = previous_limit_x + offset_x;
+
+ double previous_origin_y = origin_y;
+ double previous_limit_y = origin_y + size_y - 1;
+ double new_origin_y = origin_y + offset_y;
+ double new_limit_y = previous_limit_y + offset_y;
+
+ double previous_origin_z = origin_z;
+ double previous_limit_z = origin_z + size_z - 1;
+ double new_origin_z = origin_z + offset_z;
+ double new_limit_z = previous_limit_z + offset_z;
+
+ // get points of slice on X (we actually set a negative filter and set the ouliers (so, our slice points) to nan)
+ double lower_limit_x, upper_limit_x;
+ if(offset_x >=0)
+ {
+ lower_limit_x = previous_origin_x;
+ upper_limit_x = new_origin_x;
+ }
+ else
+ {
+ lower_limit_x = new_limit_x;
+ upper_limit_x = previous_limit_x;
+ }
+
+ // PCL_DEBUG ("Limit X: [%f - %f]\n", lower_limit_x, upper_limit_x);
+
+ ConditionOrPtr range_cond_OR_x (new pcl::ConditionOr<PointT> ());
+ range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, upper_limit_x ))); // filtered dimension
+ range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, lower_limit_x ))); // filtered dimension
+
+ range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, previous_limit_y)));
+ range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, previous_origin_y )));
+
+ range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, previous_limit_z)));
+ range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, previous_origin_z )));
+
+ pcl::ConditionalRemoval<PointT> condrem_x (true);
+ condrem_x.setCondition (range_cond_OR_x);
+ condrem_x.setInputCloud (world_);
+ condrem_x.setKeepOrganized (false);
+ // apply filter
+ condrem_x.filter (*slice);
+ IndicesConstPtr indices_x = condrem_x.getRemovedIndices ();
+
+ //set outliers (so our slice points) to nan
+ setIndicesAsNans(world_, indices_x);
+
+ // PCL_DEBUG("%d points set to nan on X\n", indices_x->size ());
+
+ // get points of slice on Y (we actually set a negative filter and set the ouliers (so, our slice points) to nan)
+ double lower_limit_y, upper_limit_y;
+ if(offset_y >=0)
+ {
+ lower_limit_y = previous_origin_y;
+ upper_limit_y = new_origin_y;
+ }
+ else
+ {
+ lower_limit_y = new_limit_y;
+ upper_limit_y = previous_limit_y;
+ }
+
+ // PCL_DEBUG ("Limit Y: [%f - %f]\n", lower_limit_y, upper_limit_y);
+
+ ConditionOrPtr range_cond_OR_y (new pcl::ConditionOr<PointT> ());
+ range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, previous_limit_x )));
+ range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, previous_origin_x )));
+
+ range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, upper_limit_y))); // filtered dimension
+ range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, lower_limit_y ))); // filtered dimension
+
+ range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, previous_limit_z)));
+ range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, previous_origin_z )));
+
+ pcl::ConditionalRemoval<PointT> condrem_y (true);
+ condrem_y.setCondition (range_cond_OR_y);
+ condrem_y.setInputCloud (world_);
+ condrem_y.setKeepOrganized (false);
+ // apply filter
+ condrem_y.filter (*slice);
+ IndicesConstPtr indices_y = condrem_y.getRemovedIndices ();
+
+ //set outliers (so our slice points) to nan
+ setIndicesAsNans(world_, indices_y);
+ // PCL_DEBUG ("%d points set to nan on Y\n", indices_y->size ());
+
+ // get points of slice on Z (we actually set a negative filter and set the ouliers (so, our slice points) to nan)
+ double lower_limit_z, upper_limit_z;
+ if(offset_z >=0)
+ {
+ lower_limit_z = previous_origin_z;
+ upper_limit_z = new_origin_z;
+ }
+ else
+ {
+ lower_limit_z = new_limit_z;
+ upper_limit_z = previous_limit_z;
+ }
+
+ // PCL_DEBUG ("Limit Z: [%f - %f]\n", lower_limit_z, upper_limit_z);
+
+ ConditionOrPtr range_cond_OR_z (new pcl::ConditionOr<PointT> ());
+ range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, previous_limit_x )));
+ range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, previous_origin_x )));
+
+ range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, previous_limit_y)));
+ range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, previous_origin_y )));
+
+ range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, upper_limit_z))); // filtered dimension
+ range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, lower_limit_z ))); // filtered dimension
+
+ pcl::ConditionalRemoval<PointT> condrem_z (true);
+ condrem_z.setCondition (range_cond_OR_z);
+ condrem_z.setInputCloud (world_);
+ condrem_z.setKeepOrganized (false);
+ // apply filter
+ condrem_z.filter (*slice);
+ IndicesConstPtr indices_z = condrem_z.getRemovedIndices ();
+
+ //set outliers (so our slice points) to nan
+ setIndicesAsNans(world_, indices_z);
+ // PCL_DEBUG("%d points set to nan on Z\n", indices_z->size ());
+
+
+}
+
+#define PCL_INSTANTIATE_WorldModel(T) template class PCL_EXPORTS pcl::kinfuLS::WorldModel<T>;
+
+#endif // PCL_WORLD_MODEL_IMPL_HPP_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_KINFU_KINFUTRACKER_HPP_
+#define PCL_KINFU_KINFUTRACKER_HPP_
+
+#include <pcl/pcl_macros.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/ply_io.h>
+
+#include <Eigen/Core>
+#include <vector>
+//#include <boost/graph/buffer_concepts.hpp>
+
+#include <pcl/gpu/kinfu_large_scale/device.h>
+
+#include <pcl/gpu/kinfu_large_scale/float3_operations.h>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/kinfu_large_scale/pixel_rgb.h>
+#include <pcl/gpu/kinfu_large_scale/tsdf_volume.h>
+#include <pcl/gpu/kinfu_large_scale/color_volume.h>
+#include <pcl/gpu/kinfu_large_scale/raycaster.h>
+
+#include <pcl/gpu/kinfu_large_scale/cyclical_buffer.h>
+//#include <pcl/gpu/kinfu_large_scale/standalone_marching_cubes.h>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace kinfuLS
+ {
+ /** \brief KinfuTracker class encapsulates implementation of Microsoft Kinect Fusion algorithm
+ * \author Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+ class PCL_EXPORTS KinfuTracker
+ {
+ public:
+
+ /** \brief Pixel type for rendered image. */
+ typedef pcl::gpu::kinfuLS::PixelRGB PixelRGB;
+
+ typedef DeviceArray2D<PixelRGB> View;
+ typedef DeviceArray2D<unsigned short> DepthMap;
+
+ typedef pcl::PointXYZ PointType;
+ typedef pcl::Normal NormalType;
+
+ void
+ performLastScan (){perform_last_scan_ = true; PCL_WARN ("Kinfu will exit after next shift\n");}
+
+ bool
+ isFinished (){return (finished_);}
+
+ /** \brief Constructor
+ * \param[in] volumeSize physical size of the volume represented by the tdsf volume. In meters.
+ * \param[in] shiftingDistance when the camera target point is farther than shiftingDistance from the center of the volume, shiting occurs. In meters.
+ * \note The target point is located at (0, 0, 0.6*volumeSize) in camera coordinates.
+ * \param[in] rows height of depth image
+ * \param[in] cols width of depth image
+ */
+ KinfuTracker (const Eigen::Vector3f &volumeSize, const float shiftingDistance, int rows = 480, int cols = 640);
+
+ /** \brief Sets Depth camera intrinsics
+ * \param[in] fx focal length x
+ * \param[in] fy focal length y
+ * \param[in] cx principal point x
+ * \param[in] cy principal point y
+ */
+ void
+ setDepthIntrinsics (float fx, float fy, float cx = -1, float cy = -1);
+
+ /** \brief Sets initial camera pose relative to volume coordiante space
+ * \param[in] pose Initial camera pose
+ */
+ void
+ setInitialCameraPose (const Eigen::Affine3f& pose);
+
+ /** \brief Sets truncation threshold for depth image for ICP step only! This helps
+ * to filter measurements that are outside tsdf volume. Pass zero to disable the truncation.
+ * \param[in] max_icp_distance Maximal distance, higher values are reset to zero (means no measurement).
+ */
+ void
+ setDepthTruncationForICP (float max_icp_distance = 0.f);
+
+ /** \brief Sets ICP filtering parameters.
+ * \param[in] distThreshold distance.
+ * \param[in] sineOfAngle sine of angle between normals.
+ */
+ void
+ setIcpCorespFilteringParams (float distThreshold, float sineOfAngle);
+
+ /** \brief Sets integration threshold. TSDF volume is integrated iff a camera movement metric exceedes the threshold value.
+ * The metric represents the following: M = (rodrigues(Rotation).norm() + alpha*translation.norm())/2, where alpha = 1.f (hardcoded constant)
+ * \param[in] threshold a value to compare with the metric. Suitable values are ~0.001
+ */
+ void
+ setCameraMovementThreshold(float threshold = 0.001f);
+
+ /** \brief Performs initialization for color integration. Must be called before calling color integration.
+ * \param[in] max_weight max weighe for color integration. -1 means default weight.
+ */
+ void
+ initColorIntegration(int max_weight = -1);
+
+ /** \brief Returns cols passed to ctor */
+ int
+ cols ();
+
+ /** \brief Returns rows passed to ctor */
+ int
+ rows ();
+
+ /** \brief Processes next frame.
+ * \param[in] depth next frame with values in millimeters
+ * \return true if can render 3D view.
+ */
+ bool operator() (const DepthMap& depth);
+
+ /** \brief Processes next frame (both depth and color integration). Please call initColorIntegration before invpoking this.
+ * \param[in] depth next depth frame with values in millimeters
+ * \param[in] colors next RGB frame
+ * \return true if can render 3D view.
+ */
+ bool operator() (const DepthMap& depth, const View& colors);
+
+ /** \brief Returns camera pose at given time, default the last pose
+ * \param[in] time Index of frame for which camera pose is returned.
+ * \return camera pose
+ */
+ Eigen::Affine3f
+ getCameraPose (int time = -1) const;
+
+ Eigen::Affine3f
+ getLastEstimatedPose () const;
+
+ /** \brief Returns number of poses including initial */
+ size_t
+ getNumberOfPoses () const;
+
+ /** \brief Returns TSDF volume storage */
+ const TsdfVolume& volume() const;
+
+ /** \brief Returns TSDF volume storage */
+ TsdfVolume& volume();
+
+ /** \brief Returns color volume storage */
+ const ColorVolume& colorVolume() const;
+
+ /** \brief Returns color volume storage */
+ ColorVolume& colorVolume();
+
+ /** \brief Renders 3D scene to display to human
+ * \param[out] view output array with image
+ */
+ void
+ getImage (View& view) const;
+
+ /** \brief Returns point cloud abserved from last camera pose
+ * \param[out] cloud output array for points
+ */
+ void
+ getLastFrameCloud (DeviceArray2D<PointType>& cloud) const;
+
+ /** \brief Returns point cloud abserved from last camera pose
+ * \param[out] normals output array for normals
+ */
+ void
+ getLastFrameNormals (DeviceArray2D<NormalType>& normals) const;
+
+
+ /** \brief Returns pointer to the cyclical buffer structure
+ */
+ tsdf_buffer*
+ getCyclicalBufferStructure ()
+ {
+ return (cyclical_.getBuffer ());
+ }
+
+ /** \brief Extract the world and save it.
+ */
+ void
+ extractAndSaveWorld ();
+
+ /** \brief Returns true if ICP is currently lost */
+ bool
+ icpIsLost ()
+ {
+ return (lost_);
+ }
+
+ /** \brief Performs the tracker reset to initial state. It's used if camera tracking fails. */
+ void
+ reset ();
+
+ void
+ setDisableICP ()
+ {
+ disable_icp_ = !disable_icp_;
+ PCL_WARN("ICP is %s\n", !disable_icp_?"ENABLED":"DISABLED");
+ }
+
+ /** \brief Return whether the last update resulted in a shift */
+ inline bool
+ hasShifted () const
+ {
+ return (has_shifted_);
+ }
+
+ private:
+
+ /** \brief Allocates all GPU internal buffers.
+ * \param[in] rows_arg
+ * \param[in] cols_arg
+ */
+ void
+ allocateBufffers (int rows_arg, int cols_arg);
+
+ /** \brief Number of pyramid levels */
+ enum { LEVELS = 3 };
+
+ /** \brief ICP Correspondences map type */
+ typedef DeviceArray2D<int> CorespMap;
+
+ /** \brief Vertex or Normal Map type */
+ typedef DeviceArray2D<float> MapArr;
+
+ typedef Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Matrix3frm;
+ typedef Eigen::Vector3f Vector3f;
+
+ /** \brief helper function that converts transforms from host to device types
+ * \param[in] transformIn1 first transform to convert
+ * \param[in] transformIn2 second transform to convert
+ * \param[in] translationIn1 first translation to convert
+ * \param[in] translationIn2 second translation to convert
+ * \param[out] transformOut1 result of first transform conversion
+ * \param[out] transformOut2 result of second transform conversion
+ * \param[out] translationOut1 result of first translation conversion
+ * \param[out] translationOut2 result of second translation conversion
+ */
+ inline void
+ convertTransforms (Matrix3frm& transform_in_1, Matrix3frm& transform_in_2, Eigen::Vector3f& translation_in_1, Eigen::Vector3f& translation_in_2,
+ pcl::device::kinfuLS::Mat33& transform_out_1, pcl::device::kinfuLS::Mat33& transform_out_2, float3& translation_out_1, float3& translation_out_2);
+
+ /** \brief helper function that converts transforms from host to device types
+ * \param[in] transformIn1 first transform to convert
+ * \param[in] transformIn2 second transform to convert
+ * \param[in] translationIn translation to convert
+ * \param[out] transformOut1 result of first transform conversion
+ * \param[out] transformOut2 result of second transform conversion
+ * \param[out] translationOut result of translation conversion
+ */
+ inline void
+ convertTransforms (Matrix3frm& transform_in_1, Matrix3frm& transform_in_2, Eigen::Vector3f& translation_in,
+ pcl::device::kinfuLS::Mat33& transform_out_1, pcl::device::kinfuLS::Mat33& transform_out_2, float3& translation_out);
+
+ /** \brief helper function that converts transforms from host to device types
+ * \param[in] transformIn transform to convert
+ * \param[in] translationIn translation to convert
+ * \param[out] transformOut result of transform conversion
+ * \param[out] translationOut result of translation conversion
+ */
+ inline void
+ convertTransforms (Matrix3frm& transform_in, Eigen::Vector3f& translation_in,
+ pcl::device::kinfuLS::Mat33& transform_out, float3& translation_out);
+
+ /** \brief helper function that pre-process a raw detph map the kinect fusion algorithm.
+ * The raw depth map is first blured, eventually truncated, and downsampled for each pyramid level.
+ * Then, vertex and normal maps are computed for each pyramid level.
+ * \param[in] depth_raw the raw depth map to process
+ * \param[in] cam_intrinsics intrinsics of the camera used to acquire the depth map
+ */
+ inline void
+ prepareMaps (const DepthMap& depth_raw, const pcl::device::kinfuLS::Intr& cam_intrinsics);
+
+ /** \brief helper function that performs GPU-based ICP, using vertex and normal maps stored in v/nmaps_curr_ and v/nmaps_g_prev_
+ * The function requires the previous local camera pose (translation and inverted rotation) as well as camera intrinsics.
+ * It will return the newly computed pose found as global rotation and translation.
+ * \param[in] cam_intrinsics intrinsics of the camera
+ * \param[in] previous_global_rotation previous local rotation of the camera
+ * \param[in] previous_global_translation previous local translation of the camera
+ * \param[out] current_global_rotation computed global rotation
+ * \param[out] current_global_translation computed global translation
+ * \return true if ICP has converged.
+ */
+ inline bool
+ performICP(const pcl::device::kinfuLS::Intr& cam_intrinsics, Matrix3frm& previous_global_rotation, Vector3f& previous_global_translation, Matrix3frm& current_global_rotation, Vector3f& current_global_translation);
+
+
+ /** \brief helper function that performs GPU-based ICP, using the current and the previous depth-maps (i.e. not using the synthetic depth map generated from the tsdf-volume)
+ * The function requires camera intrinsics.
+ * It will return the transformation between the previous and the current depth map.
+ * \param[in] cam_intrinsics intrinsics of the camera
+ * \param[out] resulting_rotation computed global rotation
+ * \param[out] resulting_translation computed global translation
+ * \return true if ICP has converged.
+ */
+ inline bool
+ performPairWiseICP(const pcl::device::kinfuLS::Intr cam_intrinsics, Matrix3frm& resulting_rotation, Vector3f& resulting_translation);
+
+ /** \brief Helper function that copies v_maps_curr and n_maps_curr to v_maps_prev_ and n_maps_prev_ */
+ inline void
+ saveCurrentMaps();
+
+ /** \brief Cyclical buffer object */
+ CyclicalBuffer cyclical_;
+
+ /** \brief Height of input depth image. */
+ int rows_;
+
+ /** \brief Width of input depth image. */
+ int cols_;
+
+ /** \brief Frame counter */
+ int global_time_;
+
+ /** \brief Truncation threshold for depth image for ICP step */
+ float max_icp_distance_;
+
+ /** \brief Intrinsic parameters of depth camera. */
+ float fx_, fy_, cx_, cy_;
+
+ /** \brief Tsdf volume container. */
+ TsdfVolume::Ptr tsdf_volume_;
+
+ /** \brief Color volume container. */
+ ColorVolume::Ptr color_volume_;
+
+ /** \brief Initial camera rotation in volume coo space. */
+ Matrix3frm init_Rcam_;
+
+ /** \brief Initial camera position in volume coo space. */
+ Vector3f init_tcam_;
+
+ /** \brief array with IPC iteration numbers for each pyramid level */
+ int icp_iterations_[LEVELS];
+
+ /** \brief distance threshold in correspondences filtering */
+ float distThres_;
+
+ /** \brief angle threshold in correspondences filtering. Represents max sine of angle between normals. */
+ float angleThres_;
+
+ /** \brief Depth pyramid. */
+ std::vector<DepthMap> depths_curr_;
+
+ /** \brief Vertex maps pyramid for current frame in global coordinate space. */
+ std::vector<MapArr> vmaps_g_curr_;
+
+ /** \brief Normal maps pyramid for current frame in global coordinate space. */
+ std::vector<MapArr> nmaps_g_curr_;
+
+ /** \brief Vertex maps pyramid for previous frame in global coordinate space. */
+ std::vector<MapArr> vmaps_g_prev_;
+
+ /** \brief Normal maps pyramid for previous frame in global coordinate space. */
+ std::vector<MapArr> nmaps_g_prev_;
+
+ /** \brief Vertex maps pyramid for current frame in current coordinate space. */
+ std::vector<MapArr> vmaps_curr_;
+
+ /** \brief Normal maps pyramid for current frame in current coordinate space. */
+ std::vector<MapArr> nmaps_curr_;
+
+ /** \brief Vertex maps pyramid for previous frame in current coordinate space. */
+ std::vector<MapArr> vmaps_prev_;
+
+ /** \brief Normal maps pyramid for previous frame in current coordinate space. */
+ std::vector<MapArr> nmaps_prev_;
+
+ /** \brief Array of buffers with ICP correspondences for each pyramid level. */
+ std::vector<CorespMap> coresps_;
+
+ /** \brief Buffer for storing scaled depth image */
+ DeviceArray2D<float> depthRawScaled_;
+
+ /** \brief Temporary buffer for ICP */
+ DeviceArray2D<double> gbuf_;
+
+ /** \brief Buffer to store MLS matrix. */
+ DeviceArray<double> sumbuf_;
+
+ /** \brief Array of camera rotation matrices for each moment of time. */
+ std::vector<Matrix3frm> rmats_;
+
+ /** \brief Array of camera translations for each moment of time. */
+ std::vector<Vector3f> tvecs_;
+
+ /** \brief Camera movement threshold. TSDF is integrated iff a camera movement metric exceedes some value. */
+ float integration_metric_threshold_;
+
+ /** \brief When set to true, KinFu will extract the whole world and mesh it. */
+ bool perform_last_scan_;
+
+ /** \brief When set to true, KinFu notifies that it is finished scanning and can be stopped. */
+ bool finished_;
+
+ /** \brief // when the camera target point is farther than DISTANCE_THRESHOLD from the current cube's center, shifting occurs. In meters . */
+ float shifting_distance_;
+
+ /** \brief Size of the TSDF volume in meters. */
+ float volume_size_;
+
+ /** \brief True if ICP is lost */
+ bool lost_;
+
+ /** \brief Last estimated rotation (estimation is done via pairwise alignment when ICP is failing) */
+ Matrix3frm last_estimated_rotation_;
+
+ /** \brief Last estimated translation (estimation is done via pairwise alignment when ICP is failing) */
+ Vector3f last_estimated_translation_;
+
+
+ bool disable_icp_;
+
+ /** \brief True or false depending on if there was a shift in the last pose update */
+ bool has_shifted_;
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ };
+ }
+ }
+};
+
+#endif /* PCL_KINFU_KINFUTRACKER_HPP_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_KINFU_TSDF_MARCHING_CUBES_H_
+#define PCL_KINFU_TSDF_MARCHING_CUBES_H_
+
+#include <pcl/pcl_macros.h>
+#include <pcl/gpu/containers/device_array.h>
+#include <Eigen/Core>
+//#include <boost/graph/buffer_concepts.hpp>
+
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace kinfuLS
+ {
+ class TsdfVolume;
+
+ /** \brief MarchingCubes implements MarchingCubes functionality for TSDF volume on GPU
+ * \author Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+ class PCL_EXPORTS MarchingCubes
+ {
+ public:
+
+ /** \brief Default size for triangles buffer */
+ enum
+ {
+ POINTS_PER_TRIANGLE = 3,
+ DEFAULT_TRIANGLES_BUFFER_SIZE = 2 * 1000 * 1000 * POINTS_PER_TRIANGLE * 2
+ };
+
+ /** \brief Point type. */
+ typedef pcl::PointXYZ PointType;
+
+ /** \brief Smart pointer. */
+ typedef boost::shared_ptr<MarchingCubes> Ptr;
+
+ /** \brief Default constructor */
+ MarchingCubes();
+
+ /** \brief Destructor */
+ ~MarchingCubes();
+
+ /** \brief Runs marching cubes triangulation.
+ * \param[in] tsdf
+ * \param[in] triangles_buffer Buffer for triangles. Its size determines max extracted triangles. If empty, it will be allocated with default size will be used.
+ * \return Array with triangles. Each 3 consequent poits belond to a single triangle. The returned array points to 'triangles_buffer' data.
+ */
+ DeviceArray<PointType>
+ run(const TsdfVolume& tsdf, DeviceArray<PointType>& triangles_buffer);
+
+ private:
+ /** \brief Edge table for marching cubes */
+ DeviceArray<int> edgeTable_;
+
+ /** \brief Number of vertextes table for marching cubes */
+ DeviceArray<int> numVertsTable_;
+
+ /** \brief Triangles table for marching cubes */
+ DeviceArray<int> triTable_;
+
+ /** \brief Temporary buffer used by marching cubes (first row stores occuped voxes id, second number of vetexes, third poits offsets */
+ DeviceArray2D<int> occupied_voxels_buffer_;
+ };
+ }
+ }
+}
+
+#endif /* PCL_KINFU_MARCHING_CUBES_H_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_KINFU_PIXEL_RGB_HPP_
+#define PCL_KINFU_PIXEL_RGB_HPP_
+//#include <boost/graph/buffer_concepts.hpp>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace kinfuLS
+ {
+ /** \brief Input/output pixel format for KinfuTracker */
+
+ struct PixelRGB
+ {
+ unsigned char r, g, b;
+ };
+ }
+ }
+}
+
+#endif /* PCL_KINFU_PIXEL_RGB_HPP_ */
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <iostream>
+#include <pcl/point_types.h>
+#include <pcl/io/pcd_io.h>
+
+
+#ifndef PCL_KINFU_POINT_INTENSITY_
+#define PCL_KINFU_POINT_INTENSITY_
+
+struct EIGEN_ALIGN16 PointIntensity
+{
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+ union
+ {
+ struct
+ {
+ float intensity;
+ };
+ float data[4];
+ };
+};
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+ PointIntensity,
+ (float, intensity, intensity) )
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+
+#ifndef PCL_KINFU_TSDF_RAYCASTERLS_H_
+#define PCL_KINFU_TSDF_RAYCASTERLS_H_
+
+
+#include <pcl/pcl_macros.h>
+#include <pcl/point_types.h>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/kinfu_large_scale/pixel_rgb.h>
+#include <boost/shared_ptr.hpp>
+//#include <boost/graph/buffer_concepts.hpp>
+#include <Eigen/Geometry>
+
+#include <pcl/gpu/kinfu_large_scale/tsdf_buffer.h>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace kinfuLS
+ {
+ class TsdfVolume;
+
+ /** \brief Class that performs raycasting for TSDF volume
+ * \author Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+ struct PCL_EXPORTS RayCaster
+ {
+ public:
+ typedef boost::shared_ptr<RayCaster> Ptr;
+ typedef pcl::gpu::DeviceArray2D<float> MapArr;
+ typedef pcl::gpu::DeviceArray2D<PixelRGB> View;
+ typedef pcl::gpu::DeviceArray2D<unsigned short> Depth;
+
+ /** \brief Image with height */
+ const int cols, rows;
+
+ /** \brief Constructor
+ * \param[in] rows image rows
+ * \param[in] cols image cols
+ * \param[in] fx focal x
+ * \param[in] fy focal y
+ * \param[in] cx principal point x
+ * \param[in] cy principal point y
+ */
+ RayCaster(int rows = 480, int cols = 640, float fx = 525.f, float fy = 525.f, float cx = -1, float cy = -1);
+
+ ~RayCaster();
+
+ /** \brief Sets camera intrinsics */
+ void
+ setIntrinsics(float fx = 525.f, float fy = 525.f, float cx = -1, float cy = -1);
+
+ /** \brief Runs raycasting algorithm from given camera pose. It writes results to internal fiels.
+ * \param[in] volume tsdf volume container
+ * \param[in] camera_pose camera pose
+ * \param buffer
+ */
+ void
+ run(const TsdfVolume& volume, const Eigen::Affine3f& camera_pose, tsdf_buffer* buffer);
+
+ /** \brief Generates scene view using data raycasted by run method. So call it before.
+ * \param[out] view output array for RGB image
+ */
+ void
+ generateSceneView(View& view) const;
+
+ /** \brief Generates scene view using data raycasted by run method. So call it before.
+ * \param[out] view output array for RGB image
+ * \param[in] light_source_pose pose of light source
+ */
+ void
+ generateSceneView(View& view, const Eigen::Vector3f& light_source_pose) const;
+
+ /** \brief Generates depth image using data raycasted by run method. So call it before.
+ * \param[out] depth output array for depth image
+ */
+ void
+ generateDepthImage(Depth& depth) const;
+
+ /** \brief Returns raycasterd vertex map. */
+ MapArr
+ getVertexMap() const;
+
+ /** \brief Returns raycasterd normal map. */
+ MapArr
+ getNormalMap() const;
+
+ private:
+ /** \brief Camera intrinsics. */
+ float fx_, fy_, cx_, cy_;
+
+ /* Vertext/normal map internal representation example for rows=2 and cols=4
+ * X X X X
+ * X X X X
+ * Y Y Y Y
+ * Y Y Y Y
+ * Z Z Z Z
+ * Z Z Z Z
+ */
+
+ /** \brief vertex map of 3D points*/
+ MapArr vertex_map_;
+
+ /** \brief normal map of 3D points*/
+ MapArr normal_map_;
+
+ /** \brief camera pose from which raycasting was done */
+ Eigen::Affine3f camera_pose_;
+
+ /** \brief Last passed volume size */
+ Eigen::Vector3f volume_size_;
+
+public:
+EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ };
+
+ /** \brief Converts from map representation to organized not-dence point cloud. */
+ template<typename PointType>
+ void convertMapToOranizedCloud(const RayCaster::MapArr& map, pcl::gpu::DeviceArray2D<PointType>& cloud);
+ }
+ }
+}
+
+#endif /* PCL_KINFU_TSDF_RAYCASTER_H_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Francisco, Technical University Eindhoven, (f.j.mysurname.soriano <At > tue.nl)
+ */
+
+#ifndef PCL_SCREENSHOT_MANAGER_H_
+#define PCL_SCREENSHOT_MANAGER_H_
+
+#include <iostream>
+#include <fstream>
+#include <stdio.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <pcl/pcl_exports.h>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/containers/kernel_containers.h>
+#include <pcl/gpu/kinfu_large_scale/pixel_rgb.h>
+#include <boost/lexical_cast.hpp>
+#include <boost/filesystem.hpp>
+//#include <boost/graph/buffer_concepts.hpp>
+
+#include <pcl/io/png_io.h>
+
+#include <pcl/console/print.h>
+
+
+namespace pcl
+{
+ namespace kinfuLS
+ {
+ /** \brief Screenshot Manager saves a screenshot with the corresponding camera pose from Kinfu. Please create a folder named "KinFuSnapshots" in the folder where you call kinfu.
+ * \author Francisco Heredia
+ */
+ class PCL_EXPORTS ScreenshotManager
+ {
+ public:
+
+ typedef pcl::gpu::kinfuLS::PixelRGB PixelRGB;
+
+ /** Constructor */
+ ScreenshotManager();
+
+ /** Destructor */
+ ~ScreenshotManager(){}
+
+ /** \brief Sets Depth camera intrinsics
+ * \param[in] focal focal length x
+ * \param height
+ * \param width
+ */
+ void
+ setCameraIntrinsics (float focal = 575.816f, float height = 480.0f, float width = 640.0f);
+
+ /**Save Screenshot*/
+ void
+ saveImage(const Eigen::Affine3f &camPose, pcl::gpu::PtrStepSz<const PixelRGB> rgb24);
+
+ private:
+
+ /**Write camera pose to file*/
+ void
+ writePose(const std::string &filename_pose, const Eigen::Vector3f &teVecs, const Eigen::Matrix<float, 3, 3, Eigen::RowMajor> &erreMats);
+
+ /**Counter of the number of screenshots taken*/
+ int screenshot_counter;
+
+ /** \brief Intrinsic parameters of depth camera. */
+ float focal_, height_, width_;
+ };
+ }
+}
+
+#endif // PCL_SCREENSHOT_MANAGER_H_
--- /dev/null
+ /*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+ #ifndef PCL_STANDALONE_MARCHING_CUBES_H_
+ #define PCL_STANDALONE_MARCHING_CUBES_H_
+
+//General includes and I/O
+
+#include <iostream>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/vtk_io.h>
+#include <pcl/point_types.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include <pcl/pcl_macros.h>
+
+//Marching cubes includes
+#include <pcl/gpu/kinfu_large_scale/marching_cubes.h>
+#include <pcl/PolygonMesh.h>
+
+#include <pcl/gpu/containers/device_array.h>
+
+//TSDF volume includes
+#include <pcl/gpu/kinfu_large_scale/tsdf_volume.h>
+
+//Eigen Geometry and Transforms
+#include <pcl/common/transforms.h>
+#include <Eigen/Geometry>
+//#include <boost/graph/buffer_concepts.hpp>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace kinfuLS
+ {
+ /** \brief The Standalone Marching Cubes Class provides encapsulated functionality for the Marching Cubes implementation originally by Anatoly Baksheev.
+ * \author Raphael Favier
+ * \author Francisco Heredia
+ */
+
+ template <typename PointT>
+ class StandaloneMarchingCubes
+ {
+ public:
+ typedef typename pcl::PointCloud<PointT> PointCloud;
+ typedef typename pcl::PointCloud<PointT>::Ptr PointCloudPtr;
+ typedef boost::shared_ptr<pcl::PolygonMesh> MeshPtr;
+
+ /** \brief Constructor
+ */
+ StandaloneMarchingCubes (int voxels_x = 512, int voxels_y = 512, int voxels_z = 512, float volume_size = 3.0f);
+
+ /** \brief Destructor
+ */
+ ~StandaloneMarchingCubes (){}
+
+ /** \brief Run marching cubes in a TSDF cloud and returns a PolygonMesh. Input X,Y,Z coordinates must be in indices of the TSDF volume grid, output is in meters.
+ * \param[in] cloud TSDF cloud with indices between [0 ... VOXELS_X][0 ... VOXELS_Y][0 ... VOXELS_Z]. Intensity value corresponds to the TSDF value in that coordinate.
+ * \return pointer to a PolygonMesh in meters generated by marching cubes.
+ */
+ MeshPtr
+ getMeshFromTSDFCloud (const PointCloud &cloud);
+
+ /** \brief Runs marching cubes on every pointcloud in the vector. Returns a vector containing the PolygonMeshes.
+ * \param[in] tsdf_clouds Vector of TSDF Clouds
+ * \param[in] tsdf_offsets Vector of the offsets for every pointcloud in TsdfClouds. This offset (in indices) indicates the position of the cloud with respect to the absolute origin of the world model
+ */
+ void
+ getMeshesFromTSDFVector (const std::vector<PointCloudPtr> &tsdf_clouds, const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &tsdf_offsets);
+
+ /** \brief Returns the associated Tsdf Volume buffer in GPU
+ * \return pointer to the Tsdf Volume buffer in GPU
+ */
+ TsdfVolume::Ptr
+ tsdfVolumeGPU ();
+
+ /** \brief Returns the associated Tsdf Volume buffer in CPU
+ * \return the Tsdf Volume buffer in CPU returned returned by reference
+ */
+ std::vector<int>&
+ tsdfVolumeCPU ();
+
+ protected:
+
+ /** \brief Loads a TSDF Cloud to the TSDF Volume in GPU
+ * \param[in] cloud TSDF cloud that will be loaded. X,Y,Z of the cloud will only be loaded if their range is between [0 ... VOXELS_X][0 ... VOXELS_Y][0 ... VOXELS_Z]
+ */
+ void
+ loadTsdfCloudToGPU (const PointCloud &cloud);
+
+ /** \brief Read the data in the point cloud. Performs a conversion to a suitable format for the TSDF Volume. Loads the converted data to the output vector.
+ * \param[in] cloud point cloud to be converted
+ * \param[out] output the vector of converted values, ready to be loaded to the GPU.
+ */
+ void
+ convertTsdfVectors (const PointCloud &cloud, std::vector<int> &output);
+
+ /** \brief Converts the triangles buffer device to a PolygonMesh.
+ * \param[in] triangles the triangles buffer containing the points of the mesh
+ * \return pointer to the PolygonMesh egnerated by marchign cubes
+ */
+ MeshPtr
+ convertTrianglesToMesh (const pcl::gpu::DeviceArray<pcl::PointXYZ>& triangles);
+
+ /** \brief Runs marching cubes on the data that is contained in the TSDF Volume in GPU.
+ * \return param[return] pointer to a PolygonMesh in meters generated by marching cubes.
+ */
+ MeshPtr
+ runMarchingCubes ();
+
+ private:
+
+ /** The TSDF volume in GPU*/
+ TsdfVolume::Ptr tsdf_volume_gpu_;
+
+ /** The TSDF volume in CPU */
+ std::vector<int> tsdf_volume_cpu_;
+
+ /** Number of voxels in the grid for each axis */
+ int voxels_x_;
+ int voxels_y_;
+ int voxels_z_;
+
+ /** Tsdf volume size in meters. Should match the ones in internal.h */
+ float volume_size_;
+
+ /** Mesh counter used to name the output meshes */
+ int mesh_counter_;
+
+ };
+ }
+ }
+}
+
+#define PCL_INSTANTIATE_StandaloneMarchingCubes(PointT) template class PCL_EXPORTS pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>;
+
+#endif // PCL_STANDALONE_MARCHING_CUBES_H_
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_TSDF_BUFFER_STRUCT_H_
+#define PCL_TSDF_BUFFER_STRUCT_H_
+
+#include <cuda_runtime.h>
+//#include <boost/graph/buffer_concepts.hpp>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace kinfuLS
+ {
+ /** \brief Structure to handle buffer addresses */
+ struct tsdf_buffer
+ {
+ /** \brief */
+ /** \brief Address of the first element of the TSDF volume in memory*/
+ short2* tsdf_memory_start;
+ /** \brief Address of the last element of the TSDF volume in memory*/
+ short2* tsdf_memory_end;
+ /** \brief Memory address of the origin of the rolling buffer. MUST BE UPDATED AFTER EACH SHIFT.*/
+ short2* tsdf_rolling_buff_origin;
+ /** \brief Internal cube origin for rollign buffer.*/
+ int3 origin_GRID;
+ /** \brief Cube origin in world coordinates.*/
+ float3 origin_GRID_global;
+ /** \brief Current metric origin of the cube, in world coordinates.*/
+ float3 origin_metric;
+ /** \brief Size of the volume, in meters.*/
+ float3 volume_size; //3.0
+ /** \brief Number of voxels in the volume, per axis*/
+ int3 voxels_size; //512
+
+ /** \brief Default constructor*/
+ tsdf_buffer ()
+ {
+ tsdf_memory_start = 0; tsdf_memory_end = 0; tsdf_rolling_buff_origin = 0;
+ origin_GRID.x = 0; origin_GRID.y = 0; origin_GRID.z = 0;
+ origin_GRID_global.x = 0.f; origin_GRID_global.y = 0.f; origin_GRID_global.z = 0.f;
+ origin_metric.x = 0.f; origin_metric.y = 0.f; origin_metric.z = 0.f;
+ volume_size.x = 3.f; volume_size.y = 3.f; volume_size.z = 3.f;
+ voxels_size.x = 512; voxels_size.y = 512; voxels_size.z = 512;
+ }
+
+ };
+ }
+ }
+}
+
+#endif /*PCL_TSDF_BUFFER_STRUCT_H_*/
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_KINFU_TSDF_VOLUME_H_
+#define PCL_KINFU_TSDF_VOLUME_H_
+
+#include <pcl/pcl_macros.h>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <Eigen/Core>
+#include <vector>
+
+#include <pcl/gpu/kinfu_large_scale/tsdf_buffer.h>
+
+#include <pcl/gpu/kinfu_large_scale/point_intensity.h>
+
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace kinfuLS
+ {
+ /** \brief TsdfVolume class
+ * \author Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+ class PCL_EXPORTS TsdfVolume
+ {
+ public:
+ typedef boost::shared_ptr<TsdfVolume> Ptr;
+
+ /** \brief Supported Point Types */
+ typedef PointXYZ PointType;
+ typedef Normal NormalType;
+
+ /** \brief Structure storing voxel grid resolution, volume size (in mm) and element_size of data stored on host*/
+ struct Header
+ {
+ Eigen::Vector3i resolution;
+ Eigen::Vector3f volume_size;
+ int volume_element_size, weights_element_size;
+
+ Header ()
+ : resolution (0,0,0),
+ volume_size (0,0,0),
+ volume_element_size (sizeof(float)),
+ weights_element_size (sizeof(short))
+ {};
+
+ Header (const Eigen::Vector3i &res, const Eigen::Vector3f &size)
+ : resolution (res),
+ volume_size (size),
+ volume_element_size (sizeof(float)),
+ weights_element_size (sizeof(short))
+ {};
+
+ /** \brief Get the size of data stored on host*/
+ inline size_t
+ getVolumeSize () const { return resolution[0] * resolution[1] * resolution[2]; };
+
+ friend inline std::ostream&
+ operator << (std::ostream& os, const Header& h)
+ {
+ os << "(resolution = " << h.resolution.transpose() << ", volume size = " << h.volume_size.transpose() << ")";
+ return (os);
+ }
+
+ public:
+EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+
+ /** \brief Default buffer size for fetching cloud. It limits max number of points that can be extracted */
+ enum { DEFAULT_CLOUD_BUFFER_SIZE = 10 * 1000 * 1000 };
+
+ /** \brief Constructor
+ * \param[in] resolution volume resolution
+ */
+ TsdfVolume (const Eigen::Vector3i& resolution);
+
+ /** \brief Sets Tsdf volume size for each dimention
+ * \param[in] size size of tsdf volume in meters
+ */
+ void
+ setSize (const Eigen::Vector3f& size);
+
+ /** \brief Sets Tsdf truncation distance. Must be greater than 2 * volume_voxel_size
+ * \param[in] distance TSDF truncation distance
+ */
+ void
+ setTsdfTruncDist (float distance);
+
+ /** \brief Returns tsdf volume container that point to data in GPU memroy */
+ DeviceArray2D<int>
+ data () const;
+
+ /** \brief Returns volume size in meters */
+ const Eigen::Vector3f&
+ getSize () const;
+
+ /** \brief Returns volume resolution */
+ const Eigen::Vector3i&
+ getResolution() const;
+
+ /** \brief Returns volume voxel size in meters */
+ const Eigen::Vector3f
+ getVoxelSize () const;
+
+ /** \brief Returns tsdf truncation distance in meters */
+ float
+ getTsdfTruncDist () const;
+
+ /** \brief Resets tsdf volume data to uninitialized state */
+ void
+ reset ();
+
+ /** \brief Generates cloud using CPU (downloads volumetric representation to CPU memory)
+ * \param[out] cloud output array for cloud
+ * \param[in] connected26 If false point cloud is extracted using 6 neighbor, otherwise 26.
+ */
+ void
+ fetchCloudHost (PointCloud<PointType>& cloud, bool connected26 = false) const;
+
+ /** \brief Generates cloud using CPU (downloads volumetric representation to CPU memory)
+ * \param[out] cloud output array for cloud
+ * \param[in] connected26 If false point cloud is extracted using 6 neighbor, otherwise 26.
+ */
+ void
+ fetchCloudHost (PointCloud<PointXYZI>& cloud, bool connected26 = false) const;
+
+ /** \brief Generates cloud using GPU in connected6 mode only
+ * \param[out] cloud_buffer buffer to store point cloud
+ * \return DeviceArray with disabled reference counting that points to filled part of cloud_buffer.
+ */
+ DeviceArray<PointType>
+ fetchCloud (DeviceArray<PointType>& cloud_buffer) const;
+
+ /** \brief Push a point cloud of previously scanned tsdf slice to the TSDF volume
+ * \param[in] existing_data_cloud point cloud pointer to the existing data. This data will be pushed to the TSDf volume. The points with indices outside the range [0 ... VOLUME_X - 1][0 ... VOLUME_Y - 1][0 ... VOLUME_Z - 1] will not be added.
+ * \param buffer
+ */
+ void
+ pushSlice (const PointCloud<PointXYZI>::Ptr existing_data_cloud, const tsdf_buffer* buffer) const;
+
+ /** \brief Generates cloud using GPU in connected6 mode only
+ * \param[out] cloud_buffer_xyz buffer to store point cloud
+ * \param cloud_buffer_intensity
+ * \param[in] buffer Pointer to the buffer struct that contains information about memory addresses of the tsdf volume memory block, which are used for the cyclic buffer.
+ * \param[in] shiftX Offset in indices.
+ * \param[in] shiftY Offset in indices.
+ * \param[in] shiftZ Offset in indices.
+ * \return DeviceArray with disabled reference counting that points to filled part of cloud_buffer.
+ */
+ size_t
+ fetchSliceAsCloud (DeviceArray<PointType>& cloud_buffer_xyz, DeviceArray<float>& cloud_buffer_intensity, const tsdf_buffer* buffer, int shiftX, int shiftY, int shiftZ ) const;
+
+ /** \brief Computes normals as gradient of tsdf for given points
+ * \param[in] cloud Points where normals are computed.
+ * \param[out] normals array for normals
+ */
+
+ void
+ fetchNormals (const DeviceArray<PointType>& cloud, DeviceArray<PointType>& normals) const;
+
+ /** \brief Computes normals as gradient of tsdf for given points
+ * \param[in] cloud Points where normals are computed.
+ * \param[out] normals array for normals
+ */
+ void
+ fetchNormals(const DeviceArray<PointType>& cloud, DeviceArray<NormalType>& normals) const;
+
+ /** \brief Downloads tsdf volume from GPU memory.
+ * \param[out] tsdf Array with tsdf values. if volume resolution is 512x512x512, so for voxel (x,y,z) tsdf value can be retrieved as volume[512*512*z + 512*y + x];
+ */
+ void
+ downloadTsdf (std::vector<float>& tsdf) const;
+
+ /** \brief Downloads tsdf volume from GPU memory to local CPU buffer*/
+ void
+ downloadTsdfLocal () const;
+
+ /** \brief Downloads TSDF volume and according voxel weights from GPU memory
+ * \param[out] tsdf Array with tsdf values. if volume resolution is 512x512x512, so for voxel (x,y,z) tsdf value can be retrieved as volume[512*512*z + 512*y + x];
+ * \param[out] weights Array with tsdf voxel weights. Same size and access index as for tsdf. A weight of 0 indicates the voxel was never used.
+ */
+ void
+ downloadTsdfAndWeights (std::vector<float>& tsdf, std::vector<short>& weights) const;
+
+ /** \brief Downloads TSDF volume and according voxel weights from GPU memory to local CPU buffers*/
+ void
+ downloadTsdfAndWeightsLocal () const;
+
+ /** \brief Releases tsdf buffer on GPU */
+ void releaseVolume () {volume_.release();}
+
+ void print_warn(const char* arg1, size_t size);
+
+ /** \brief Set the header for data stored on host directly. Useful if directly writing into volume and weights */
+ inline void
+ setHeader (const Eigen::Vector3i& resolution, const Eigen::Vector3f& volume_size) {
+ header_ = Header (resolution, volume_size);
+ if (volume_host_->size() != this->size())
+ pcl::console::print_warn ("[TSDFVolume::setHeader] Header volume size (%d) doesn't fit underlying data size (%d)", volume_host_->size(), size());
+ }
+
+ /** \brief Returns overall number of voxels in grid stored on host */
+ inline size_t
+ size () const {
+ return header_.getVolumeSize ();
+ }
+
+ /** \brief Converts volume stored on host to cloud of TSDF values*/
+ void
+ convertToTsdfCloud (pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) const;
+
+ /** \brief Returns the voxel grid resolution */
+ inline const Eigen::Vector3i &
+ gridResolution () const { return header_.resolution; };
+
+ /** \brief Saves local volume buffer to file */
+ bool
+ save (const std::string &filename = "tsdf_volume.dat", bool binary = true) const;
+
+ /** \brief Loads local volume from file */
+ bool
+ load (const std::string &filename, bool binary = true);
+
+ private:
+ /** \brief tsdf volume size in meters */
+ Eigen::Vector3f size_;
+
+ /** \brief tsdf volume resolution */
+ Eigen::Vector3i resolution_;
+
+ /** \brief tsdf volume data container */
+ DeviceArray2D<int> volume_;
+
+ /** \brief tsdf truncation distance */
+ float tranc_dist_;
+
+ // The following member are resulting from the merge of TSDFVolume with TsdfVolume class.
+
+ typedef boost::shared_ptr<std::vector<float> > VolumePtr;
+ typedef boost::shared_ptr<std::vector<short> > WeightsPtr;
+
+ Header header_;
+ VolumePtr volume_host_;
+ WeightsPtr weights_host_;
+public:
+EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+ }
+ }
+}
+
+#endif /* PCL_KINFU_TSDF_VOLUME_H_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Raphael Favier, Technical University Eindhoven, (r.mysurname <aT> tue.nl)
+ */
+
+#ifndef PCL_WORLD_MODEL_H_
+#define PCL_WORLD_MODEL_H_
+
+#include <pcl/common/impl/common.hpp>
+#include <pcl/octree/octree.h>
+#include <pcl/octree/octree_impl.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/filters/filter_indices.h>
+#include <pcl/filters/crop_box.h>
+#include <pcl/filters/conditional_removal.h>
+#include <pcl/point_types.h>
+#include <pcl/io/pcd_io.h>
+//#include <pcl/gpu/kinfu_large_scale/tsdf_buffer.h>
+//#include <boost/graph/buffer_concepts.hpp>
+
+
+namespace pcl
+{
+ namespace kinfuLS
+ {
+ /** \brief WorldModel maintains a 3D point cloud that can be queried and updated via helper functions.\n
+ * The world is represented as a point cloud.\n
+ * When new points are added to the world, we replace old ones by the newest ones.
+ * This is acheived by setting old points to nan (for speed)
+ * \author Raphael Favier
+ */
+ template <typename PointT>
+ class WorldModel
+ {
+ public:
+
+ typedef boost::shared_ptr<WorldModel<PointT> > Ptr;
+ typedef boost::shared_ptr<const WorldModel<PointT> > ConstPtr;
+
+ typedef pcl::PointCloud<PointT> PointCloud;
+ typedef typename PointCloud::Ptr PointCloudPtr;
+ typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+ typedef typename pcl::ConditionAnd<PointT>::Ptr ConditionAndPtr;
+ typedef typename pcl::ConditionOr<PointT>::Ptr ConditionOrPtr;
+ typedef typename pcl::FieldComparison<PointT>::ConstPtr FieldComparisonConstPtr;
+
+ typedef typename pcl::traits::fieldList<PointT>::type FieldList;
+
+ /** \brief Default constructor for the WorldModel.
+ */
+ WorldModel() :
+ world_ (new PointCloud)
+ {
+ world_->is_dense = false;
+ }
+
+ /** \brief Clear the world.
+ */
+ void reset()
+ {
+ if(world_->points.size () != 0)
+ {
+ PCL_WARN("Clearing world model\n");
+ world_->points.clear ();
+ }
+ }
+
+ /** \brief Append a new point cloud (slice) to the world.
+ * \param[in] new_cloud the point cloud to add to the world
+ */
+ void addSlice (const PointCloudPtr new_cloud);
+
+
+ /** \brief Retreive existing data from the world model, after a shift
+ * \param[in] previous_origin_x global origin of the cube on X axis, before the shift
+ * \param[in] previous_origin_y global origin of the cube on Y axis, before the shift
+ * \param[in] previous_origin_z global origin of the cube on Z axis, before the shift
+ * \param[in] offset_x shift on X, in indices
+ * \param[in] offset_y shift on Y, in indices
+ * \param[in] offset_z shift on Z, in indices
+ * \param[in] volume_x size of the cube, X axis, in indices
+ * \param[in] volume_y size of the cube, Y axis, in indices
+ * \param[in] volume_z size of the cube, Z axis, in indices
+ * \param[out] existing_slice the extracted point cloud representing the slice
+ */
+ void getExistingData(const double previous_origin_x, const double previous_origin_y, const double previous_origin_z,
+ const double offset_x, const double offset_y, const double offset_z,
+ const double volume_x, const double volume_y, const double volume_z, pcl::PointCloud<PointT> &existing_slice);
+
+ /** \brief Give nan values to the slice of the world
+ * \param[in] origin_x global origin of the cube on X axis, before the shift
+ * \param[in] origin_y global origin of the cube on Y axis, before the shift
+ * \param[in] origin_z global origin of the cube on Z axis, before the shift
+ * \param[in] offset_x shift on X, in indices
+ * \param[in] offset_y shift on Y, in indices
+ * \param[in] offset_z shift on Z, in indices
+ * \param[in] size_x size of the cube, X axis, in indices
+ * \param[in] size_y size of the cube, Y axis, in indices
+ * \param[in] size_z size of the cube, Z axis, in indices
+ */
+ void setSliceAsNans (const double origin_x, const double origin_y, const double origin_z,
+ const double offset_x, const double offset_y, const double offset_z,
+ const int size_x, const int size_y, const int size_z);
+
+ /** \brief Remove points with nan values from the world.
+ */
+ void cleanWorldFromNans ()
+ {
+ world_->is_dense = false;
+ std::vector<int> indices;
+ pcl::removeNaNFromPointCloud (*world_, *world_, indices);
+ }
+
+ /** \brief Returns the world as a point cloud.
+ */
+ PointCloudPtr getWorld ()
+ {
+ return (world_);
+ }
+
+ /** \brief Returns the number of points contained in the world.
+ */
+ size_t getWorldSize ()
+ {
+ return (world_->points.size () );
+ }
+
+ /** \brief Returns the world as two vectors of cubes of size "size" (pointclouds) and transforms
+ * \param[in] size the size of a 3D cube.
+ * \param[out] cubes a vector of point clouds representing each cube (in their original world coordinates).
+ * \param[out] transforms a vector containing the xyz position of each cube in world coordinates.
+ * \param[in] overlap optional overlap (in percent) between each cube (usefull to create overlapped meshes).
+ */
+ void getWorldAsCubes (double size, std::vector<PointCloudPtr> &cubes, std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &transforms, double overlap = 0.0);
+
+
+ private:
+
+ /** \brief cloud containing our world */
+ PointCloudPtr world_;
+
+ /** \brief set the points which index is in the indices vector to nan
+ * \param[in] cloud the cloud that contains the point to be set to nan
+ * \param[in] indices the vector of indices to set to nan
+ */
+ inline void setIndicesAsNans (PointCloudPtr cloud, IndicesConstPtr indices);
+
+ };
+ }
+}
+
+#endif // PCL_WORLD_MODEL_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/gpu/kinfu_large_scale/color_volume.h>
+#include <pcl/gpu/kinfu_large_scale/tsdf_volume.h>
+#include "internal.h"
+#include <algorithm>
+#include <Eigen/Core>
+
+using namespace pcl;
+using namespace Eigen;
+using pcl::device::kinfuLS::device_cast;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+pcl::gpu::kinfuLS::ColorVolume::ColorVolume(const TsdfVolume& tsdf, int max_weight) : resolution_(tsdf.getResolution()), volume_size_(tsdf.getSize()), max_weight_(1)
+{
+ max_weight_ = max_weight < 0 ? max_weight_ : max_weight;
+ max_weight_ = max_weight_ > 255 ? 255 : max_weight_;
+
+ int volume_x = resolution_(0);
+ int volume_y = resolution_(1);
+ int volume_z = resolution_(2);
+
+ color_volume_.create (volume_y * volume_z, volume_x);
+ reset();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+pcl::gpu::kinfuLS::ColorVolume::~ColorVolume()
+{
+
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::kinfuLS::ColorVolume::reset()
+{
+ pcl::device::kinfuLS::initColorVolume(color_volume_);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+int
+pcl::gpu::kinfuLS::ColorVolume::getMaxWeight() const
+{
+ return max_weight_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+pcl::gpu::DeviceArray2D<int>
+pcl::gpu::kinfuLS::ColorVolume::data() const
+{
+ return color_volume_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::kinfuLS::ColorVolume::fetchColors (const DeviceArray<PointType>& cloud, DeviceArray<RGB>& colors) const
+{
+ colors.create(cloud.size());
+ pcl::device::kinfuLS::exctractColors(color_volume_, device_cast<const float3> (volume_size_), cloud, (uchar4*)colors.ptr()/*bgra*/);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ const float sigma_color = 30; //in mm
+ const float sigma_space = 4.5; // in pixels
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ __global__ void
+ bilateralKernel (const PtrStepSz<ushort> src,
+ PtrStep<ushort> dst,
+ float sigma_space2_inv_half, float sigma_color2_inv_half)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x >= src.cols || y >= src.rows)
+ return;
+
+ const int R = 6; //static_cast<int>(sigma_space * 1.5);
+ const int D = R * 2 + 1;
+
+ int value = src.ptr (y)[x];
+
+ int tx = min (x - D / 2 + D, src.cols - 1);
+ int ty = min (y - D / 2 + D, src.rows - 1);
+
+ float sum1 = 0;
+ float sum2 = 0;
+
+ for (int cy = max (y - D / 2, 0); cy < ty; ++cy)
+ {
+ for (int cx = max (x - D / 2, 0); cx < tx; ++cx)
+ {
+ int tmp = src.ptr (cy)[cx];
+
+ float space2 = (x - cx) * (x - cx) + (y - cy) * (y - cy);
+ float color2 = (value - tmp) * (value - tmp);
+
+ float weight = __expf (-(space2 * sigma_space2_inv_half + color2 * sigma_color2_inv_half));
+
+ sum1 += tmp * weight;
+ sum2 += weight;
+ }
+ }
+
+ int res = __float2int_rn (sum1 / sum2);
+ dst.ptr (y)[x] = max (0, min (res, numeric_limits<short>::max ()));
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ __global__ void
+ pyrDownKernel (const PtrStepSz<ushort> src, PtrStepSz<ushort> dst, float sigma_color)
+ {
+ int x = blockIdx.x * blockDim.x + threadIdx.x;
+ int y = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if (x >= dst.cols || y >= dst.rows)
+ return;
+
+ const int D = 5;
+
+ int center = src.ptr (2 * y)[2 * x];
+
+ int tx = min (2 * x - D / 2 + D, src.cols - 1);
+ int ty = min (2 * y - D / 2 + D, src.rows - 1);
+ int cy = max (0, 2 * y - D / 2);
+
+ int sum = 0;
+ int count = 0;
+
+ for (; cy < ty; ++cy)
+ for (int cx = max (0, 2 * x - D / 2); cx < tx; ++cx)
+ {
+ int val = src.ptr (cy)[cx];
+ if (abs (val - center) < 3 * sigma_color)
+ {
+ sum += val;
+ ++count;
+ }
+ }
+ dst.ptr (y)[x] = sum / count;
+ }
+
+ __global__ void
+ truncateDepthKernel(PtrStepSz<ushort> depth, ushort max_distance_mm)
+ {
+ int x = blockIdx.x * blockDim.x + threadIdx.x;
+ int y = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if (x < depth.cols && y < depth.rows)
+ if(depth.ptr(y)[x] > max_distance_mm)
+ depth.ptr(y)[x] = 0;
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ bilateralFilter (const DepthMap& src, DepthMap& dst)
+ {
+ dim3 block (32, 8);
+ dim3 grid (divUp (src.cols (), block.x), divUp (src.rows (), block.y));
+
+ cudaFuncSetCacheConfig (bilateralKernel, cudaFuncCachePreferL1);
+ bilateralKernel<<<grid, block>>>(src, dst, 0.5f / (sigma_space * sigma_space), 0.5f / (sigma_color * sigma_color));
+
+ cudaSafeCall ( cudaGetLastError () );
+ };
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ pyrDown (const DepthMap& src, DepthMap& dst)
+ {
+ dst.create (src.rows () / 2, src.cols () / 2);
+
+ dim3 block (32, 8);
+ dim3 grid (divUp (dst.cols (), block.x), divUp (dst.rows (), block.y));
+
+ pyrDownKernel<<<grid, block>>>(src, dst, sigma_color);
+ cudaSafeCall ( cudaGetLastError () );
+ };
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ truncateDepth(DepthMap& depth, float max_distance)
+ {
+ dim3 block (32, 8);
+ dim3 grid (divUp (depth.cols (), block.x), divUp (depth.rows (), block.y));
+
+ truncateDepthKernel<<<grid, block>>>(depth, static_cast<ushort>(max_distance * 1000.f));
+
+ cudaSafeCall ( cudaGetLastError () );
+ }
+ }
+ }
+}
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+//#include <boost/graph/buffer_concepts.hpp>
+
+//#include "pcl/gpu/utils/device/vector_math.hpp"
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ __global__ void
+ initColorVolumeKernel (PtrStep<uchar4> volume)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x < VOLUME_X && y < VOLUME_Y)
+ {
+ uchar4 *pos = volume.ptr (y) + x;
+ int z_step = VOLUME_Y * volume.step / sizeof(*pos);
+
+ #pragma unroll
+ for (int z = 0; z < VOLUME_Z; ++z, pos += z_step)
+ *pos = make_uchar4 (0, 0, 0, 0);
+ }
+ }
+
+ void
+ initColorVolume (PtrStep<uchar4> color_volume)
+ {
+ dim3 block (32, 16);
+ dim3 grid (1, 1, 1);
+ grid.x = divUp (VOLUME_X, block.x);
+ grid.y = divUp (VOLUME_Y, block.y);
+
+ initColorVolumeKernel<<<grid, block>>>(color_volume);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+ }
+ }
+}
+
+
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ struct ColorVolumeImpl
+ {
+ enum
+ {
+ CTA_SIZE_X = 32,
+ CTA_SIZE_Y = 8,
+
+ ONE_VOXEL = 0
+ };
+
+ Intr intr;
+
+ PtrStep<float> vmap;
+ PtrStepSz<uchar3> colors;
+
+ Mat33 R_inv;
+ float3 t;
+
+ float3 cell_size;
+ float tranc_dist;
+
+ int max_weight;
+
+ mutable PtrStep<uchar4> color_volume;
+
+ __device__ __forceinline__ int3
+ getVoxel (float3 point) const
+ {
+ int vx = __float2int_rd (point.x / cell_size.x); // round to negative infinity
+ int vy = __float2int_rd (point.y / cell_size.y);
+ int vz = __float2int_rd (point.z / cell_size.z);
+
+ return make_int3 (vx, vy, vz);
+ }
+
+ __device__ __forceinline__ float3
+ getVoxelGCoo (int x, int y, int z) const
+ {
+ float3 coo = make_float3 (x, y, z);
+ coo += 0.5f; //shift to cell center;
+
+ coo.x *= cell_size.x;
+ coo.y *= cell_size.y;
+ coo.z *= cell_size.z;
+
+ return coo;
+ }
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x >= VOLUME_X || y >= VOLUME_Y)
+ return;
+
+ for (int z = 0; z < VOLUME_X; ++z)
+ {
+ float3 v_g = getVoxelGCoo (x, y, z);
+
+ float3 v = R_inv * (v_g - t);
+
+ if (v.z <= 0)
+ continue;
+
+ int2 coo; //project to current cam
+ coo.x = __float2int_rn (v.x * intr.fx / v.z + intr.cx);
+ coo.y = __float2int_rn (v.y * intr.fy / v.z + intr.cy);
+
+ if (coo.x >= 0 && coo.y >= 0 && coo.x < colors.cols && coo.y < colors.rows)
+ {
+ float3 p;
+ p.x = vmap.ptr (coo.y)[coo.x];
+
+ if (isnan (p.x))
+ continue;
+
+ p.y = vmap.ptr (coo.y + colors.rows )[coo.x];
+ p.z = vmap.ptr (coo.y + colors.rows * 2)[coo.x];
+
+ bool update = false;
+ if (ONE_VOXEL)
+ {
+ int3 vp = getVoxel (p);
+ update = vp.x == x && vp.y == y && vp.z == z;
+ }
+ else
+ {
+ float dist = norm (p - v_g);
+ update = dist < tranc_dist;
+ }
+
+ if (update)
+ {
+ uchar4 *ptr = color_volume.ptr (VOLUME_Y * z + y) + x;
+ uchar3 rgb = colors.ptr (coo.y)[coo.x];
+ uchar4 volume_rgbw = *ptr;
+
+ int weight_prev = volume_rgbw.w;
+
+ const float Wrk = 1.f;
+ float new_x = (volume_rgbw.x * weight_prev + Wrk * rgb.x) / (weight_prev + Wrk);
+ float new_y = (volume_rgbw.y * weight_prev + Wrk * rgb.y) / (weight_prev + Wrk);
+ float new_z = (volume_rgbw.z * weight_prev + Wrk * rgb.z) / (weight_prev + Wrk);
+
+ int weight_new = weight_prev + 1;
+
+ uchar4 volume_rgbw_new;
+ volume_rgbw_new.x = min (255, max (0, __float2int_rn (new_x)));
+ volume_rgbw_new.y = min (255, max (0, __float2int_rn (new_y)));
+ volume_rgbw_new.z = min (255, max (0, __float2int_rn (new_z)));
+ volume_rgbw_new.w = min (max_weight, weight_new);
+
+ *ptr = volume_rgbw_new;
+ }
+ } /* in camera image range */
+ } /* for(int z = 0; z < VOLUME_X; ++z) */
+ } /* void operator() */
+ };
+
+ __global__ void
+ updateColorVolumeKernel (const ColorVolumeImpl cvi) {
+ cvi ();
+ }
+
+ void
+ updateColorVolume (const Intr& intr, float tranc_dist, const Mat33& R_inv, const float3& t,
+ const MapArr& vmap, const PtrStepSz<uchar3>& colors, const float3& volume_size, PtrStep<uchar4> color_volume, int max_weight)
+ {
+ ColorVolumeImpl cvi;
+ cvi.vmap = vmap;
+ cvi.colors = colors;
+ cvi.color_volume = color_volume;
+
+ cvi.R_inv = R_inv;
+ cvi.t = t;
+ cvi.intr = intr;
+ cvi.tranc_dist = tranc_dist;
+ cvi.max_weight = min (max (0, max_weight), 255);
+
+ cvi.cell_size.x = volume_size.x / VOLUME_X;
+ cvi.cell_size.y = volume_size.y / VOLUME_Y;
+ cvi.cell_size.z = volume_size.z / VOLUME_Z;
+
+ dim3 block (ColorVolumeImpl::CTA_SIZE_X, ColorVolumeImpl::CTA_SIZE_Y);
+ dim3 grid (divUp (VOLUME_X, block.x), divUp (VOLUME_Y, block.y));
+
+ updateColorVolumeKernel<<<grid, block>>>(cvi);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+ }
+ }
+}
+
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ __global__ void
+ extractColorsKernel (const float3 cell_size, const PtrStep<uchar4> color_volume, const PtrSz<PointType> points, uchar4 *colors)
+ {
+ int idx = threadIdx.x + blockIdx.x * blockDim.x;
+
+ if (idx < points.size)
+ {
+ int3 v;
+ float3 p = *(const float3*)(points.data + idx);
+ v.x = __float2int_rd (p.x / cell_size.x); // round to negative infinity
+ v.y = __float2int_rd (p.y / cell_size.y);
+ v.z = __float2int_rd (p.z / cell_size.z);
+
+ uchar4 rgbw = color_volume.ptr (VOLUME_Y * v.z + v.y)[v.x];
+ colors[idx] = make_uchar4 (rgbw.z, rgbw.y, rgbw.x, 0); //bgra
+ }
+ }
+
+ void
+ exctractColors (const PtrStep<uchar4>& color_volume, const float3& volume_size, const PtrSz<PointType>& points, uchar4* colors)
+ {
+ const int block = 256;
+ float3 cell_size = make_float3 (volume_size.x / VOLUME_X, volume_size.y / VOLUME_Y, volume_size.z / VOLUME_Z);
+ extractColorsKernel<<<divUp (points.size, block), block>>>(cell_size, color_volume, points, colors);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+ }
+ }
+}
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+//#include <boost/graph/buffer_concepts.hpp>
+//#include <pcl/gpu/utils/device/block.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ __device__ unsigned int count = 0;
+
+ struct CorespSearch
+ {
+ enum { CTA_SIZE_X = 32, CTA_SIZE_Y = 8, CTA_SIZE = CTA_SIZE_X * CTA_SIZE_Y };
+
+ struct plus
+ {
+ __forceinline__ __device__ int
+ operator () (const int &lhs, const volatile int& rhs) const {
+ return lhs + rhs;
+ }
+ };
+
+ PtrStep<float> vmap_g_curr;
+ PtrStep<float> nmap_g_curr;
+
+ Mat33 Rprev_inv;
+ float3 tprev;
+
+ Intr intr;
+
+ PtrStep<float> vmap_g_prev;
+ PtrStep<float> nmap_g_prev;
+
+ float distThres;
+ float angleThres;
+
+ mutable PtrStepSz<short2> coresp;
+
+ mutable int* gbuf;
+
+ __device__ __forceinline__ int
+ search () const
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x >= coresp.cols || y >= coresp.rows)
+ return 0;
+
+ coresp.ptr (y)[x] = make_short2 (-1, -1);
+
+ float3 ncurr_g;
+ ncurr_g.x = nmap_g_curr.ptr (y)[x];
+
+ if (isnan (ncurr_g.x))
+ return 0;
+
+ float3 vcurr_g;
+ vcurr_g.x = vmap_g_curr.ptr (y )[x];
+ vcurr_g.y = vmap_g_curr.ptr (y + coresp.rows)[x];
+ vcurr_g.z = vmap_g_curr.ptr (y + 2 * coresp.rows)[x];
+
+ float3 vcurr_cp = Rprev_inv * (vcurr_g - tprev); // prev camera coo space
+
+ int2 ukr; //projection
+ ukr.x = __float2int_rn (vcurr_cp.x * intr.fx / vcurr_cp.z + intr.cx); //4
+ ukr.y = __float2int_rn (vcurr_cp.y * intr.fy / vcurr_cp.z + intr.cy); //4
+
+ if (ukr.x < 0 || ukr.y < 0 || ukr.x >= coresp.cols || ukr.y >= coresp.rows)
+ return 0;
+
+ float3 nprev_g;
+ nprev_g.x = nmap_g_prev.ptr (ukr.y)[ukr.x];
+
+ if (isnan (nprev_g.x))
+ return 0;
+
+ float3 vprev_g;
+ vprev_g.x = vmap_g_prev.ptr (ukr.y )[ukr.x];
+ vprev_g.y = vmap_g_prev.ptr (ukr.y + coresp.rows)[ukr.x];
+ vprev_g.z = vmap_g_prev.ptr (ukr.y + 2 * coresp.rows)[ukr.x];
+
+ float dist = norm (vcurr_g - vprev_g);
+ if (dist > distThres)
+ return 0;
+
+ ncurr_g.y = nmap_g_curr.ptr (y + coresp.rows)[x];
+ ncurr_g.z = nmap_g_curr.ptr (y + 2 * coresp.rows)[x];
+
+ nprev_g.y = nmap_g_prev.ptr (ukr.y + coresp.rows)[ukr.x];
+ nprev_g.z = nmap_g_prev.ptr (ukr.y + 2 * coresp.rows)[ukr.x];
+
+ float sine = norm (cross (ncurr_g, nprev_g));
+
+ /*if (sine >= 1 || asinf(sine) >= angleThres)
+ return 0;*/
+
+ if (/*sine >= 1 || */ sine >= angleThres)
+ return 0;
+
+ coresp.ptr (y)[x] = make_short2 (ukr.x, ukr.y);
+ return 1;
+ }
+
+ __device__ __forceinline__ void
+ reduce (int i) const
+ {
+ __shared__ volatile int smem[CTA_SIZE];
+
+ int tid = Block::flattenedThreadId ();
+
+ smem[tid] = i;
+ __syncthreads ();
+
+ Block::reduce<CTA_SIZE>(smem, plus ());
+
+ __shared__ bool isLastBlockDone;
+
+ if (tid == 0)
+ {
+ gbuf[blockIdx.x + gridDim.x * blockIdx.y] = smem[0];
+ __threadfence ();
+
+ unsigned int value = atomicInc (&count, gridDim.x * gridDim.y);
+
+ isLastBlockDone = (value == (gridDim.x * gridDim.y - 1));
+ }
+ __syncthreads ();
+
+ if (isLastBlockDone)
+ {
+ int sum = 0;
+ int stride = Block::stride ();
+ for (int pos = tid; pos < gridDim.x * gridDim.y; pos += stride)
+ sum += gbuf[pos];
+
+ smem[tid] = sum;
+ __syncthreads ();
+ Block::reduce<CTA_SIZE>(smem, plus ());
+
+ if (tid == 0)
+ {
+ gbuf[0] = smem[0];
+ count = 0;
+ }
+ }
+ }
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int mask = search ();
+ //reduce(mask); if uncomment -> need to allocate and set gbuf
+ }
+ };
+
+ __global__ void
+ corespKernel (const CorespSearch cs) {
+ cs ();
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ findCoresp (const MapArr& vmap_g_curr, const MapArr& nmap_g_curr,
+ const Mat33& Rprev_inv, const float3& tprev, const Intr& intr,
+ const MapArr& vmap_g_prev, const MapArr& nmap_g_prev,
+ float distThres, float angleThres, PtrStepSz<short2> coresp)
+ {
+ CorespSearch cs;
+
+ cs.vmap_g_curr = vmap_g_curr;
+ cs.nmap_g_curr = nmap_g_curr;
+
+ cs.Rprev_inv = Rprev_inv;
+ cs.tprev = tprev;
+
+ cs.intr = intr;
+
+ cs.vmap_g_prev = vmap_g_prev;
+ cs.nmap_g_prev = nmap_g_prev;
+
+ cs.distThres = distThres;
+ cs.angleThres = angleThres;
+
+ cs.coresp = coresp;
+
+ dim3 block (CorespSearch::CTA_SIZE_X, CorespSearch::CTA_SIZE_Y);
+ dim3 grid (divUp (coresp.cols, block.x), divUp (coresp.rows, block.y));
+
+ corespKernel<<<grid, block>>>(cs);
+
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+ }
+ }
+}
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_GPU_KINFU_DEVICE_HPP_
+#define PCL_GPU_KINFU_DEVICE_HPP_
+
+//#include <pcl/gpu/utils/device/limits.hpp>
+//#include <pcl/gpu/utils/device/vector_math.hpp>
+#include "utils.hpp" //temporary reimplementing to release kinfu without pcl_gpu_utils
+
+#include "internal.h"
+
+#include "pointer_shift.cu" // contains primitive needed by all cuda functions dealing with rolling tsdf buffer
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ #define INV_DIV 3.051850947599719e-5f
+
+ __device__ __forceinline__ void
+ pack_tsdf (float tsdf, int weight, short2& value)
+ {
+ int fixedp = max (-DIVISOR, min (DIVISOR, __float2int_rz (tsdf * DIVISOR)));
+ //int fixedp = __float2int_rz(tsdf * DIVISOR);
+ value = make_short2 (fixedp, weight);
+ }
+
+ __device__ __forceinline__ void
+ unpack_tsdf (short2 value, float& tsdf, int& weight)
+ {
+ weight = value.y;
+ tsdf = __int2float_rn (value.x) / DIVISOR; //*/ * INV_DIV;
+ }
+
+ __device__ __forceinline__ float
+ unpack_tsdf (short2 value)
+ {
+ return static_cast<float>(value.x) / DIVISOR; //*/ * INV_DIV;
+ }
+
+
+ __device__ __forceinline__ float3
+ operator* (const Mat33& m, const float3& vec)
+ {
+ return make_float3 (dot (m.data[0], vec), dot (m.data[1], vec), dot (m.data[2], vec));
+ }
+
+
+ ////////////////////////////////////////////////////////////////////////////////////////
+ ///// Prefix Scan utility
+
+ enum ScanKind { exclusive, inclusive };
+
+ template<ScanKind Kind, class T>
+ __device__ __forceinline__ T
+ scan_warp ( volatile T *ptr, const unsigned int idx = threadIdx.x )
+ {
+ const unsigned int lane = idx & 31; // index of thread in warp (0..31)
+
+ if (lane >= 1) ptr[idx] = ptr[idx - 1] + ptr[idx];
+ if (lane >= 2) ptr[idx] = ptr[idx - 2] + ptr[idx];
+ if (lane >= 4) ptr[idx] = ptr[idx - 4] + ptr[idx];
+ if (lane >= 8) ptr[idx] = ptr[idx - 8] + ptr[idx];
+ if (lane >= 16) ptr[idx] = ptr[idx - 16] + ptr[idx];
+
+ if (Kind == inclusive)
+ return ptr[idx];
+ else
+ return (lane > 0) ? ptr[idx - 1] : 0;
+ }
+ }
+ }
+}
+
+#endif /* PCL_GPU_KINFU_DEVICE_HPP_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+//#include <pcl/gpu/utils/device/block.hpp>
+//#include <pcl/gpu/utils/device/funcattrib.hpp>
+#include "device.hpp"
+#include "estimate_combined.h"
+//#include <boost/graph/buffer_concepts.hpp>
+
+using namespace pcl::device;
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ typedef double float_type;
+
+ template<int CTA_SIZE_, typename T>
+ static __device__ __forceinline__ void reduce(volatile T* buffer)
+ {
+ int tid = Block::flattenedThreadId();
+ T val = buffer[tid];
+
+ if (CTA_SIZE_ >= 1024) { if (tid < 512) buffer[tid] = val = val + buffer[tid + 512]; __syncthreads(); }
+ if (CTA_SIZE_ >= 512) { if (tid < 256) buffer[tid] = val = val + buffer[tid + 256]; __syncthreads(); }
+ if (CTA_SIZE_ >= 256) { if (tid < 128) buffer[tid] = val = val + buffer[tid + 128]; __syncthreads(); }
+ if (CTA_SIZE_ >= 128) { if (tid < 64) buffer[tid] = val = val + buffer[tid + 64]; __syncthreads(); }
+
+ if (tid < 32)
+ {
+ if (CTA_SIZE_ >= 64) { buffer[tid] = val = val + buffer[tid + 32]; }
+ if (CTA_SIZE_ >= 32) { buffer[tid] = val = val + buffer[tid + 16]; }
+ if (CTA_SIZE_ >= 16) { buffer[tid] = val = val + buffer[tid + 8]; }
+ if (CTA_SIZE_ >= 8) { buffer[tid] = val = val + buffer[tid + 4]; }
+ if (CTA_SIZE_ >= 4) { buffer[tid] = val = val + buffer[tid + 2]; }
+ if (CTA_SIZE_ >= 2) { buffer[tid] = val = val + buffer[tid + 1]; }
+ }
+ }
+
+ struct Combined
+ {
+ enum
+ {
+ CTA_SIZE_X = ESTIMATE_COMBINED_CUDA_GRID_X,
+ CTA_SIZE_Y = ESTIMATE_COMBINED_CUDA_GRID_Y,
+ CTA_SIZE = CTA_SIZE_X * CTA_SIZE_Y
+ };
+
+ Mat33 Rcurr;
+ float3 tcurr;
+
+ PtrStep<float> vmap_curr;
+ PtrStep<float> nmap_curr;
+
+ Mat33 Rprev_inv;
+ float3 tprev;
+
+ Intr intr;
+
+ PtrStep<float> vmap_g_prev;
+ PtrStep<float> nmap_g_prev;
+
+ float distThres;
+ float angleThres;
+
+ int cols;
+ int rows;
+
+ mutable PtrStep<float_type> gbuf;
+
+ __device__ __forceinline__ bool
+ search (int x, int y, float3& n, float3& d, float3& s) const
+ {
+ float3 ncurr;
+ ncurr.x = nmap_curr.ptr (y)[x];
+
+ if (isnan (ncurr.x))
+ return (false);
+
+ float3 vcurr;
+ vcurr.x = vmap_curr.ptr (y )[x];
+ vcurr.y = vmap_curr.ptr (y + rows)[x];
+ vcurr.z = vmap_curr.ptr (y + 2 * rows)[x];
+
+ float3 vcurr_g = Rcurr * vcurr + tcurr;
+
+ float3 vcurr_cp = Rprev_inv * (vcurr_g - tprev); // prev camera coo space
+
+ int2 ukr; //projection
+ ukr.x = __float2int_rn (vcurr_cp.x * intr.fx / vcurr_cp.z + intr.cx); //4
+ ukr.y = __float2int_rn (vcurr_cp.y * intr.fy / vcurr_cp.z + intr.cy); //4
+
+ if (ukr.x < 0 || ukr.y < 0 || ukr.x >= cols || ukr.y >= rows || vcurr_cp.z < 0)
+ return (false);
+
+ float3 nprev_g;
+ nprev_g.x = nmap_g_prev.ptr (ukr.y)[ukr.x];
+
+ if (isnan (nprev_g.x))
+ return (false);
+
+ float3 vprev_g;
+ vprev_g.x = vmap_g_prev.ptr (ukr.y )[ukr.x];
+ vprev_g.y = vmap_g_prev.ptr (ukr.y + rows)[ukr.x];
+ vprev_g.z = vmap_g_prev.ptr (ukr.y + 2 * rows)[ukr.x];
+
+ float dist = norm (vprev_g - vcurr_g);
+ if (dist > distThres)
+ return (false);
+
+ ncurr.y = nmap_curr.ptr (y + rows)[x];
+ ncurr.z = nmap_curr.ptr (y + 2 * rows)[x];
+
+ float3 ncurr_g = Rcurr * ncurr;
+
+ nprev_g.y = nmap_g_prev.ptr (ukr.y + rows)[ukr.x];
+ nprev_g.z = nmap_g_prev.ptr (ukr.y + 2 * rows)[ukr.x];
+
+ float sine = norm (cross (ncurr_g, nprev_g));
+
+ if (sine >= angleThres)
+ return (false);
+ n = nprev_g;
+ d = vprev_g;
+ s = vcurr_g;
+ return (true);
+ }
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int x = threadIdx.x + blockIdx.x * CTA_SIZE_X;
+ int y = threadIdx.y + blockIdx.y * CTA_SIZE_Y;
+
+ float3 n, d, s;
+ bool found_coresp = false;
+
+ if (x < cols && y < rows)
+ found_coresp = search (x, y, n, d, s);
+
+ float row[7];
+
+ if (found_coresp)
+ {
+ *(float3*)&row[0] = cross (s, n);
+ *(float3*)&row[3] = n;
+ row[6] = dot (n, d - s);
+ }
+ else
+ row[0] = row[1] = row[2] = row[3] = row[4] = row[5] = row[6] = 0.f;
+
+ __shared__ float_type smem[CTA_SIZE];
+ int tid = Block::flattenedThreadId ();
+
+ int shift = 0;
+ for (int i = 0; i < 6; ++i) //rows
+ {
+ #pragma unroll
+ for (int j = i; j < 7; ++j) // cols + b
+ {
+ __syncthreads ();
+ smem[tid] = row[i] * row[j];
+ __syncthreads ();
+
+ reduce<CTA_SIZE>(smem);
+
+ if (tid == 0)
+ gbuf.ptr (shift++)[blockIdx.x + gridDim.x * blockIdx.y] = smem[0];
+ }
+ }
+ }
+ };
+
+ __global__ void
+ combinedKernel (const Combined cs)
+ {
+ cs ();
+ }
+
+ struct TranformReduction
+ {
+ enum
+ {
+ CTA_SIZE = 512,
+ STRIDE = CTA_SIZE,
+
+ B = 6, COLS = 6, ROWS = 6, DIAG = 6,
+ UPPER_DIAG_MAT = (COLS * ROWS - DIAG) / 2 + DIAG,
+ TOTAL = UPPER_DIAG_MAT + B,
+
+ GRID_X = TOTAL
+ };
+
+ PtrStep<float_type> gbuf;
+ int length;
+ mutable float_type* output;
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ const float_type *beg = gbuf.ptr (blockIdx.x);
+ const float_type *end = beg + length;
+
+ int tid = threadIdx.x;
+
+ float_type sum = 0.f;
+ for (const float_type *t = beg + tid; t < end; t += STRIDE)
+ sum += *t;
+
+ __shared__ float_type smem[CTA_SIZE];
+
+ smem[tid] = sum;
+ __syncthreads ();
+
+ reduce<CTA_SIZE>(smem);
+
+ if (tid == 0)
+ output[blockIdx.x] = smem[0];
+ }
+ };
+
+ __global__ void
+ TransformEstimatorKernel2 (const TranformReduction tr)
+ {
+ tr ();
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ estimateCombined (const Mat33& Rcurr, const float3& tcurr,
+ const MapArr& vmap_curr, const MapArr& nmap_curr,
+ const Mat33& Rprev_inv, const float3& tprev, const Intr& intr,
+ const MapArr& vmap_g_prev, const MapArr& nmap_g_prev,
+ float distThres, float angleThres,
+ DeviceArray2D<float_type>& gbuf, DeviceArray<float_type>& mbuf,
+ float_type* matrixA_host, float_type* vectorB_host)
+ {
+ int cols = vmap_curr.cols ();
+ int rows = vmap_curr.rows () / 3;
+
+ Combined cs;
+
+ cs.Rcurr = Rcurr;
+ cs.tcurr = tcurr;
+
+ cs.vmap_curr = vmap_curr;
+ cs.nmap_curr = nmap_curr;
+
+ cs.Rprev_inv = Rprev_inv;
+ cs.tprev = tprev;
+
+ cs.intr = intr;
+
+ cs.vmap_g_prev = vmap_g_prev;
+ cs.nmap_g_prev = nmap_g_prev;
+
+ cs.distThres = distThres;
+ cs.angleThres = angleThres;
+
+ cs.cols = cols;
+ cs.rows = rows;
+
+ //////////////////////////////
+
+ dim3 block (Combined::CTA_SIZE_X, Combined::CTA_SIZE_Y);
+ dim3 grid (1, 1, 1);
+ grid.x = divUp (cols, block.x);
+ grid.y = divUp (rows, block.y);
+
+ mbuf.create (TranformReduction::TOTAL);
+ if (gbuf.rows () != TranformReduction::TOTAL || gbuf.cols () < (int)(grid.x * grid.y))
+ gbuf.create (TranformReduction::TOTAL, grid.x * grid.y);
+
+ cs.gbuf = gbuf;
+
+ combinedKernel<<<grid, block>>>(cs);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall(cudaDeviceSynchronize());
+
+ //printFuncAttrib(combinedKernel);
+
+ TranformReduction tr;
+ tr.gbuf = gbuf;
+ tr.length = grid.x * grid.y;
+ tr.output = mbuf;
+
+ TransformEstimatorKernel2<<<TranformReduction::TOTAL, TranformReduction::CTA_SIZE>>>(tr);
+ cudaSafeCall (cudaGetLastError ());
+ cudaSafeCall (cudaDeviceSynchronize ());
+
+ float_type host_data[TranformReduction::TOTAL];
+ mbuf.download (host_data);
+
+ int shift = 0;
+ for (int i = 0; i < 6; ++i) //rows
+ for (int j = i; j < 7; ++j) // cols + b
+ {
+ float_type value = host_data[shift++];
+ if (j == 6) // vector b
+ vectorB_host[i] = value;
+ else
+ matrixA_host[j * 6 + i] = matrixA_host[i * 6 + j] = value;
+ }
+ }
+ }
+ }
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+//#include <boost/graph/buffer_concepts.hpp>
+//#include <pcl/gpu/utils/device/block.hpp>
+//#include <pcl/gpu/utils/device/funcattrib.hpp>
+//#include <pcl/gpu/utils/timers_cuda.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ template<typename T>
+ struct TransformEstimator
+ {
+ enum
+ {
+ CTA_SIZE_X = 32,
+ CTA_SIZE_Y = 8,
+ CTA_SIZE = CTA_SIZE_X * CTA_SIZE_Y
+ };
+
+ struct plus
+ {
+ __forceinline__ __device__ T
+ operator () (const T &lhs, const volatile T &rhs) const {
+ return lhs + rhs;
+ }
+ };
+
+ PtrStep<float> v_dst;
+ PtrStep<float> n_dst;
+ PtrStep<float> v_src;
+ PtrStepSz<short2> coresp;
+
+ mutable PtrStep<T> gbuf;
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ float row[7];
+ row[0] = row[1] = row[2] = row[3] = row[4] = row[5] = row[6] = 0.f;
+
+ if (x < coresp.cols || y < coresp.rows)
+ {
+ short2 ukr = coresp.ptr (y)[x];
+
+ if (ukr.x != -1)
+ {
+ float3 n;
+ n.x = n_dst.ptr (ukr.y )[ukr.x];
+ n.y = n_dst.ptr (ukr.y + coresp.rows)[ukr.x];
+ n.z = n_dst.ptr (ukr.y + 2 * coresp.rows)[ukr.x];
+
+ float3 d;
+ d.x = v_dst.ptr (ukr.y )[ukr.x];
+ d.y = v_dst.ptr (ukr.y + coresp.rows)[ukr.x];
+ d.z = v_dst.ptr (ukr.y + 2 * coresp.rows)[ukr.x];
+
+ float3 s;
+ s.x = v_src.ptr (y )[x];
+ s.y = v_src.ptr (y + coresp.rows)[x];
+ s.z = v_src.ptr (y + 2 * coresp.rows)[x];
+
+ float b = dot (n, d - s);
+
+ *(float3*)&row[0] = cross (s, n);
+ *(float3*)&row[3] = n;
+ row[6] = b;
+ }
+ }
+
+ __shared__ T smem[CTA_SIZE];
+ int tid = Block::flattenedThreadId ();
+
+ int shift = 0;
+ for (int i = 0; i < 6; ++i) //rows
+ {
+ #pragma unroll
+ for (int j = i; j < 7; ++j) // cols + b
+ {
+ __syncthreads ();
+ smem[tid] = row[i] * row[j];
+ __syncthreads ();
+
+ Block::reduce<CTA_SIZE>(smem, plus ());
+
+ if (tid == 0)
+ gbuf.ptr (shift++)[blockIdx.x + gridDim.x * blockIdx.y] = smem[0];
+ }
+ }
+ }
+ };
+
+ template<typename T>
+ struct TranformReduction
+ {
+ enum
+ {
+ CTA_SIZE = 512,
+ STRIDE = CTA_SIZE,
+
+ B = 6, COLS = 6, ROWS = 6, DIAG = 6,
+ UPPER_DIAG_MAT = (COLS * ROWS - DIAG) / 2 + DIAG,
+ TOTAL = UPPER_DIAG_MAT + B,
+
+ GRID_X = TOTAL
+ };
+
+ PtrStep<T> gbuf;
+ int length;
+ mutable T* output;
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ const T *beg = gbuf.ptr (blockIdx.x);
+ const T *end = beg + length;
+
+ int tid = threadIdx.x;
+
+ T sum = 0.f;
+ for (const T *t = beg + tid; t < end; t += STRIDE)
+ sum += *t;
+
+ __shared__ T smem[CTA_SIZE];
+
+ smem[tid] = sum;
+ __syncthreads ();
+
+ Block::reduce<CTA_SIZE>(smem, TransformEstimator<T>::plus ());
+
+ if (tid == 0)
+ output[blockIdx.x] = smem[0];
+ }
+ };
+
+ __global__ void
+ TransformEstimatorKernel1 (const TransformEstimator<float> te) {
+ te ();
+ }
+ __global__ void
+ TransformEstimatorKernel2 (const TranformReduction<float> tr) {
+ tr ();
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ estimateTransform (const MapArr& v_dst, const MapArr& n_dst,
+ const MapArr& v_src, const PtrStepSz<short2>& coresp,
+ DeviceArray2D<float>& gbuf, DeviceArray<float>& mbuf,
+ float* matrixA_host, float* vectorB_host)
+ {
+ typedef TransformEstimator<float> TEst;
+ typedef TranformReduction<float> TRed;
+
+ dim3 block (TEst::CTA_SIZE_X, TEst::CTA_SIZE_Y);
+ dim3 grid (1, 1, 1);
+ grid.x = divUp (coresp.cols, block.x);
+ grid.y = divUp (coresp.rows, block.y);
+
+ mbuf.create (TRed::TOTAL);
+ if (gbuf.rows () != TRed::TOTAL || gbuf.cols () < (int)(grid.x * grid.y))
+ gbuf.create (TRed::TOTAL, grid.x * grid.y);
+
+ TEst te;
+ te.n_dst = n_dst;
+ te.v_dst = v_dst;
+ te.v_src = v_src;
+ te.coresp = coresp;
+ te.gbuf = gbuf;
+
+ TransformEstimatorKernel1<<<grid, block>>>(te);
+ cudaSafeCall ( cudaGetLastError () );
+ //cudaSafeCall(cudaDeviceSynchronize());
+
+ TRed tr;
+ tr.gbuf = gbuf;
+ tr.length = grid.x * grid.y;
+ tr.output = mbuf;
+
+ TransformEstimatorKernel2<<<TRed::TOTAL, TRed::CTA_SIZE>>>(tr);
+
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+
+ float host_data[TRed::TOTAL];
+ mbuf.download (host_data);
+
+ int shift = 0;
+ for (int i = 0; i < 6; ++i) //rows
+ for (int j = i; j < 7; ++j) // cols + b
+ {
+ float value = host_data[shift++];
+ if (j == 6) // vector b
+ vectorB_host[i] = value;
+ else
+ matrixA_host[j * 6 + i] = matrixA_host[i * 6 + j] = value;
+ }
+ }
+ }
+ }
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+#include <iostream>
+//#include <boost/graph/buffer_concepts.hpp>
+
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+
+ ////////////////////////////////////////////////////////////////////////////////////////
+ ///// Full Volume Scan6
+ enum
+ {
+ CTA_SIZE_X = 32,
+ CTA_SIZE_Y = 6,
+ CTA_SIZE = CTA_SIZE_X * CTA_SIZE_Y,
+
+ MAX_LOCAL_POINTS = 3
+ };
+
+ __device__ int global_count = 0;
+ __device__ int output_xyz_count = 0; // *************************************************
+ __device__ unsigned int blocks_done = 0;
+
+ __shared__ float storage_X[CTA_SIZE * MAX_LOCAL_POINTS];
+ __shared__ float storage_Y[CTA_SIZE * MAX_LOCAL_POINTS];
+ __shared__ float storage_Z[CTA_SIZE * MAX_LOCAL_POINTS];
+ __shared__ float storage_I[CTA_SIZE * MAX_LOCAL_POINTS];
+
+ struct FullScan6
+ {
+ PtrStep<short2> volume;
+ float3 cell_size;
+
+ mutable PtrSz<PointType> output;
+ mutable PtrSz<PointType> output_xyz;
+ mutable PtrSz<float> output_intensity;
+
+ __device__ __forceinline__ float
+ fetch (pcl::gpu::kinfuLS::tsdf_buffer buffer, int x, int y, int z, int& weight) const
+ {
+ float tsdf;
+ const short2* tmp_pos = &(volume.ptr (buffer.voxels_size.y * z + y)[x]);
+ short2* pos = const_cast<short2*> (tmp_pos);
+
+ shift_tsdf_pointer (&pos, buffer);
+
+ unpack_tsdf (*pos, tsdf, weight);
+
+ return tsdf;
+ }
+
+ __device__ __forceinline__ float
+ fetch (int x, int y, int z, int& weight) const
+ {
+ float tsdf;
+ unpack_tsdf (volume.ptr (VOLUME_Y * z + y)[x], tsdf, weight);
+ return tsdf;
+ }
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int x = threadIdx.x + blockIdx.x * CTA_SIZE_X;
+ int y = threadIdx.y + blockIdx.y * CTA_SIZE_Y;
+
+ #if __CUDA_ARCH__ < 200
+ __shared__ int cta_buffer[CTA_SIZE];
+ #endif
+
+ #if __CUDA_ARCH__ >= 120
+ if (__all (x >= VOLUME_X) || __all (y >= VOLUME_Y))
+ return;
+ #else
+ if (Emulation::All(x >= VOLUME_X, cta_buffer) ||
+ Emulation::All(y >= VOLUME_Y, cta_buffer))
+ return;
+ #endif
+
+ float3 V;
+ V.x = (x + 0.5f) * cell_size.x;
+ V.y = (y + 0.5f) * cell_size.y;
+
+ int ftid = Block::flattenedThreadId ();
+
+ for (int z = 0; z < VOLUME_Z - 1; ++z)
+ {
+ float3 points[MAX_LOCAL_POINTS];
+ int local_count = 0;
+
+ if (x < VOLUME_X && y < VOLUME_Y)
+ {
+ int W;
+ float F = fetch (x, y, z, W);
+
+ if (W != 0 && F != 1.f)
+ {
+ V.z = (z + 0.5f) * cell_size.z;
+
+ // process dx
+ if (x + 1 < VOLUME_X)
+ {
+ int Wn;
+ float Fn = fetch (x + 1, y, z, Wn);
+
+ if (Wn != 0 && Fn != 1.f)
+ if ( (F > 0 && Fn < 0) || (F < 0 && Fn > 0) )
+ {
+ float3 p;
+ p.y = V.y;
+ p.z = V.z;
+
+ float Vnx = V.x + cell_size.x;
+
+ float d_inv = 1.f / (fabs (F) + fabs (Fn));
+ p.x = (V.x * fabs (Fn) + Vnx * fabs (F)) * d_inv;
+
+ points[local_count++] = p;
+ }
+ } /* if (x + 1 < VOLUME_X) */
+
+ // process dy
+ if (y + 1 < VOLUME_Y)
+ {
+ int Wn;
+ float Fn = fetch (x, y + 1, z, Wn);
+
+ if (Wn != 0 && Fn != 1.f)
+ if ( (F > 0 && Fn < 0) || (F < 0 && Fn > 0) )
+ {
+ float3 p;
+ p.x = V.x;
+ p.z = V.z;
+
+ float Vny = V.y + cell_size.y;
+
+ float d_inv = 1.f / (fabs (F) + fabs (Fn));
+ p.y = (V.y * fabs (Fn) + Vny * fabs (F)) * d_inv;
+
+ points[local_count++] = p;
+ }
+ } /* if (y + 1 < VOLUME_Y) */
+
+ // process dz
+ // if (z + 1 < VOLUME_Z) // guaranteed by loop
+ {
+ int Wn;
+ float Fn = fetch (x, y, z + 1, Wn);
+
+ if (Wn != 0 && Fn != 1.f)
+ if ((F > 0 && Fn < 0) || (F < 0 && Fn > 0))
+ {
+ float3 p;
+ p.x = V.x;
+ p.y = V.y;
+
+ float Vnz = V.z + cell_size.z;
+
+ float d_inv = 1.f / (fabs (F) + fabs (Fn));
+ p.z = (V.z * fabs (Fn) + Vnz * fabs (F)) * d_inv;
+
+ points[local_count++] = p;
+ }
+ }/* if (z + 1 < VOLUME_Z) */
+ }/* if (W != 0 && F != 1.f) */
+ }/* if (x < VOLUME_X && y < VOLUME_Y) */
+
+
+ #if __CUDA_ARCH__ >= 200
+ //not we fulfilled points array at current iteration
+ int total_warp = __popc (__ballot (local_count > 0)) + __popc (__ballot (local_count > 1)) + __popc (__ballot (local_count > 2));
+ #else
+ int tid = Block::flattenedThreadId ();
+ cta_buffer[tid] = local_count;
+ int total_warp = Emulation::warp_reduce (cta_buffer, tid);
+ #endif
+
+ if (total_warp > 0)
+ {
+ int lane = Warp::laneId ();
+ int storage_index = (ftid >> Warp::LOG_WARP_SIZE) * Warp::WARP_SIZE * MAX_LOCAL_POINTS;
+
+ volatile int* cta_buffer = (int*)(storage_X + storage_index);
+
+ cta_buffer[lane] = local_count;
+ int offset = scan_warp<exclusive>(cta_buffer, lane);
+
+ if (lane == 0)
+ {
+ int old_global_count = atomicAdd (&global_count, total_warp);
+ cta_buffer[0] = old_global_count;
+ }
+ int old_global_count = cta_buffer[0];
+
+ for (int l = 0; l < local_count; ++l)
+ {
+ storage_X[storage_index + offset + l] = points[l].x;
+ storage_Y[storage_index + offset + l] = points[l].y;
+ storage_Z[storage_index + offset + l] = points[l].z;
+ }
+
+ int offset_storage = old_global_count + lane;
+ for (int idx = lane; idx < total_warp; idx += Warp::STRIDE, offset_storage += Warp::STRIDE)
+ {
+ if (offset_storage >= output_xyz.size) break;
+ float x = storage_X[storage_index + idx];
+ float y = storage_Y[storage_index + idx];
+ float z = storage_Z[storage_index + idx];
+ store_point_type (x, y, z, output_xyz.data, offset_storage);
+ }
+
+ bool full = (old_global_count + total_warp) >= output_xyz.size;
+
+ if (full)
+ break;
+ }
+
+ }/* for(int z = 0; z < VOLUME_Z - 1; ++z) */
+
+ ///////////////////////////
+ // Prepare for future scans
+ if (ftid == 0)
+ {
+ unsigned int total_blocks = gridDim.x * gridDim.y * gridDim.z;
+ unsigned int value = atomicInc (&blocks_done, total_blocks);
+
+ // Last block
+ if (value == total_blocks - 1)
+ {
+ output_xyz_count = min ((int)output_xyz.size, global_count);
+ blocks_done = 0;
+ global_count = 0;
+ }
+ }
+ } /* operator() */
+
+ // OPERATOR USED BY EXTRACT_SLICE_AS_CLOUD.
+ // This operator extracts the cloud as TSDF values and X,Y,Z indices.
+ // The previous operator generates a regular point cloud in meters.
+ // This one generates a TSDF Point Cloud in grid indices.
+ __device__ __forceinline__ void
+ operator () (pcl::gpu::kinfuLS::tsdf_buffer buffer, int3 minBounds, int3 maxBounds) const
+ {
+ int x = threadIdx.x + blockIdx.x * CTA_SIZE_X;
+ int y = threadIdx.y + blockIdx.y * CTA_SIZE_Y;
+
+ int ftid = Block::flattenedThreadId ();
+
+ int minimum_Z = 0;
+ int maximum_Z = VOLUME_Z - 1;
+
+ for (int z = minimum_Z; z < maximum_Z; ++z)
+ {
+ // The black zone is the name given to the subvolume within the TSDF Volume grid that is shifted out.
+ // In other words, the set of points in the TSDF grid that we want to extract in order to add it to the world model being built in CPU.
+ bool in_black_zone = ( (x >= minBounds.x && x <= maxBounds.x) || (y >= minBounds.y && y <= maxBounds.y) || ( z >= minBounds.z && z <= maxBounds.z) ) ;
+
+ float4 points[MAX_LOCAL_POINTS];
+ int local_count = 0;
+
+ if (x < buffer.voxels_size.x && y < buffer.voxels_size.y && in_black_zone)
+ {
+ int W;
+ float F = fetch (buffer, x, y, z, W);
+
+ if (W != 0.0f && F != 1.f && F < 0.98 && F != 0.0f && F > -1.0f)
+ {
+ float4 p;
+ p.x = x;
+ p.y = y;
+ p.z = z;
+ p.w = F;
+ points[local_count++] = p;
+ }
+ }/* if (x < VOLUME_X && y < VOLUME_Y) */
+
+ // local_count counts the number of zero crossing for the current thread. Now we need to merge this knowledge with the other threads
+ // not we fulfilled points array at current iteration
+ int total_warp = __popc (__ballot (local_count > 0)) + __popc (__ballot (local_count > 1)) + __popc (__ballot (local_count > 2));
+
+
+ if (total_warp > 0) ///more than 0 zero-crossings
+ {
+ int lane = Warp::laneId (); ///index of thread within warp [0-31]
+ int storage_index = (ftid >> Warp::LOG_WARP_SIZE) * Warp::WARP_SIZE * MAX_LOCAL_POINTS;
+
+ // Pointer to the beginning of the current warp buffer
+ volatile int* cta_buffer = (int*)(storage_X + storage_index);
+
+ // Compute offset of current warp
+ // Call in place scanning (see http://http.developer.nvidia.com/GPUGems3/gpugems3_ch39.html)
+ cta_buffer[lane] = local_count;
+ int offset = scan_warp<exclusive>(cta_buffer, lane); //How many crossings did we have before index "lane" ?
+
+ // We want to do only 1 operation per warp (not thread) -> because it is faster
+ if (lane == 0)
+ {
+ int old_global_count = atomicAdd (&global_count, total_warp); ///We use atomicAdd, so that threads do not collide
+ cta_buffer[0] = old_global_count;
+ }
+ int old_global_count = cta_buffer[0];
+
+ // Perform compaction (dump all current crossings)
+ for (int l = 0; l < local_count; ++l)
+ {
+ storage_X[storage_index + offset + l] = points[l].x;// x coordinates of the points we found in STORAGE_X
+ storage_Y[storage_index + offset + l] = points[l].y;// y coordinates of the points we found in STORAGE_Y
+ storage_Z[storage_index + offset + l] = points[l].z;// z coordinates of the points we found in STORAGE_Z
+ storage_I[storage_index + offset + l] = points[l].w;// Intensity values of the points we found in STORAGE_I
+ }
+
+ // Retrieve Zero-crossings as 3D points
+ int offset_storage = old_global_count + lane;
+ for (int idx = lane; idx < total_warp; idx += Warp::STRIDE, offset_storage += Warp::STRIDE)
+ {
+ if (offset_storage >= output_xyz.size) break;
+ float x = storage_X[storage_index + idx];
+ float y = storage_Y[storage_index + idx];
+ float z = storage_Z[storage_index + idx];
+ float i = storage_I[storage_index + idx];
+ store_point_intensity (x, y, z, i, output_xyz.data, output_intensity.data, offset_storage);
+ }
+
+ // Sanity check to make sure our output_xyz buffer is not full already
+ bool full = (old_global_count + total_warp) >= output_xyz.size;
+
+ if (full)
+ break;
+ }
+
+ } /* for(int z = 0; z < VOLUME_Z - 1; ++z) */
+
+ ///////////////////////////
+ // Prepare for future scans
+ if (ftid == 0)
+ {
+ unsigned int total_blocks = gridDim.x * gridDim.y * gridDim.z;
+ unsigned int value = atomicInc (&blocks_done, total_blocks);
+
+ // Last block
+ if (value == total_blocks - 1)
+ {
+ output_xyz_count = min ((int)output_xyz.size, global_count);
+ blocks_done = 0;
+ global_count = 0;
+ }
+ }
+ } /* operator() */
+
+ __device__ __forceinline__ void
+ store_point_type (float x, float y, float z, float4* ptr, int offset) const
+ {
+ *(ptr + offset) = make_float4 (x, y, z, 0);
+ }
+
+ //INLINE FUNCTION THAT STORES XYZ AND INTENSITY VALUES IN 2 SEPARATE DeviceArrays.
+ // ptr_xyz: pointer to the BEGINNING of the XYZ deviceArray
+ // ptr_instensity: pointer to the BEGINNING of the Intensity deviceArray
+ // offset: offset to apply to both XYZ and Intensity
+ __device__ __forceinline__ void
+ store_point_intensity (float x, float y, float z, float i, float4* ptr_xyz, float* ptr_intensity, int offset) const
+ {
+ *(ptr_xyz + offset) = make_float4 (x, y, z, 0);
+ *(ptr_intensity + offset) = i;
+ }
+
+ __device__ __forceinline__ void
+ store_point_type (float x, float y, float z, float3* ptr, int offset) const
+ {
+ *(ptr + offset) = make_float3 (x, y, z);
+ }
+ };
+
+ __global__ void
+ extractKernel (const FullScan6 fs)
+ {
+ fs ();
+ }
+
+ __global__ void
+ extractSliceKernel (const FullScan6 fs, pcl::gpu::kinfuLS::tsdf_buffer buffer, int3 minBounds, int3 maxBounds)
+ {
+ fs (buffer, minBounds, maxBounds);
+ }
+
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ size_t
+ extractCloud (const PtrStep<short2>& volume, const float3& volume_size, PtrSz<PointType> output_xyz)
+ {
+ FullScan6 fs;
+ fs.volume = volume;
+ fs.cell_size.x = volume_size.x / VOLUME_X;
+ fs.cell_size.y = volume_size.y / VOLUME_Y;
+ fs.cell_size.z = volume_size.z / VOLUME_Z;
+ fs.output_xyz = output_xyz;
+
+ dim3 block (CTA_SIZE_X, CTA_SIZE_Y);
+ dim3 grid (divUp (VOLUME_X, block.x), divUp (VOLUME_Y, block.y));
+
+ extractKernel<<<grid, block>>>(fs);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall ( cudaDeviceSynchronize () );
+
+ int size;
+ cudaSafeCall ( cudaMemcpyFromSymbol (&size, output_xyz_count, sizeof (size)) );
+ // cudaSafeCall ( cudaMemcpyFromSymbol (&size, "output_xyz_count", sizeof (size)) );
+ return ((size_t)size);
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ size_t
+ extractSliceAsCloud (const PtrStep<short2>& volume, const float3& volume_size, const pcl::gpu::kinfuLS::tsdf_buffer* buffer,
+ const int shiftX, const int shiftY, const int shiftZ,
+ PtrSz<PointType> output_xyz, PtrSz<float> output_intensities)
+ {
+ FullScan6 fs;
+ fs.volume = volume;
+ fs.cell_size.x = volume_size.x / buffer->voxels_size.x;
+ fs.cell_size.y = volume_size.y / buffer->voxels_size.y;
+ fs.cell_size.z = volume_size.z / buffer->voxels_size.z;
+ fs.output_xyz = output_xyz;
+ fs.output_intensity = output_intensities;
+
+ dim3 block (CTA_SIZE_X, CTA_SIZE_Y);
+ dim3 grid (divUp (VOLUME_X, block.x), divUp (VOLUME_Y, block.y));
+
+ //Compute slice bounds
+ int newX = buffer->origin_GRID.x + shiftX;
+ int newY = buffer->origin_GRID.y + shiftY;
+ int newZ = buffer->origin_GRID.z + shiftZ;
+
+ int3 minBounds, maxBounds;
+
+ //X
+ if (newX >= 0)
+ {
+ minBounds.x = buffer->origin_GRID.x;
+ maxBounds.x = newX;
+ }
+ else
+ {
+ minBounds.x = newX + buffer->voxels_size.x - 1;
+ maxBounds.x = buffer->origin_GRID.x + buffer->voxels_size.x - 1;
+ }
+
+ if (minBounds.x > maxBounds.x)
+ std::swap (minBounds.x, maxBounds.x);
+
+ //Y
+ if (newY >= 0)
+ {
+ minBounds.y = buffer->origin_GRID.y;
+ maxBounds.y = newY;
+ }
+ else
+ {
+ minBounds.y = newY + buffer->voxels_size.y - 1;
+ maxBounds.y = buffer->origin_GRID.y + buffer->voxels_size.y - 1;
+ }
+
+ if(minBounds.y > maxBounds.y)
+ std::swap (minBounds.y, maxBounds.y);
+
+ //Z
+ if (newZ >= 0)
+ {
+ minBounds.z = buffer->origin_GRID.z;
+ maxBounds.z = newZ;
+ }
+ else
+ {
+ minBounds.z = newZ + buffer->voxels_size.z - 1;
+ maxBounds.z = buffer->origin_GRID.z + buffer->voxels_size.z - 1;
+ }
+
+ if (minBounds.z > maxBounds.z)
+ std::swap(minBounds.z, maxBounds.z);
+
+ minBounds.x -= buffer->origin_GRID.x;
+ maxBounds.x -= buffer->origin_GRID.x;
+
+ minBounds.y -= buffer->origin_GRID.y;
+ maxBounds.y -= buffer->origin_GRID.y;
+
+ minBounds.z -= buffer->origin_GRID.z;
+ maxBounds.z -= buffer->origin_GRID.z;
+
+ if (minBounds.x < 0) // We are shifting Left
+ {
+ minBounds.x += buffer->voxels_size.x;
+ maxBounds.x += (buffer->voxels_size.x);
+ }
+
+ if (minBounds.y < 0) // We are shifting up
+ {
+ minBounds.y += buffer->voxels_size.y;
+ maxBounds.y += (buffer->voxels_size.y);
+ }
+
+ if (minBounds.z < 0) // We are shifting back
+ {
+ minBounds.z += buffer->voxels_size.z;
+ maxBounds.z += buffer->voxels_size.z;
+ }
+
+ // Extraction call
+ extractSliceKernel<<<grid, block>>>(fs, *buffer, minBounds, maxBounds);
+
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall ( cudaDeviceSynchronize () );
+
+ int size;
+ cudaSafeCall ( cudaMemcpyFromSymbol (&size, output_xyz_count, sizeof(size)) );
+ return (size_t)size;
+ }
+ }
+ }
+}
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ template<typename NormalType>
+ struct ExtractNormals
+ {
+ float3 cell_size;
+ PtrStep<short2> volume;
+ PtrSz<PointType> points;
+
+ mutable NormalType* output;
+
+ __device__ __forceinline__ float
+ readTsdf (int x, int y, int z) const
+ {
+ return unpack_tsdf (volume.ptr (VOLUME_Y * z + y)[x]);
+ }
+
+ __device__ __forceinline__ float3
+ fetchPoint (int idx) const
+ {
+ PointType p = points.data[idx];
+ return make_float3 (p.x, p.y, p.z);
+ }
+ __device__ __forceinline__ void
+ storeNormal (int idx, float3 normal) const
+ {
+ NormalType n;
+ n.x = normal.x; n.y = normal.y; n.z = normal.z;
+ output[idx] = n;
+ }
+
+ __device__ __forceinline__ int3
+ getVoxel (const float3& point) const
+ {
+ int vx = __float2int_rd (point.x / cell_size.x); // round to negative infinity
+ int vy = __float2int_rd (point.y / cell_size.y);
+ int vz = __float2int_rd (point.z / cell_size.z);
+
+ return make_int3 (vx, vy, vz);
+ }
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int idx = threadIdx.x + blockIdx.x * blockDim.x;
+
+ if (idx >= points.size)
+ return;
+ const float qnan = numeric_limits<float>::quiet_NaN ();
+ float3 n = make_float3 (qnan, qnan, qnan);
+
+ float3 point = fetchPoint (idx);
+ int3 g = getVoxel (point);
+
+ if (g.x > 1 && g.y > 1 && g.z > 1 && g.x < VOLUME_X - 2 && g.y < VOLUME_Y - 2 && g.z < VOLUME_Z - 2)
+ {
+ float3 t;
+
+ t = point;
+ t.x += cell_size.x;
+ float Fx1 = interpolateTrilineary (t);
+
+ t = point;
+ t.x -= cell_size.x;
+ float Fx2 = interpolateTrilineary (t);
+
+ n.x = (Fx1 - Fx2);
+
+ t = point;
+ t.y += cell_size.y;
+ float Fy1 = interpolateTrilineary (t);
+
+ t = point;
+ t.y -= cell_size.y;
+ float Fy2 = interpolateTrilineary (t);
+
+ n.y = (Fy1 - Fy2);
+
+ t = point;
+ t.z += cell_size.z;
+ float Fz1 = interpolateTrilineary (t);
+
+ t = point;
+ t.z -= cell_size.z;
+ float Fz2 = interpolateTrilineary (t);
+
+ n.z = (Fz1 - Fz2);
+
+ n = normalized (n);
+ }
+ storeNormal (idx, n);
+ }
+
+ __device__ __forceinline__ float
+ interpolateTrilineary (const float3& point) const
+ {
+ int3 g = getVoxel (point);
+
+ /*
+ //OLD CODE
+ float vx = (g.x + 0.5f) * cell_size.x;
+ float vy = (g.y + 0.5f) * cell_size.y;
+ float vz = (g.z + 0.5f) * cell_size.z;
+
+ if (point.x < vx) g.x--;
+ if (point.y < vy) g.y--;
+ if (point.z < vz) g.z--;
+
+ //float a = (point.x - (g.x + 0.5f) * cell_size.x) / cell_size.x;
+ //float b = (point.y - (g.y + 0.5f) * cell_size.y) / cell_size.y;
+ //float c = (point.z - (g.z + 0.5f) * cell_size.z) / cell_size.z;
+ float a = point.x/ cell_size.x - (g.x + 0.5f);
+ float b = point.y/ cell_size.y - (g.y + 0.5f);
+ float c = point.z/ cell_size.z - (g.z + 0.5f);
+ */
+ //NEW CODE
+ float a = point.x/ cell_size.x - (g.x + 0.5f); if (a<0) { g.x--; a+=1.0f; };
+ float b = point.y/ cell_size.y - (g.y + 0.5f); if (b<0) { g.y--; b+=1.0f; };
+ float c = point.z/ cell_size.z - (g.z + 0.5f); if (c<0) { g.z--; c+=1.0f; };
+
+ float res = (1 - a) * (
+ (1 - b) * ( readTsdf (g.x + 0, g.y + 0, g.z + 0) * (1 - c) +
+ readTsdf (g.x + 0, g.y + 0, g.z + 1) * c )
+ + b * ( readTsdf (g.x + 0, g.y + 1, g.z + 0) * (1 - c) +
+ readTsdf (g.x + 0, g.y + 1, g.z + 1) * c )
+ ) + a * (
+ (1 - b) * ( readTsdf (g.x + 1, g.y + 0, g.z + 0) * (1 - c) +
+ readTsdf (g.x + 1, g.y + 0, g.z + 1) * c )
+ + b * ( readTsdf (g.x + 1, g.y + 1, g.z + 0) * (1 - c) +
+ readTsdf (g.x + 1, g.y + 1, g.z + 1) * c )
+ );
+
+ return res;
+ }
+ };
+
+ template<typename NormalType>
+ __global__ void
+ extractNormalsKernel (const ExtractNormals<NormalType> en) {
+ en ();
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ template<typename NormalType> void
+ extractNormals (const PtrStep<short2>& volume, const float3& volume_size,
+ const PtrSz<PointType>& points, NormalType* output)
+ {
+ ExtractNormals<NormalType> en;
+ en.volume = volume;
+ en.cell_size.x = volume_size.x / VOLUME_X;
+ en.cell_size.y = volume_size.y / VOLUME_Y;
+ en.cell_size.z = volume_size.z / VOLUME_Z;
+ en.points = points;
+ en.output = output;
+
+ dim3 block (256);
+ dim3 grid (divUp (points.size, block.x));
+
+ extractNormalsKernel<<<grid, block>>>(en);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+
+ template void extractNormals<PointType>(const PtrStep<short2>&volume, const float3 &volume_size, const PtrSz<PointType>&input, PointType * output);
+ template void extractNormals<float8>(const PtrStep<short2>&volume, const float3 &volume_size, const PtrSz<PointType>&input, float8 * output);
+
+ }
+ }
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+//#include <boost/graph/buffer_concepts.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ struct ImageGenerator
+ {
+ enum
+ {
+ CTA_SIZE_X = 32, CTA_SIZE_Y = 8
+ };
+
+ PtrStep<float> vmap;
+ PtrStep<float> nmap;
+
+ LightSource light;
+
+ mutable PtrStepSz<uchar3> dst;
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int x = threadIdx.x + blockIdx.x * CTA_SIZE_X;
+ int y = threadIdx.y + blockIdx.y * CTA_SIZE_Y;
+
+ if (x >= dst.cols || y >= dst.rows)
+ return;
+
+ float3 v, n;
+ v.x = vmap.ptr (y)[x];
+ n.x = nmap.ptr (y)[x];
+
+ uchar3 color = make_uchar3 (0, 0, 0);
+
+ if (!isnan (v.x) && !isnan (n.x))
+ {
+ v.y = vmap.ptr (y + dst.rows)[x];
+ v.z = vmap.ptr (y + 2 * dst.rows)[x];
+
+ n.y = nmap.ptr (y + dst.rows)[x];
+ n.z = nmap.ptr (y + 2 * dst.rows)[x];
+
+ float weight = 1.f;
+
+ for (int i = 0; i < light.number; ++i)
+ {
+ float3 vec = normalized (light.pos[i] - v);
+
+ weight *= fabs (dot (vec, n));
+ }
+
+ int br = (int)(205 * weight) + 50;
+ br = max (0, min (255, br));
+ color = make_uchar3 (br, br, br);
+ }
+ dst.ptr (y)[x] = color;
+ }
+ };
+
+ __global__ void
+ generateImageKernel (const ImageGenerator ig) {
+ ig ();
+ }
+
+ void
+ generateImage (const MapArr& vmap, const MapArr& nmap, const LightSource& light,
+ PtrStepSz<uchar3> dst)
+ {
+ ImageGenerator ig;
+ ig.vmap = vmap;
+ ig.nmap = nmap;
+ ig.light = light;
+ ig.dst = dst;
+
+ dim3 block (ImageGenerator::CTA_SIZE_X, ImageGenerator::CTA_SIZE_Y);
+ dim3 grid (divUp (dst.cols, block.x), divUp (dst.rows, block.y));
+
+ generateImageKernel<<<grid, block>>>(ig);
+ cudaSafeCall (cudaGetLastError ());
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ __global__ void generateDepthKernel(const float3 R_inv_row3, const float3 t, const PtrStep<float> vmap, PtrStepSz<unsigned short> depth)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x < depth.cols && y < depth.rows)
+ {
+ unsigned short result = 0;
+
+ float3 v_g;
+ v_g.x = vmap.ptr (y)[x];
+ if (!isnan (v_g.x))
+ {
+ v_g.y = vmap.ptr (y + depth.rows)[x];
+ v_g.z = vmap.ptr (y + 2 * depth.rows)[x];
+
+ float v_z = dot(R_inv_row3, v_g - t);
+
+ result = static_cast<unsigned short>(v_z * 1000);
+ }
+ depth.ptr(y)[x] = result;
+ }
+ }
+
+ void
+ generateDepth (const Mat33& R_inv, const float3& t, const MapArr& vmap, DepthMap& dst)
+ {
+ dim3 block(32, 8);
+ dim3 grid(divUp(dst.cols(), block.x), divUp(dst.rows(), block.y));
+
+ generateDepthKernel<<<grid, block>>>(R_inv.data[2], t, vmap, dst);
+ cudaSafeCall (cudaGetLastError ());
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+ }
+ }
+}
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ __global__ void
+ paint3DViewKernel(const PtrStep<uchar3> colors, PtrStepSz<uchar3> dst, float colors_weight)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x < dst.cols && y < dst.rows)
+ {
+ uchar3 value = dst.ptr(y)[x];
+ uchar3 color = colors.ptr(y)[x];
+
+ if (value.x != 0 || value.y != 0 || value.z != 0)
+ {
+ float cx = value.x * (1.f - colors_weight) + color.x * colors_weight;
+ float cy = value.y * (1.f - colors_weight) + color.y * colors_weight;
+ float cz = value.z * (1.f - colors_weight) + color.z * colors_weight;
+
+ value.x = min(255, max(0, __float2int_rn(cx)));
+ value.y = min(255, max(0, __float2int_rn(cy)));
+ value.z = min(255, max(0, __float2int_rn(cz)));
+ }
+
+ dst.ptr(y)[x] = value;
+ }
+ }
+
+ void
+ paint3DView(const PtrStep<uchar3>& colors, PtrStepSz<uchar3> dst, float colors_weight)
+ {
+ dim3 block(32, 8);
+ dim3 grid(divUp(dst.cols, block.x), divUp(dst.rows, block.y));
+
+ colors_weight = min(1.f, max(0.f, colors_weight));
+
+ paint3DViewKernel<<<grid, block>>>(colors, dst, colors_weight);
+ cudaSafeCall (cudaGetLastError ());
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+ }
+ }
+}
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+//#include <boost/graph/buffer_concepts.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ __global__ void
+ computeVmapKernel (const PtrStepSz<unsigned short> depth, PtrStep<float> vmap, float fx_inv, float fy_inv, float cx, float cy)
+ {
+ int u = threadIdx.x + blockIdx.x * blockDim.x;
+ int v = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (u < depth.cols && v < depth.rows)
+ {
+ float z = depth.ptr (v)[u] / 1000.f; // load and convert: mm -> meters
+
+ if (z != 0)
+ {
+ float vx = z * (u - cx) * fx_inv;
+ float vy = z * (v - cy) * fy_inv;
+ float vz = z;
+
+ vmap.ptr (v )[u] = vx;
+ vmap.ptr (v + depth.rows )[u] = vy;
+ vmap.ptr (v + depth.rows * 2)[u] = vz;
+ }
+ else
+ vmap.ptr (v)[u] = numeric_limits<float>::quiet_NaN ();
+
+ }
+ }
+
+ __global__ void
+ computeNmapKernel (int rows, int cols, const PtrStep<float> vmap, PtrStep<float> nmap)
+ {
+ int u = threadIdx.x + blockIdx.x * blockDim.x;
+ int v = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (u >= cols || v >= rows)
+ return;
+
+ if (u == cols - 1 || v == rows - 1)
+ {
+ nmap.ptr (v)[u] = numeric_limits<float>::quiet_NaN ();
+ return;
+ }
+
+ float3 v00, v01, v10;
+ v00.x = vmap.ptr (v )[u];
+ v01.x = vmap.ptr (v )[u + 1];
+ v10.x = vmap.ptr (v + 1)[u];
+
+ if (!isnan (v00.x) && !isnan (v01.x) && !isnan (v10.x))
+ {
+ v00.y = vmap.ptr (v + rows)[u];
+ v01.y = vmap.ptr (v + rows)[u + 1];
+ v10.y = vmap.ptr (v + 1 + rows)[u];
+
+ v00.z = vmap.ptr (v + 2 * rows)[u];
+ v01.z = vmap.ptr (v + 2 * rows)[u + 1];
+ v10.z = vmap.ptr (v + 1 + 2 * rows)[u];
+
+ float3 r = normalized (cross (v01 - v00, v10 - v00));
+
+ nmap.ptr (v )[u] = r.x;
+ nmap.ptr (v + rows)[u] = r.y;
+ nmap.ptr (v + 2 * rows)[u] = r.z;
+ }
+ else
+ nmap.ptr (v)[u] = numeric_limits<float>::quiet_NaN ();
+ }
+ }
+ }
+}
+
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ createVMap (const Intr& intr, const DepthMap& depth, MapArr& vmap)
+ {
+ vmap.create (depth.rows () * 3, depth.cols ());
+
+ dim3 block (32, 8);
+ dim3 grid (1, 1, 1);
+ grid.x = divUp (depth.cols (), block.x);
+ grid.y = divUp (depth.rows (), block.y);
+
+ float fx = intr.fx, cx = intr.cx;
+ float fy = intr.fy, cy = intr.cy;
+
+ computeVmapKernel<<<grid, block>>>(depth, vmap, 1.f / fx, 1.f / fy, cx, cy);
+ cudaSafeCall (cudaGetLastError ());
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ createNMap (const MapArr& vmap, MapArr& nmap)
+ {
+ nmap.create (vmap.rows (), vmap.cols ());
+
+ int rows = vmap.rows () / 3;
+ int cols = vmap.cols ();
+
+ dim3 block (32, 8);
+ dim3 grid (1, 1, 1);
+ grid.x = divUp (cols, block.x);
+ grid.y = divUp (rows, block.y);
+
+ computeNmapKernel<<<grid, block>>>(rows, cols, vmap, nmap);
+ cudaSafeCall (cudaGetLastError ());
+ }
+ }
+ }
+}
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ __global__ void
+ transformMapsKernel (int rows, int cols, const PtrStep<float> vmap_src, const PtrStep<float> nmap_src,
+ const Mat33 Rmat, const float3 tvec, PtrStepSz<float> vmap_dst, PtrStep<float> nmap_dst)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ const float qnan = numeric_limits<float>::quiet_NaN ();
+
+ if (x < cols && y < rows)
+ {
+ //vetexes
+ float3 vsrc, vdst = make_float3 (qnan, qnan, qnan);
+ vsrc.x = vmap_src.ptr (y)[x];
+
+ if (!isnan (vsrc.x))
+ {
+ vsrc.y = vmap_src.ptr (y + rows)[x];
+ vsrc.z = vmap_src.ptr (y + 2 * rows)[x];
+
+ vdst = Rmat * vsrc + tvec;
+
+ vmap_dst.ptr (y + rows)[x] = vdst.y;
+ vmap_dst.ptr (y + 2 * rows)[x] = vdst.z;
+ }
+
+ vmap_dst.ptr (y)[x] = vdst.x;
+
+ //normals
+ float3 nsrc, ndst = make_float3 (qnan, qnan, qnan);
+ nsrc.x = nmap_src.ptr (y)[x];
+
+ if (!isnan (nsrc.x))
+ {
+ nsrc.y = nmap_src.ptr (y + rows)[x];
+ nsrc.z = nmap_src.ptr (y + 2 * rows)[x];
+
+ ndst = Rmat * nsrc;
+
+ nmap_dst.ptr (y + rows)[x] = ndst.y;
+ nmap_dst.ptr (y + 2 * rows)[x] = ndst.z;
+ }
+
+ nmap_dst.ptr (y)[x] = ndst.x;
+ }
+ }
+
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ transformMaps (const MapArr& vmap_src, const MapArr& nmap_src,
+ const Mat33& Rmat, const float3& tvec,
+ MapArr& vmap_dst, MapArr& nmap_dst)
+ {
+ int cols = vmap_src.cols ();
+ int rows = vmap_src.rows () / 3;
+
+ vmap_dst.create (rows * 3, cols);
+ nmap_dst.create (rows * 3, cols);
+
+ dim3 block (32, 8);
+ dim3 grid (1, 1, 1);
+ grid.x = divUp (cols, block.x);
+ grid.y = divUp (rows, block.y);
+
+ transformMapsKernel<<<grid, block>>>(rows, cols, vmap_src, nmap_src, Rmat, tvec, vmap_dst, nmap_dst);
+ cudaSafeCall (cudaGetLastError ());
+
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+ }
+ }
+}
+
+
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ template<bool normalize>
+ __global__ void
+ resizeMapKernel (int drows, int dcols, int srows, const PtrStep<float> input, PtrStep<float> output)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x >= dcols || y >= drows)
+ return;
+
+ const float qnan = numeric_limits<float>::quiet_NaN ();
+
+ int xs = x * 2;
+ int ys = y * 2;
+
+ float x00 = input.ptr (ys + 0)[xs + 0];
+ float x01 = input.ptr (ys + 0)[xs + 1];
+ float x10 = input.ptr (ys + 1)[xs + 0];
+ float x11 = input.ptr (ys + 1)[xs + 1];
+
+ if (isnan (x00) || isnan (x01) || isnan (x10) || isnan (x11))
+ {
+ output.ptr (y)[x] = qnan;
+ return;
+ }
+ else
+ {
+ float3 n;
+
+ n.x = (x00 + x01 + x10 + x11) / 4;
+
+ float y00 = input.ptr (ys + srows + 0)[xs + 0];
+ float y01 = input.ptr (ys + srows + 0)[xs + 1];
+ float y10 = input.ptr (ys + srows + 1)[xs + 0];
+ float y11 = input.ptr (ys + srows + 1)[xs + 1];
+
+ n.y = (y00 + y01 + y10 + y11) / 4;
+
+ float z00 = input.ptr (ys + 2 * srows + 0)[xs + 0];
+ float z01 = input.ptr (ys + 2 * srows + 0)[xs + 1];
+ float z10 = input.ptr (ys + 2 * srows + 1)[xs + 0];
+ float z11 = input.ptr (ys + 2 * srows + 1)[xs + 1];
+
+ n.z = (z00 + z01 + z10 + z11) / 4;
+
+ if (normalize)
+ n = normalized (n);
+
+ output.ptr (y )[x] = n.x;
+ output.ptr (y + drows)[x] = n.y;
+ output.ptr (y + 2 * drows)[x] = n.z;
+ }
+ }
+
+ template<bool normalize>
+ void
+ resizeMap (const MapArr& input, MapArr& output)
+ {
+ int in_cols = input.cols ();
+ int in_rows = input.rows () / 3;
+
+ int out_cols = in_cols / 2;
+ int out_rows = in_rows / 2;
+
+ output.create (out_rows * 3, out_cols);
+
+ dim3 block (32, 8);
+ dim3 grid (divUp (out_cols, block.x), divUp (out_rows, block.y));
+ resizeMapKernel<normalize><< < grid, block>>>(out_rows, out_cols, in_rows, input, output);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ resizeVMap (const MapArr& input, MapArr& output)
+ {
+ resizeMap<false>(input, output);
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ resizeNMap (const MapArr& input, MapArr& output)
+ {
+ resizeMap<true>(input, output);
+ }
+
+ }
+ }
+}
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+
+ template<typename T>
+ __global__ void
+ convertMapKernel (int rows, int cols, const PtrStep<float> map, PtrStep<T> output)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x >= cols || y >= rows)
+ return;
+
+ const float qnan = numeric_limits<float>::quiet_NaN ();
+
+ T t;
+ t.x = map.ptr (y)[x];
+ if (!isnan (t.x))
+ {
+ t.y = map.ptr (y + rows)[x];
+ t.z = map.ptr (y + 2 * rows)[x];
+ }
+ else
+ t.y = t.z = qnan;
+
+ output.ptr (y)[x] = t;
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ template<typename T> void
+ convert (const MapArr& vmap, DeviceArray2D<T>& output)
+ {
+ int cols = vmap.cols ();
+ int rows = vmap.rows () / 3;
+
+ output.create (rows, cols);
+
+ dim3 block (32, 8);
+ dim3 grid (divUp (cols, block.x), divUp (rows, block.y));
+
+ convertMapKernel<T><< < grid, block>>>(rows, cols, vmap, output);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+
+ template void convert (const MapArr& vmap, DeviceArray2D<float4>& output);
+ template void convert (const MapArr& vmap, DeviceArray2D<float8>& output);
+ }
+ }
+}
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ __global__ void
+ mergePointNormalKernel (const float4* cloud, const float8* normals, PtrSz<float12> output)
+ {
+ int idx = threadIdx.x + blockIdx.x * blockDim.x;
+
+ if (idx < output.size)
+ {
+ float4 p = cloud[idx];
+ float8 n = normals[idx];
+
+ float12 o;
+ o.x = p.x;
+ o.y = p.y;
+ o.z = p.z;
+
+ o.normal_x = n.x;
+ o.normal_y = n.y;
+ o.normal_z = n.z;
+
+ output.data[idx] = o;
+ }
+ }
+
+ void
+ mergePointNormal (const DeviceArray<float4>& cloud, const DeviceArray<float8>& normals, const DeviceArray<float12>& output)
+ {
+ const int block = 256;
+ int total = (int)output.size ();
+
+ mergePointNormalKernel<<<divUp (total, block), block>>>(cloud, normals, output);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+ }
+ }
+}
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+//#include <boost/graph/buffer_concepts.hpp>
+//~ #include "pcl/gpu/utils/device/block.hpp"
+//~ #include "pcl/gpu/utils/device/warp.hpp"
+//#include "pcl/gpu/utils/device/vector_math.hpp"
+
+#include "thrust/device_ptr.h"
+#include "thrust/scan.h"
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ //texture<int, 1, cudaReadModeElementType> edgeTex;
+ texture<int, 1, cudaReadModeElementType> triTex;
+ texture<int, 1, cudaReadModeElementType> numVertsTex;
+
+ void
+ bindTextures (const int */*edgeBuf*/, const int *triBuf, const int *numVertsBuf)
+ {
+ cudaChannelFormatDesc desc = cudaCreateChannelDesc<int>();
+ //cudaSafeCall(cudaBindTexture(0, edgeTex, edgeBuf, desc) );
+ cudaSafeCall (cudaBindTexture (0, triTex, triBuf, desc) );
+ cudaSafeCall (cudaBindTexture (0, numVertsTex, numVertsBuf, desc) );
+ }
+ void
+ unbindTextures ()
+ {
+ //cudaSafeCall( cudaUnbindTexture(edgeTex) );
+ cudaSafeCall ( cudaUnbindTexture (numVertsTex) );
+ cudaSafeCall ( cudaUnbindTexture (triTex) );
+ }
+ }
+ }
+}
+
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ __device__ int global_count = 0;
+ __device__ int output_count;
+ __device__ unsigned int blocks_done = 0;
+
+ struct CubeIndexEstimator
+ {
+ PtrStep<short2> volume;
+
+ static __device__ __forceinline__ float isoValue() { return 0.f; }
+
+ __device__ __forceinline__ void
+ readTsdf (int x, int y, int z, float& tsdf, int& weight) const
+ {
+ short2 pos = volume.ptr (VOLUME_Y * z + y)[x];
+ unpack_tsdf (pos, tsdf, weight);
+ }
+
+ __device__ __forceinline__ int
+ computeCubeIndex (int x, int y, int z, float f[8]) const
+ {
+ int weight;
+ readTsdf (x, y, z, f[0], weight); if (weight == 0) return 0;
+ readTsdf (x + 1, y, z, f[1], weight); if (weight == 0) return 0;
+ readTsdf (x + 1, y + 1, z, f[2], weight); if (weight == 0) return 0;
+ readTsdf (x, y + 1, z, f[3], weight); if (weight == 0) return 0;
+ readTsdf (x, y, z + 1, f[4], weight); if (weight == 0) return 0;
+ readTsdf (x + 1, y, z + 1, f[5], weight); if (weight == 0) return 0;
+ readTsdf (x + 1, y + 1, z + 1, f[6], weight); if (weight == 0) return 0;
+ readTsdf (x, y + 1, z + 1, f[7], weight); if (weight == 0) return 0;
+
+ // calculate flag indicating if each vertex is inside or outside isosurface
+ int cubeindex;
+ cubeindex = int(f[0] < isoValue());
+ cubeindex += int(f[1] < isoValue()) * 2;
+ cubeindex += int(f[2] < isoValue()) * 4;
+ cubeindex += int(f[3] < isoValue()) * 8;
+ cubeindex += int(f[4] < isoValue()) * 16;
+ cubeindex += int(f[5] < isoValue()) * 32;
+ cubeindex += int(f[6] < isoValue()) * 64;
+ cubeindex += int(f[7] < isoValue()) * 128;
+
+ return cubeindex;
+ }
+ };
+
+ struct OccupiedVoxels : public CubeIndexEstimator
+ {
+ enum
+ {
+ CTA_SIZE_X = 32,
+ CTA_SIZE_Y = 8,
+ CTA_SIZE = CTA_SIZE_X * CTA_SIZE_Y,
+
+ WARPS_COUNT = CTA_SIZE / Warp::WARP_SIZE,
+ };
+
+ mutable int* voxels_indeces;
+ mutable int* vetexes_number;
+ int max_size;
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int x = threadIdx.x + blockIdx.x * CTA_SIZE_X;
+ int y = threadIdx.y + blockIdx.y * CTA_SIZE_Y;
+
+ if (__all (x >= VOLUME_X) || __all (y >= VOLUME_Y))
+ return;
+
+ int ftid = Block::flattenedThreadId ();
+ int warp_id = Warp::id();
+ int lane_id = Warp::laneId();
+
+ volatile __shared__ int warps_buffer[WARPS_COUNT];
+
+ for (int z = 0; z < VOLUME_Z - 1; z++)
+ {
+ int numVerts = 0;;
+ if (x + 1 < VOLUME_X && y + 1 < VOLUME_Y)
+ {
+ float field[8];
+ int cubeindex = computeCubeIndex (x, y, z, field);
+
+ // read number of vertices from texture
+ numVerts = (cubeindex == 0 || cubeindex == 255) ? 0 : tex1Dfetch (numVertsTex, cubeindex);
+ }
+
+ int total = __popc (__ballot (numVerts > 0));
+ if (total == 0)
+ continue;
+
+ if (lane_id == 0)
+ {
+ int old = atomicAdd (&global_count, total);
+ warps_buffer[warp_id] = old;
+ }
+ int old_global_voxels_count = warps_buffer[warp_id];
+
+ int offs = Warp::binaryExclScan (__ballot (numVerts > 0));
+
+ if (old_global_voxels_count + offs < max_size && numVerts > 0)
+ {
+ voxels_indeces[old_global_voxels_count + offs] = VOLUME_Y * VOLUME_X * z + VOLUME_X * y + x;
+ vetexes_number[old_global_voxels_count + offs] = numVerts;
+ }
+
+ bool full = old_global_voxels_count + total >= max_size;
+
+ if (full)
+ break;
+
+ } /* for(int z = 0; z < VOLUME_Z - 1; z++) */
+
+
+ /////////////////////////
+ // prepare for future scans
+ if (ftid == 0)
+ {
+ unsigned int total_blocks = gridDim.x * gridDim.y * gridDim.z;
+ unsigned int value = atomicInc (&blocks_done, total_blocks);
+
+ //last block
+ if (value == total_blocks - 1)
+ {
+ output_count = min (max_size, global_count);
+ blocks_done = 0;
+ global_count = 0;
+ }
+ }
+ } /* operator () */
+ };
+ __global__ void getOccupiedVoxelsKernel (const OccupiedVoxels ov) { ov (); }
+
+ int
+ getOccupiedVoxels (const PtrStep<short2>& volume, DeviceArray2D<int>& occupied_voxels)
+ {
+ OccupiedVoxels ov;
+ ov.volume = volume;
+
+ ov.voxels_indeces = occupied_voxels.ptr (0);
+ ov.vetexes_number = occupied_voxels.ptr (1);
+ ov.max_size = occupied_voxels.cols ();
+
+ dim3 block (OccupiedVoxels::CTA_SIZE_X, OccupiedVoxels::CTA_SIZE_Y);
+ dim3 grid (divUp (VOLUME_X, block.x), divUp (VOLUME_Y, block.y));
+
+ //cudaFuncSetCacheConfig(getOccupiedVoxelsKernel, cudaFuncCachePreferL1);
+ //printFuncAttrib(getOccupiedVoxelsKernel);
+
+ getOccupiedVoxelsKernel<<<grid, block>>>(ov);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+
+ int size;
+ cudaSafeCall ( cudaMemcpyFromSymbol (&size, output_count, sizeof(size)) );
+ return size;
+ }
+
+ int
+ computeOffsetsAndTotalVertexes (DeviceArray2D<int>& occupied_voxels)
+ {
+ thrust::device_ptr<int> beg (occupied_voxels.ptr (1));
+ thrust::device_ptr<int> end = beg + occupied_voxels.cols ();
+
+ thrust::device_ptr<int> out (occupied_voxels.ptr (2));
+ thrust::exclusive_scan (beg, end, out);
+
+ int lastElement, lastScanElement;
+
+ DeviceArray<int> last_elem (occupied_voxels.ptr(1) + occupied_voxels.cols () - 1, 1);
+ DeviceArray<int> last_scan (occupied_voxels.ptr(2) + occupied_voxels.cols () - 1, 1);
+
+ last_elem.download (&lastElement);
+ last_scan.download (&lastScanElement);
+
+ return lastElement + lastScanElement;
+ }
+ }
+ }
+}
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ struct TrianglesGenerator : public CubeIndexEstimator
+ {
+ enum { CTA_SIZE = 256 };
+
+ const int* occupied_voxels;
+ const int* vertex_ofssets;
+ int voxels_count;
+ float3 cell_size;
+
+ mutable PointType *output;
+
+ __device__ __forceinline__ float3
+ getNodeCoo (int x, int y, int z) const
+ {
+ float3 coo = make_float3 (x, y, z);
+ coo += 0.5f; //shift to volume cell center;
+
+ coo.x *= cell_size.x;
+ coo.y *= cell_size.y;
+ coo.z *= cell_size.z;
+
+ return coo;
+ }
+
+ __device__ __forceinline__ float3
+ vertex_interp (float3 p0, float3 p1, float f0, float f1) const
+ {
+ float t = (isoValue() - f0) / (f1 - f0 + 1e-15f);
+ float x = p0.x + t * (p1.x - p0.x);
+ float y = p0.y + t * (p1.y - p0.y);
+ float z = p0.z + t * (p1.z - p0.z);
+ return make_float3 (x, y, z);
+ }
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int tid = threadIdx.x;
+ int idx = blockIdx.x * CTA_SIZE + tid;
+
+ if (idx >= voxels_count)
+ return;
+
+ int voxel = occupied_voxels[idx];
+
+ int z = voxel / (VOLUME_X * VOLUME_Y);
+ int y = (voxel - z * VOLUME_X * VOLUME_Y) / VOLUME_X;
+ int x = (voxel - z * VOLUME_X * VOLUME_Y) - y * VOLUME_X;
+
+ float f[8];
+ int cubeindex = computeCubeIndex (x, y, z, f);
+
+ // calculate cell vertex positions
+ float3 v[8];
+ v[0] = getNodeCoo (x, y, z);
+ v[1] = getNodeCoo (x + 1, y, z);
+ v[2] = getNodeCoo (x + 1, y + 1, z);
+ v[3] = getNodeCoo (x, y + 1, z);
+ v[4] = getNodeCoo (x, y, z + 1);
+ v[5] = getNodeCoo (x + 1, y, z + 1);
+ v[6] = getNodeCoo (x + 1, y + 1, z + 1);
+ v[7] = getNodeCoo (x, y + 1, z + 1);
+
+ // find the vertices where the surface intersects the cube
+ // use shared memory to avoid using local
+ __shared__ float3 vertlist[12][CTA_SIZE];
+
+ vertlist[0][tid] = vertex_interp (v[0], v[1], f[0], f[1]);
+ vertlist[1][tid] = vertex_interp (v[1], v[2], f[1], f[2]);
+ vertlist[2][tid] = vertex_interp (v[2], v[3], f[2], f[3]);
+ vertlist[3][tid] = vertex_interp (v[3], v[0], f[3], f[0]);
+ vertlist[4][tid] = vertex_interp (v[4], v[5], f[4], f[5]);
+ vertlist[5][tid] = vertex_interp (v[5], v[6], f[5], f[6]);
+ vertlist[6][tid] = vertex_interp (v[6], v[7], f[6], f[7]);
+ vertlist[7][tid] = vertex_interp (v[7], v[4], f[7], f[4]);
+ vertlist[8][tid] = vertex_interp (v[0], v[4], f[0], f[4]);
+ vertlist[9][tid] = vertex_interp (v[1], v[5], f[1], f[5]);
+ vertlist[10][tid] = vertex_interp (v[2], v[6], f[2], f[6]);
+ vertlist[11][tid] = vertex_interp (v[3], v[7], f[3], f[7]);
+ __syncthreads ();
+
+ // output triangle vertices
+ int numVerts = tex1Dfetch (numVertsTex, cubeindex);
+
+ for (int i = 0; i < numVerts; i += 3)
+ {
+ int index = vertex_ofssets[idx] + i;
+
+ int v1 = tex1Dfetch (triTex, (cubeindex * 16) + i + 0);
+ int v2 = tex1Dfetch (triTex, (cubeindex * 16) + i + 1);
+ int v3 = tex1Dfetch (triTex, (cubeindex * 16) + i + 2);
+
+ store_point (output, index + 0, vertlist[v1][tid]);
+ store_point (output, index + 1, vertlist[v2][tid]);
+ store_point (output, index + 2, vertlist[v3][tid]);
+ }
+ }
+
+ __device__ __forceinline__ void
+ store_point (float4 *ptr, int index, const float3& point) const {
+ ptr[index] = make_float4 (point.x, point.y, point.z, 1.0f);
+ }
+ };
+ __global__ void
+ trianglesGeneratorKernel (const TrianglesGenerator tg) {tg (); }
+
+ void
+ generateTriangles (const PtrStep<short2>& volume, const DeviceArray2D<int>& occupied_voxels, const float3& volume_size, DeviceArray<PointType>& output)
+ {
+ TrianglesGenerator tg;
+
+ tg.volume = volume;
+ tg.occupied_voxels = occupied_voxels.ptr (0);
+ tg.vertex_ofssets = occupied_voxels.ptr (2);
+ tg.voxels_count = occupied_voxels.cols ();
+ tg.cell_size.x = volume_size.x / VOLUME_X;
+ tg.cell_size.y = volume_size.y / VOLUME_Y;
+ tg.cell_size.z = volume_size.z / VOLUME_Z;
+ tg.output = output;
+
+ dim3 block (TrianglesGenerator::CTA_SIZE);
+ dim3 grid (divUp (tg.voxels_count, block.x));
+
+ trianglesGeneratorKernel<<<grid, block>>>(tg);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+ }
+ }
+}
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+//#include <boost/graph/buffer_concepts.hpp>
+//#include <pcl/gpu/features/device/eigen.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ enum
+ {
+ kx = 7,
+ ky = 7,
+ STEP = 1
+ };
+
+ __global__ void
+ computeNmapKernelEigen (int rows, int cols, const PtrStep<float> vmap, PtrStep<float> nmap)
+ {
+ int u = threadIdx.x + blockIdx.x * blockDim.x;
+ int v = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (u >= cols || v >= rows)
+ return;
+
+ nmap.ptr (v)[u] = numeric_limits<float>::quiet_NaN ();
+
+ if (isnan (vmap.ptr (v)[u]))
+ return;
+
+ int ty = min (v - ky / 2 + ky, rows - 1);
+ int tx = min (u - kx / 2 + kx, cols - 1);
+
+ float3 centroid = make_float3 (0.f, 0.f, 0.f);
+ int counter = 0;
+ for (int cy = max (v - ky / 2, 0); cy < ty; cy += STEP)
+ for (int cx = max (u - kx / 2, 0); cx < tx; cx += STEP)
+ {
+ float v_x = vmap.ptr (cy)[cx];
+ if (!isnan (v_x))
+ {
+ centroid.x += v_x;
+ centroid.y += vmap.ptr (cy + rows)[cx];
+ centroid.z += vmap.ptr (cy + 2 * rows)[cx];
+ ++counter;
+ }
+ }
+
+ if (counter < kx * ky / 2)
+ return;
+
+ centroid *= 1.f / counter;
+
+ float cov[] = {0, 0, 0, 0, 0, 0};
+
+ for (int cy = max (v - ky / 2, 0); cy < ty; cy += STEP)
+ for (int cx = max (u - kx / 2, 0); cx < tx; cx += STEP)
+ {
+ float3 v;
+ v.x = vmap.ptr (cy)[cx];
+ if (isnan (v.x))
+ continue;
+
+ v.y = vmap.ptr (cy + rows)[cx];
+ v.z = vmap.ptr (cy + 2 * rows)[cx];
+
+ float3 d = v - centroid;
+
+ cov[0] += d.x * d.x; //cov (0, 0)
+ cov[1] += d.x * d.y; //cov (0, 1)
+ cov[2] += d.x * d.z; //cov (0, 2)
+ cov[3] += d.y * d.y; //cov (1, 1)
+ cov[4] += d.y * d.z; //cov (1, 2)
+ cov[5] += d.z * d.z; //cov (2, 2)
+ }
+
+ typedef Eigen33::Mat33 Mat33;
+ Eigen33 eigen33 (cov);
+
+ Mat33 tmp;
+ Mat33 vec_tmp;
+ Mat33 evecs;
+ float3 evals;
+ eigen33.compute (tmp, vec_tmp, evecs, evals);
+
+ float3 n = normalized (evecs[0]);
+
+ u = threadIdx.x + blockIdx.x * blockDim.x;
+ v = threadIdx.y + blockIdx.y * blockDim.y;
+
+ nmap.ptr (v )[u] = n.x;
+ nmap.ptr (v + rows)[u] = n.y;
+ nmap.ptr (v + 2 * rows)[u] = n.z;
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ computeNormalsEigen (const MapArr& vmap, MapArr& nmap)
+ {
+ int cols = vmap.cols ();
+ int rows = vmap.rows () / 3;
+
+ nmap.create (vmap.rows (), vmap.cols ());
+
+ dim3 block (32, 8);
+ dim3 grid (1, 1, 1);
+ grid.x = divUp (cols, block.x);
+ grid.y = divUp (rows, block.y);
+
+ computeNmapKernelEigen<<<grid, block>>>(rows, cols, vmap, nmap);
+ cudaSafeCall (cudaGetLastError ());
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+ }
+ }
+}
--- /dev/null
+#include <pcl/gpu/kinfu_large_scale/tsdf_buffer.h>
+
+__device__ __forceinline__ static void
+shift_tsdf_pointer(short2 ** value, pcl::gpu::kinfuLS::tsdf_buffer buffer)
+{
+ ///Shift the pointer by (@origin - @start)
+ *value += (buffer.tsdf_rolling_buff_origin - buffer.tsdf_memory_start);
+
+ ///If we land outside of the memory, make sure to "modulo" the new value
+ if(*value > buffer.tsdf_memory_end)
+ {
+ *value -= (buffer.tsdf_memory_end - buffer.tsdf_memory_start + 1); /// correction of bug found my qianyizh
+ }
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+//#include <boost/graph/buffer_concepts.hpp>
+
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ __global__ void
+ loadTsdfValueKernel (short2* first_point_pointer, float4 *data_ptr, int number_of_points, const int divisor, pcl::gpu::kinfuLS::tsdf_buffer buffer)
+ {
+ int i = blockDim.x * blockIdx.x + threadIdx.x;
+ if (i < number_of_points)
+ {
+ float4 *point_ptr = data_ptr + (i * 2);
+ float4 *tsdfvalue_ptr = point_ptr + 1;
+
+ if( //Indices x,y,z are within [0...VOLUME_{X,Y,Z}]
+ ( ( (int) ( (*point_ptr).x ) < buffer.voxels_size.x) && ( (int) ( (*point_ptr).x ) >= 0) ) ||
+ ( ( (int) ( (*point_ptr).y ) < buffer.voxels_size.y) && ( (int) ( (*point_ptr).y ) >= 0) ) ||
+ ( ( (int) ( (*point_ptr).z ) < buffer.voxels_size.z) && ( (int) ( (*point_ptr).z ) >= 0) )
+ )//Not sure whether this long condition is necessary since destination indices are calculated based on rolling buffer
+ {
+ int dest_x = ( (int) ( (*point_ptr).x ) + buffer.origin_GRID.x ) % buffer.voxels_size.x;
+ int dest_y = ( (int) ( (*point_ptr).y ) + buffer.origin_GRID.y ) % buffer.voxels_size.y;
+ int dest_z = ( (int) ( (*point_ptr).z ) + buffer.origin_GRID.z ) % buffer.voxels_size.z;
+
+ short2 *dest_tsdf_index = first_point_pointer + ( buffer.voxels_size.y * buffer.voxels_size.x * dest_z + ( buffer.voxels_size.x * dest_y ) ) + dest_x;
+ (*dest_tsdf_index).x = (*tsdfvalue_ptr).x * divisor;
+ (*dest_tsdf_index).y = 1.0;
+ }
+ }
+ else
+ return;
+ }
+
+ void
+ pushCloudAsSliceGPU (const PtrStep<short2>& volume, pcl::gpu::DeviceArray<PointType> cloud_gpu, const pcl::gpu::kinfuLS::tsdf_buffer* buffer)
+ {
+ // Because every point is 8 floats.
+ // X Y Z 0
+ // I 0 0 0
+ int number_of_points = cloud_gpu.size () / 2;
+
+ if(number_of_points == 0)
+ return;
+
+ const int DIVISOR = 32767;
+
+ const short2 *first_point_pointer_const = &(volume.ptr (0)[0]);
+ short2 *first_point_pointer = const_cast<short2*>(first_point_pointer_const);
+
+ const int BlockX = 512;
+ const int BlockY = 1;
+ const int BlockSize = BlockX * BlockY;
+ int numBlocks = divUp (number_of_points, BlockSize);
+ dim3 threadsPerBlock (BlockX, BlockY);
+
+ loadTsdfValueKernel <<< numBlocks, threadsPerBlock >>> (first_point_pointer, cloud_gpu.ptr(), number_of_points, DIVISOR, *buffer);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall ( cudaDeviceSynchronize () );
+ }
+ } /*namespace kinfuLS*/
+ } /*namespace device*/
+} /*namespace pcl*/
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+#include "device.hpp"
+//#include <boost/graph/buffer_concepts.hpp>
+
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ __device__ __forceinline__ float
+ getMinTime (const float3& volume_max, const float3& origin, const float3& dir)
+ {
+ float txmin = ( (dir.x > 0 ? 0.f : volume_max.x) - origin.x) / dir.x;
+ float tymin = ( (dir.y > 0 ? 0.f : volume_max.y) - origin.y) / dir.y;
+ float tzmin = ( (dir.z > 0 ? 0.f : volume_max.z) - origin.z) / dir.z;
+
+ return fmax ( fmax (txmin, tymin), tzmin);
+ }
+
+ __device__ __forceinline__ float
+ getMaxTime (const float3& volume_max, const float3& origin, const float3& dir)
+ {
+ float txmax = ( (dir.x > 0 ? volume_max.x : 0.f) - origin.x) / dir.x;
+ float tymax = ( (dir.y > 0 ? volume_max.y : 0.f) - origin.y) / dir.y;
+ float tzmax = ( (dir.z > 0 ? volume_max.z : 0.f) - origin.z) / dir.z;
+
+ return fmin (fmin (txmax, tymax), tzmax);
+ }
+
+ struct RayCaster
+ {
+ enum { CTA_SIZE_X = 32, CTA_SIZE_Y = 8 };
+
+ Mat33 Rcurr;
+ float3 tcurr;
+
+ float time_step;
+ float3 volume_size;
+
+ float3 cell_size;
+ int cols, rows;
+
+ PtrStep<short2> volume;
+
+ Intr intr;
+
+ mutable PtrStep<float> nmap;
+ mutable PtrStep<float> vmap;
+
+ __device__ __forceinline__ float3
+ get_ray_next (int x, int y) const
+ {
+ float3 ray_next;
+ ray_next.x = (x - intr.cx) / intr.fx;
+ ray_next.y = (y - intr.cy) / intr.fy;
+ ray_next.z = 1;
+ return ray_next;
+ }
+
+ __device__ __forceinline__ bool
+ checkInds (const int3& g) const
+ {
+ return (g.x >= 0 && g.y >= 0 && g.z >= 0 && g.x < VOLUME_X && g.y < VOLUME_Y && g.z < VOLUME_Z);
+ }
+
+ __device__ __forceinline__ float
+ readTsdf (int x, int y, int z, pcl::gpu::kinfuLS::tsdf_buffer buffer) const
+ {
+ const short2* tmp_pos = &(volume.ptr (buffer.voxels_size.y * z + y)[x]);
+ short2* pos = const_cast<short2*> (tmp_pos);
+ shift_tsdf_pointer(&pos, buffer);
+ return unpack_tsdf (*pos);
+ }
+
+ __device__ __forceinline__ int3
+ getVoxel (float3 point) const
+ {
+ int vx = __float2int_rd (point.x / cell_size.x); // round to negative infinity
+ int vy = __float2int_rd (point.y / cell_size.y);
+ int vz = __float2int_rd (point.z / cell_size.z);
+
+ return make_int3 (vx, vy, vz);
+ }
+
+ __device__ __forceinline__ float
+ interpolateTrilineary (const float3& origin, const float3& dir, float time, pcl::gpu::kinfuLS::tsdf_buffer buffer) const
+ {
+ return interpolateTrilineary (origin + dir * time, buffer);
+ }
+
+ __device__ __forceinline__ float
+ interpolateTrilineary (const float3& point, pcl::gpu::kinfuLS::tsdf_buffer buffer) const
+ {
+ int3 g = getVoxel (point);
+
+ if (g.x <= 0 || g.x >= buffer.voxels_size.x - 1)
+ return numeric_limits<float>::quiet_NaN ();
+
+ if (g.y <= 0 || g.y >= buffer.voxels_size.y - 1)
+ return numeric_limits<float>::quiet_NaN ();
+
+ if (g.z <= 0 || g.z >= buffer.voxels_size.z - 1)
+ return numeric_limits<float>::quiet_NaN ();
+
+ /* //OLD CODE
+ float vx = (g.x + 0.5f) * cell_size.x;
+ float vy = (g.y + 0.5f) * cell_size.y;
+ float vz = (g.z + 0.5f) * cell_size.z;
+
+ g.x = (point.x < vx) ? (g.x - 1) : g.x;
+ g.y = (point.y < vy) ? (g.y - 1) : g.y;
+ g.z = (point.z < vz) ? (g.z - 1) : g.z;
+
+ float a = (point.x - (g.x + 0.5f) * cell_size.x) / cell_size.x;
+ float b = (point.y - (g.y + 0.5f) * cell_size.y) / cell_size.y;
+ float c = (point.z - (g.z + 0.5f) * cell_size.z) / cell_size.z;
+
+ float res = readTsdf (g.x + 0, g.y + 0, g.z + 0, buffer) * (1 - a) * (1 - b) * (1 - c) +
+ readTsdf (g.x + 0, g.y + 0, g.z + 1, buffer) * (1 - a) * (1 - b) * c +
+ readTsdf (g.x + 0, g.y + 1, g.z + 0, buffer) * (1 - a) * b * (1 - c) +
+ readTsdf (g.x + 0, g.y + 1, g.z + 1, buffer) * (1 - a) * b * c +
+ readTsdf (g.x + 1, g.y + 0, g.z + 0, buffer) * a * (1 - b) * (1 - c) +
+ readTsdf (g.x + 1, g.y + 0, g.z + 1, buffer) * a * (1 - b) * c +
+ readTsdf (g.x + 1, g.y + 1, g.z + 0, buffer) * a * b * (1 - c) +
+ readTsdf (g.x + 1, g.y + 1, g.z + 1, buffer) * a * b * c;
+ */
+ //NEW CODE
+ float a = point.x/ cell_size.x - (g.x + 0.5f); if (a<0) { g.x--; a+=1.0f; };
+ float b = point.y/ cell_size.y - (g.y + 0.5f); if (b<0) { g.y--; b+=1.0f; };
+ float c = point.z/ cell_size.z - (g.z + 0.5f); if (c<0) { g.z--; c+=1.0f; };
+
+ float res = (1 - a) * (
+ (1 - b) * (
+ readTsdf (g.x + 0, g.y + 0, g.z + 0, buffer) * (1 - c) +
+ readTsdf (g.x + 0, g.y + 0, g.z + 1, buffer) * c
+ )
+ + b * (
+ readTsdf (g.x + 0, g.y + 1, g.z + 0, buffer) * (1 - c) +
+ readTsdf (g.x + 0, g.y + 1, g.z + 1, buffer) * c
+ )
+ )
+ + a * (
+ (1 - b) * (
+ readTsdf (g.x + 1, g.y + 0, g.z + 0, buffer) * (1 - c) +
+ readTsdf (g.x + 1, g.y + 0, g.z + 1, buffer) * c
+ )
+ + b * (
+ readTsdf (g.x + 1, g.y + 1, g.z + 0, buffer) * (1 - c) +
+ readTsdf (g.x + 1, g.y + 1, g.z + 1, buffer) * c
+ )
+ )
+ ;
+ return res;
+ }
+
+
+ __device__ __forceinline__ void
+ operator () (pcl::gpu::kinfuLS::tsdf_buffer buffer) const
+ {
+ int x = threadIdx.x + blockIdx.x * CTA_SIZE_X;
+ int y = threadIdx.y + blockIdx.y * CTA_SIZE_Y;
+
+ if (x >= cols || y >= rows)
+ return;
+
+ vmap.ptr (y)[x] = numeric_limits<float>::quiet_NaN ();
+ nmap.ptr (y)[x] = numeric_limits<float>::quiet_NaN ();
+
+ float3 ray_start = tcurr;
+ float3 ray_next = Rcurr * get_ray_next (x, y) + tcurr;
+
+ float3 ray_dir = normalized (ray_next - ray_start);
+
+ //ensure that it isn't a degenerate case
+ ray_dir.x = (ray_dir.x == 0.f) ? 1e-15 : ray_dir.x;
+ ray_dir.y = (ray_dir.y == 0.f) ? 1e-15 : ray_dir.y;
+ ray_dir.z = (ray_dir.z == 0.f) ? 1e-15 : ray_dir.z;
+
+ // computer time when entry and exit volume
+ float time_start_volume = getMinTime (volume_size, ray_start, ray_dir);
+ float time_exit_volume = getMaxTime (volume_size, ray_start, ray_dir);
+
+ const float min_dist = 0.f; //in meters
+ time_start_volume = fmax (time_start_volume, min_dist);
+ if (time_start_volume >= time_exit_volume)
+ return;
+
+ float time_curr = time_start_volume;
+ int3 g = getVoxel (ray_start + ray_dir * time_curr);
+ g.x = max (0, min (g.x, buffer.voxels_size.x - 1));
+ g.y = max (0, min (g.y, buffer.voxels_size.y - 1));
+ g.z = max (0, min (g.z, buffer.voxels_size.z - 1));
+
+ float tsdf = readTsdf (g.x, g.y, g.z, buffer);
+
+ //infinite loop guard
+ const float max_time = 3 * (volume_size.x + volume_size.y + volume_size.z);
+
+ for (; time_curr < max_time; time_curr += time_step)
+ {
+ float tsdf_prev = tsdf;
+
+ int3 g = getVoxel ( ray_start + ray_dir * (time_curr + time_step) );
+ if (!checkInds (g))
+ break;
+
+ tsdf = readTsdf (g.x, g.y, g.z, buffer);
+
+ if (tsdf_prev < 0.f && tsdf > 0.f)
+ break;
+
+ if (tsdf_prev > 0.f && tsdf < 0.f) //zero crossing
+ {
+ float Ftdt = interpolateTrilineary (ray_start, ray_dir, time_curr + time_step, buffer);
+ if (isnan (Ftdt))
+ break;
+
+ float Ft = interpolateTrilineary (ray_start, ray_dir, time_curr, buffer);
+ if (isnan (Ft))
+ break;
+
+ //float Ts = time_curr - time_step * Ft/(Ftdt - Ft);
+ float Ts = time_curr - time_step * Ft / (Ftdt - Ft);
+
+ float3 vetex_found = ray_start + ray_dir * Ts;
+
+ vmap.ptr (y )[x] = vetex_found.x;
+ vmap.ptr (y + rows)[x] = vetex_found.y;
+ vmap.ptr (y + 2 * rows)[x] = vetex_found.z;
+
+ int3 g = getVoxel ( ray_start + ray_dir * time_curr );
+ if (g.x > 1 && g.y > 1 && g.z > 1 && g.x < buffer.voxels_size.x - 2 && g.y < buffer.voxels_size.y - 2 && g.z < buffer.voxels_size.z - 2)
+ {
+ float3 t;
+ float3 n;
+
+ t = vetex_found;
+ t.x += cell_size.x;
+ float Fx1 = interpolateTrilineary (t, buffer);
+
+ t = vetex_found;
+ t.x -= cell_size.x;
+ float Fx2 = interpolateTrilineary (t, buffer);
+
+ n.x = (Fx1 - Fx2);
+
+ t = vetex_found;
+ t.y += cell_size.y;
+ float Fy1 = interpolateTrilineary (t, buffer);
+
+ t = vetex_found;
+ t.y -= cell_size.y;
+ float Fy2 = interpolateTrilineary (t, buffer);
+
+ n.y = (Fy1 - Fy2);
+
+ t = vetex_found;
+ t.z += cell_size.z;
+ float Fz1 = interpolateTrilineary (t, buffer);
+
+ t = vetex_found;
+ t.z -= cell_size.z;
+ float Fz2 = interpolateTrilineary (t, buffer);
+
+ n.z = (Fz1 - Fz2);
+
+ n = normalized (n);
+
+ nmap.ptr (y )[x] = n.x;
+ nmap.ptr (y + rows)[x] = n.y;
+ nmap.ptr (y + 2 * rows)[x] = n.z;
+ }
+ break;
+ }
+
+ } /* for(;;) */
+ }
+ };
+
+ __global__ void
+ rayCastKernel (const RayCaster rc, pcl::gpu::kinfuLS::tsdf_buffer buffer) {
+ rc (buffer);
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ raycast (const Intr& intr, const Mat33& Rcurr, const float3& tcurr,
+ float tranc_dist, const float3& volume_size,
+ const PtrStep<short2>& volume, const pcl::gpu::kinfuLS::tsdf_buffer* buffer, MapArr& vmap, MapArr& nmap)
+ {
+ RayCaster rc;
+
+ rc.Rcurr = Rcurr;
+ rc.tcurr = tcurr;
+
+ rc.time_step = tranc_dist * 0.8f;
+
+ rc.volume_size = volume_size;
+
+ rc.cell_size.x = volume_size.x / buffer->voxels_size.x;
+ rc.cell_size.y = volume_size.y / buffer->voxels_size.y;
+ rc.cell_size.z = volume_size.z / buffer->voxels_size.z;
+
+ rc.cols = vmap.cols ();
+ rc.rows = vmap.rows () / 3;
+
+ rc.intr = intr;
+
+ rc.volume = volume;
+ rc.vmap = vmap;
+ rc.nmap = nmap;
+
+ dim3 block (RayCaster::CTA_SIZE_X, RayCaster::CTA_SIZE_Y);
+ dim3 grid (divUp (rc.cols, block.x), divUp (rc.rows, block.y));
+
+ rayCastKernel<<<grid, block>>>(rc, *buffer);
+ cudaSafeCall (cudaGetLastError ());
+ cudaSafeCall(cudaDeviceSynchronize());
+ }
+ }
+ }
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "device.hpp"
+//#include <boost/graph/buffer_concepts.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ template<typename T>
+ __global__ void
+ initializeVolume (PtrStep<T> volume)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+
+ if (x < VOLUME_X && y < VOLUME_Y)
+ {
+ T *pos = volume.ptr(y) + x;
+ int z_step = VOLUME_Y * volume.step / sizeof(*pos);
+
+ #pragma unroll
+ for(int z = 0; z < VOLUME_Z; ++z, pos+=z_step)
+ pack_tsdf (0.f, 0, *pos);
+ }
+ }
+
+ template<typename T>
+ __global__ void
+ clearSliceKernel (PtrStep<T> volume, pcl::gpu::kinfuLS::tsdf_buffer buffer, int3 minBounds, int3 maxBounds)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ //compute relative indices
+ int idX, idY;
+
+ if(x <= minBounds.x)
+ idX = x + buffer.voxels_size.x;
+ else
+ idX = x;
+
+ if(y <= minBounds.y)
+ idY = y + buffer.voxels_size.y;
+ else
+ idY = y;
+
+
+ if ( x < buffer.voxels_size.x && y < buffer.voxels_size.y)
+ {
+ if( (idX >= minBounds.x && idX <= maxBounds.x) || (idY >= minBounds.y && idY <= maxBounds.y) )
+ {
+ // BLACK ZONE => clear on all Z values
+
+ ///Pointer to the first x,y,0
+ T *pos = volume.ptr(y) + x;
+
+ ///Get the step on Z
+ int z_step = buffer.voxels_size.y * volume.step / sizeof(*pos);
+
+ ///Get the size of the whole TSDF memory
+ int size = buffer.tsdf_memory_end - buffer.tsdf_memory_start + 1;
+
+ ///Move along z axis
+ #pragma unroll
+ for(int z = 0; z < buffer.voxels_size.z; ++z, pos+=z_step)
+ {
+ ///If we went outside of the memory, make sure we go back to the begining of it
+ if(pos > buffer.tsdf_memory_end)
+ pos = pos - size;
+
+ if (pos >= buffer.tsdf_memory_start && pos <= buffer.tsdf_memory_end) // quickfix for http://dev.pointclouds.org/issues/894
+ pack_tsdf (0.f, 0, *pos);
+ }
+ }
+ else /* if( idX > maxBounds.x && idY > maxBounds.y)*/
+ {
+
+ ///RED ZONE => clear only appropriate Z
+
+ ///Pointer to the first x,y,0
+ T *pos = volume.ptr(y) + x;
+
+ ///Get the step on Z
+ int z_step = buffer.voxels_size.y * volume.step / sizeof(*pos);
+
+ ///Get the size of the whole TSDF memory
+ int size = buffer.tsdf_memory_end - buffer.tsdf_memory_start + 1;
+
+ ///Move pointer to the Z origin
+ pos+= minBounds.z * z_step;
+
+ ///If the Z offset is negative, we move the pointer back
+ if(maxBounds.z < 0)
+ pos += maxBounds.z * z_step;
+
+ ///We make sure that we are not already before the start of the memory
+ if(pos < buffer.tsdf_memory_start)
+ pos = pos + size;
+
+ int nbSteps = abs(maxBounds.z);
+
+ #pragma unroll
+ for(int z = 0; z < nbSteps; ++z, pos+=z_step)
+ {
+ ///If we went outside of the memory, make sure we go back to the begining of it
+ if(pos > buffer.tsdf_memory_end)
+ pos = pos - size;
+
+ if (pos >= buffer.tsdf_memory_start && pos <= buffer.tsdf_memory_end) // quickfix for http://dev.pointclouds.org/issues/894
+ pack_tsdf (0.f, 0, *pos);
+ }
+ } //else /* if( idX > maxBounds.x && idY > maxBounds.y)*/
+ } // if ( x < VOLUME_X && y < VOLUME_Y)
+ } // clearSliceKernel
+
+ void
+ initVolume (PtrStep<short2> volume)
+ {
+ dim3 block (16, 16);
+ dim3 grid (1, 1, 1);
+ grid.x = divUp (VOLUME_X, block.x);
+ grid.y = divUp (VOLUME_Y, block.y);
+
+ initializeVolume<<<grid, block>>>(volume);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+ }
+ }
+}
+
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ struct Tsdf
+ {
+ enum
+ {
+ CTA_SIZE_X = 32, CTA_SIZE_Y = 8,
+ MAX_WEIGHT = 1 << 7
+ };
+
+ mutable PtrStep<short2> volume;
+ float3 cell_size;
+
+ Intr intr;
+
+ Mat33 Rcurr_inv;
+ float3 tcurr;
+
+ PtrStepSz<ushort> depth_raw; //depth in mm
+
+ float tranc_dist_mm;
+
+ __device__ __forceinline__ float3
+ getVoxelGCoo (int x, int y, int z) const
+ {
+ float3 coo = make_float3 (x, y, z);
+ coo += 0.5f; //shift to cell center;
+
+ coo.x *= cell_size.x;
+ coo.y *= cell_size.y;
+ coo.z *= cell_size.z;
+
+ return coo;
+ }
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ int x = threadIdx.x + blockIdx.x * CTA_SIZE_X;
+ int y = threadIdx.y + blockIdx.y * CTA_SIZE_Y;
+
+ if (x >= VOLUME_X || y >= VOLUME_Y)
+ return;
+
+ short2 *pos = volume.ptr (y) + x;
+ int elem_step = volume.step * VOLUME_Y / sizeof(*pos);
+
+ for (int z = 0; z < VOLUME_Z; ++z, pos += elem_step)
+ {
+ float3 v_g = getVoxelGCoo (x, y, z); //3 // p
+
+ //tranform to curr cam coo space
+ float3 v = Rcurr_inv * (v_g - tcurr); //4
+
+ int2 coo; //project to current cam
+ coo.x = __float2int_rn (v.x * intr.fx / v.z + intr.cx);
+ coo.y = __float2int_rn (v.y * intr.fy / v.z + intr.cy);
+
+ if (v.z > 0 && coo.x >= 0 && coo.y >= 0 && coo.x < depth_raw.cols && coo.y < depth_raw.rows) //6
+ {
+ int Dp = depth_raw.ptr (coo.y)[coo.x];
+
+ if (Dp != 0)
+ {
+ float xl = (coo.x - intr.cx) / intr.fx;
+ float yl = (coo.y - intr.cy) / intr.fy;
+ float lambda_inv = rsqrtf (xl * xl + yl * yl + 1);
+
+ float sdf = 1000 * norm (tcurr - v_g) * lambda_inv - Dp; //mm
+
+ sdf *= (-1);
+
+ if (sdf >= -tranc_dist_mm)
+ {
+ float tsdf = fmin (1.f, sdf / tranc_dist_mm);
+
+ int weight_prev;
+ float tsdf_prev;
+
+ //read and unpack
+ unpack_tsdf (*pos, tsdf_prev, weight_prev);
+
+ const int Wrk = 1;
+
+ float tsdf_new = (tsdf_prev * weight_prev + Wrk * tsdf) / (weight_prev + Wrk);
+ int weight_new = min (weight_prev + Wrk, MAX_WEIGHT);
+
+ pack_tsdf (tsdf_new, weight_new, *pos);
+ }
+ }
+ }
+ }
+ }
+ };
+
+ __global__ void
+ integrateTsdfKernel (const Tsdf tsdf) {
+ tsdf ();
+ }
+
+ __global__ void
+ tsdf2 (PtrStep<short2> volume, const float tranc_dist_mm, const Mat33 Rcurr_inv, float3 tcurr,
+ const Intr intr, const PtrStepSz<ushort> depth_raw, const float3 cell_size)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x >= VOLUME_X || y >= VOLUME_Y)
+ return;
+
+ short2 *pos = volume.ptr (y) + x;
+ int elem_step = volume.step * VOLUME_Y / sizeof(short2);
+
+ float v_g_x = (x + 0.5f) * cell_size.x - tcurr.x;
+ float v_g_y = (y + 0.5f) * cell_size.y - tcurr.y;
+ float v_g_z = (0 + 0.5f) * cell_size.z - tcurr.z;
+
+ float v_x = Rcurr_inv.data[0].x * v_g_x + Rcurr_inv.data[0].y * v_g_y + Rcurr_inv.data[0].z * v_g_z;
+ float v_y = Rcurr_inv.data[1].x * v_g_x + Rcurr_inv.data[1].y * v_g_y + Rcurr_inv.data[1].z * v_g_z;
+ float v_z = Rcurr_inv.data[2].x * v_g_x + Rcurr_inv.data[2].y * v_g_y + Rcurr_inv.data[2].z * v_g_z;
+
+ //#pragma unroll
+ for (int z = 0; z < VOLUME_Z; ++z)
+ {
+ float3 vr;
+ vr.x = v_g_x;
+ vr.y = v_g_y;
+ vr.z = (v_g_z + z * cell_size.z);
+
+ float3 v;
+ v.x = v_x + Rcurr_inv.data[0].z * z * cell_size.z;
+ v.y = v_y + Rcurr_inv.data[1].z * z * cell_size.z;
+ v.z = v_z + Rcurr_inv.data[2].z * z * cell_size.z;
+
+ int2 coo; //project to current cam
+ coo.x = __float2int_rn (v.x * intr.fx / v.z + intr.cx);
+ coo.y = __float2int_rn (v.y * intr.fy / v.z + intr.cy);
+
+
+ if (v.z > 0 && coo.x >= 0 && coo.y >= 0 && coo.x < depth_raw.cols && coo.y < depth_raw.rows) //6
+ {
+ int Dp = depth_raw.ptr (coo.y)[coo.x]; //mm
+
+ if (Dp != 0)
+ {
+ float xl = (coo.x - intr.cx) / intr.fx;
+ float yl = (coo.y - intr.cy) / intr.fy;
+ float lambda_inv = rsqrtf (xl * xl + yl * yl + 1);
+
+ float sdf = Dp - norm (vr) * lambda_inv * 1000; //mm
+
+
+ if (sdf >= -tranc_dist_mm)
+ {
+ float tsdf = fmin (1.f, sdf / tranc_dist_mm);
+
+ int weight_prev;
+ float tsdf_prev;
+
+ //read and unpack
+ unpack_tsdf (*pos, tsdf_prev, weight_prev);
+
+ const int Wrk = 1;
+
+ float tsdf_new = (tsdf_prev * weight_prev + Wrk * tsdf) / (weight_prev + Wrk);
+ int weight_new = min (weight_prev + Wrk, Tsdf::MAX_WEIGHT);
+
+ pack_tsdf (tsdf_new, weight_new, *pos);
+ }
+ }
+ }
+ pos += elem_step;
+ } /* for(int z = 0; z < VOLUME_Z; ++z) */
+ } /* __global__ */
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ integrateTsdfVolume (const PtrStepSz<ushort>& depth_raw, const Intr& intr, const float3& volume_size,
+ const Mat33& Rcurr_inv, const float3& tcurr, float tranc_dist,
+ PtrStep<short2> volume)
+ {
+ Tsdf tsdf;
+
+ tsdf.volume = volume;
+ tsdf.cell_size.x = volume_size.x / VOLUME_X;
+ tsdf.cell_size.y = volume_size.y / VOLUME_Y;
+ tsdf.cell_size.z = volume_size.z / VOLUME_Z;
+
+ tsdf.intr = intr;
+
+ tsdf.Rcurr_inv = Rcurr_inv;
+ tsdf.tcurr = tcurr;
+ tsdf.depth_raw = depth_raw;
+
+ tsdf.tranc_dist_mm = tranc_dist*1000; //mm
+
+ dim3 block (Tsdf::CTA_SIZE_X, Tsdf::CTA_SIZE_Y);
+ dim3 grid (divUp (VOLUME_X, block.x), divUp (VOLUME_Y, block.y));
+
+ #if 0
+ //tsdf2<<<grid, block>>>(volume, tranc_dist, Rcurr_inv, tcurr, intr, depth_raw, tsdf.cell_size);
+ integrateTsdfKernel<<<grid, block>>>(tsdf);
+ #endif
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+ }
+ }
+}
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ __global__ void
+ scaleDepth (const PtrStepSz<ushort> depth, PtrStep<float> scaled, const Intr intr)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x >= depth.cols || y >= depth.rows)
+ return;
+
+ int Dp = depth.ptr (y)[x];
+
+ float xl = (x - intr.cx) / intr.fx;
+ float yl = (y - intr.cy) / intr.fy;
+ float lambda = sqrtf (xl * xl + yl * yl + 1);
+
+ scaled.ptr (y)[x] = Dp * lambda/1000.f; //meters
+ }
+
+ __global__ void
+ tsdf23 (const PtrStepSz<float> depthScaled, PtrStep<short2> volume,
+ const float tranc_dist, const Mat33 Rcurr_inv, const float3 tcurr, const Intr intr, const float3 cell_size, const pcl::gpu::kinfuLS::tsdf_buffer buffer)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x >= buffer.voxels_size.x || y >= buffer.voxels_size.y)
+ return;
+
+ float v_g_x = (x + 0.5f) * cell_size.x - tcurr.x;
+ float v_g_y = (y + 0.5f) * cell_size.y - tcurr.y;
+ float v_g_z = (0 + 0.5f) * cell_size.z - tcurr.z;
+
+ float v_g_part_norm = v_g_x * v_g_x + v_g_y * v_g_y;
+
+ float v_x = (Rcurr_inv.data[0].x * v_g_x + Rcurr_inv.data[0].y * v_g_y + Rcurr_inv.data[0].z * v_g_z) * intr.fx;
+ float v_y = (Rcurr_inv.data[1].x * v_g_x + Rcurr_inv.data[1].y * v_g_y + Rcurr_inv.data[1].z * v_g_z) * intr.fy;
+ float v_z = (Rcurr_inv.data[2].x * v_g_x + Rcurr_inv.data[2].y * v_g_y + Rcurr_inv.data[2].z * v_g_z);
+
+ float z_scaled = 0;
+
+ float Rcurr_inv_0_z_scaled = Rcurr_inv.data[0].z * cell_size.z * intr.fx;
+ float Rcurr_inv_1_z_scaled = Rcurr_inv.data[1].z * cell_size.z * intr.fy;
+
+ float tranc_dist_inv = 1.0f / tranc_dist;
+
+ short2* pos = volume.ptr (y) + x;
+
+ // shift the pointer to relative indices
+ shift_tsdf_pointer(&pos, buffer);
+
+ int elem_step = volume.step * buffer.voxels_size.y / sizeof(short2);
+
+ //#pragma unroll
+ for (int z = 0; z < buffer.voxels_size.z;
+ ++z,
+ v_g_z += cell_size.z,
+ z_scaled += cell_size.z,
+ v_x += Rcurr_inv_0_z_scaled,
+ v_y += Rcurr_inv_1_z_scaled,
+ pos += elem_step)
+ {
+
+ // As the pointer is incremented in the for loop, we have to make sure that the pointer is never outside the memory
+ if(pos > buffer.tsdf_memory_end)
+ pos -= (buffer.tsdf_memory_end - buffer.tsdf_memory_start + 1);
+
+ float inv_z = 1.0f / (v_z + Rcurr_inv.data[2].z * z_scaled);
+ if (inv_z < 0)
+ continue;
+
+ // project to current cam
+ int2 coo =
+ {
+ __float2int_rn (v_x * inv_z + intr.cx),
+ __float2int_rn (v_y * inv_z + intr.cy)
+ };
+
+ if (coo.x >= 0 && coo.y >= 0 && coo.x < depthScaled.cols && coo.y < depthScaled.rows) //6
+ {
+ float Dp_scaled = depthScaled.ptr (coo.y)[coo.x]; //meters
+
+ float sdf = Dp_scaled - sqrtf (v_g_z * v_g_z + v_g_part_norm);
+
+ if (Dp_scaled != 0 && sdf >= -tranc_dist) //meters
+ {
+ float tsdf = fmin (1.0f, sdf * tranc_dist_inv);
+
+ //read and unpack
+ float tsdf_prev;
+ int weight_prev;
+ unpack_tsdf (*pos, tsdf_prev, weight_prev);
+
+ const int Wrk = 1;
+
+ float tsdf_new = (tsdf_prev * weight_prev + Wrk * tsdf) / (weight_prev + Wrk);
+ int weight_new = min (weight_prev + Wrk, Tsdf::MAX_WEIGHT);
+
+ pack_tsdf (tsdf_new, weight_new, *pos);
+ }
+ }
+ } // for(int z = 0; z < VOLUME_Z; ++z)
+ } // __global__
+
+ __global__ void
+ tsdf23normal_hack (const PtrStepSz<float> depthScaled, PtrStep<short2> volume,
+ const float tranc_dist, const Mat33 Rcurr_inv, const float3 tcurr, const Intr intr, const float3 cell_size)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x >= VOLUME_X || y >= VOLUME_Y)
+ return;
+
+ const float v_g_x = (x + 0.5f) * cell_size.x - tcurr.x;
+ const float v_g_y = (y + 0.5f) * cell_size.y - tcurr.y;
+ float v_g_z = (0 + 0.5f) * cell_size.z - tcurr.z;
+
+ float v_g_part_norm = v_g_x * v_g_x + v_g_y * v_g_y;
+
+ float v_x = (Rcurr_inv.data[0].x * v_g_x + Rcurr_inv.data[0].y * v_g_y + Rcurr_inv.data[0].z * v_g_z) * intr.fx;
+ float v_y = (Rcurr_inv.data[1].x * v_g_x + Rcurr_inv.data[1].y * v_g_y + Rcurr_inv.data[1].z * v_g_z) * intr.fy;
+ float v_z = (Rcurr_inv.data[2].x * v_g_x + Rcurr_inv.data[2].y * v_g_y + Rcurr_inv.data[2].z * v_g_z);
+
+ float z_scaled = 0;
+
+ float Rcurr_inv_0_z_scaled = Rcurr_inv.data[0].z * cell_size.z * intr.fx;
+ float Rcurr_inv_1_z_scaled = Rcurr_inv.data[1].z * cell_size.z * intr.fy;
+
+ float tranc_dist_inv = 1.0f / tranc_dist;
+
+ short2* pos = volume.ptr (y) + x;
+ int elem_step = volume.step * VOLUME_Y / sizeof(short2);
+
+ //#pragma unroll
+ for (int z = 0; z < VOLUME_Z;
+ ++z,
+ v_g_z += cell_size.z,
+ z_scaled += cell_size.z,
+ v_x += Rcurr_inv_0_z_scaled,
+ v_y += Rcurr_inv_1_z_scaled,
+ pos += elem_step)
+ {
+ float inv_z = 1.0f / (v_z + Rcurr_inv.data[2].z * z_scaled);
+ if (inv_z < 0)
+ continue;
+
+ // project to current cam
+ int2 coo =
+ {
+ __float2int_rn (v_x * inv_z + intr.cx),
+ __float2int_rn (v_y * inv_z + intr.cy)
+ };
+
+ if (coo.x >= 0 && coo.y >= 0 && coo.x < depthScaled.cols && coo.y < depthScaled.rows) //6
+ {
+ float Dp_scaled = depthScaled.ptr (coo.y)[coo.x]; //meters
+
+ float sdf = Dp_scaled - sqrtf (v_g_z * v_g_z + v_g_part_norm);
+
+ if (Dp_scaled != 0 && sdf >= -tranc_dist) //meters
+ {
+ float tsdf = fmin (1.0f, sdf * tranc_dist_inv);
+
+ bool integrate = true;
+ if ((x > 0 && x < VOLUME_X-2) && (y > 0 && y < VOLUME_Y-2) && (z > 0 && z < VOLUME_Z-2))
+ {
+ const float qnan = numeric_limits<float>::quiet_NaN();
+ float3 normal = make_float3(qnan, qnan, qnan);
+
+ float Fn, Fp;
+ int Wn = 0, Wp = 0;
+ unpack_tsdf (*(pos + elem_step), Fn, Wn);
+ unpack_tsdf (*(pos - elem_step), Fp, Wp);
+
+ if (Wn > 16 && Wp > 16)
+ normal.z = (Fn - Fp)/cell_size.z;
+
+ unpack_tsdf (*(pos + volume.step/sizeof(short2) ), Fn, Wn);
+ unpack_tsdf (*(pos - volume.step/sizeof(short2) ), Fp, Wp);
+
+ if (Wn > 16 && Wp > 16)
+ normal.y = (Fn - Fp)/cell_size.y;
+
+ unpack_tsdf (*(pos + 1), Fn, Wn);
+ unpack_tsdf (*(pos - 1), Fp, Wp);
+
+ if (Wn > 16 && Wp > 16)
+ normal.x = (Fn - Fp)/cell_size.x;
+
+ if (normal.x != qnan && normal.y != qnan && normal.z != qnan)
+ {
+ float norm2 = dot(normal, normal);
+ if (norm2 >= 1e-10)
+ {
+ normal *= rsqrt(norm2);
+
+ float nt = v_g_x * normal.x + v_g_y * normal.y + v_g_z * normal.z;
+ float cosine = nt * rsqrt(v_g_x * v_g_x + v_g_y * v_g_y + v_g_z * v_g_z);
+
+ if (cosine < 0.5)
+ integrate = false;
+ }
+ }
+ }
+
+ if (integrate)
+ {
+ //read and unpack
+ float tsdf_prev;
+ int weight_prev;
+ unpack_tsdf (*pos, tsdf_prev, weight_prev);
+
+ const int Wrk = 1;
+
+ float tsdf_new = (tsdf_prev * weight_prev + Wrk * tsdf) / (weight_prev + Wrk);
+ int weight_new = min (weight_prev + Wrk, Tsdf::MAX_WEIGHT);
+
+ pack_tsdf (tsdf_new, weight_new, *pos);
+ }
+ }
+ }
+ } // for(int z = 0; z < VOLUME_Z; ++z)
+ } // __global__
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ integrateTsdfVolume (const PtrStepSz<ushort>& depth, const Intr& intr,
+ const float3& volume_size, const Mat33& Rcurr_inv, const float3& tcurr,
+ float tranc_dist,
+ PtrStep<short2> volume, const pcl::gpu::kinfuLS::tsdf_buffer* buffer, DeviceArray2D<float>& depthScaled)
+ {
+ depthScaled.create (depth.rows, depth.cols);
+
+ dim3 block_scale (32, 8);
+ dim3 grid_scale (divUp (depth.cols, block_scale.x), divUp (depth.rows, block_scale.y));
+
+ //scales depth along ray and converts mm -> meters.
+ scaleDepth<<<grid_scale, block_scale>>>(depth, depthScaled, intr);
+ cudaSafeCall ( cudaGetLastError () );
+
+ float3 cell_size;
+ cell_size.x = volume_size.x / buffer->voxels_size.x;
+ cell_size.y = volume_size.y / buffer->voxels_size.y;
+ cell_size.z = volume_size.z / buffer->voxels_size.z;
+
+ //dim3 block(Tsdf::CTA_SIZE_X, Tsdf::CTA_SIZE_Y);
+ dim3 block (16, 16);
+ dim3 grid (divUp (buffer->voxels_size.x, block.x), divUp (buffer->voxels_size.y, block.y));
+
+ tsdf23<<<grid, block>>>(depthScaled, volume, tranc_dist, Rcurr_inv, tcurr, intr, cell_size, *buffer);
+ //tsdf23normal_hack<<<grid, block>>>(depthScaled, volume, tranc_dist, Rcurr_inv, tcurr, intr, cell_size);
+
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ clearTSDFSlice (PtrStep<short2> volume, pcl::gpu::kinfuLS::tsdf_buffer* buffer, int shiftX, int shiftY, int shiftZ)
+ {
+ int newX = buffer->origin_GRID.x + shiftX;
+ int newY = buffer->origin_GRID.y + shiftY;
+
+ int3 minBounds, maxBounds;
+
+ //X
+ if(newX >= 0)
+ {
+ minBounds.x = buffer->origin_GRID.x;
+ maxBounds.x = newX;
+ }
+ else
+ {
+ minBounds.x = newX + buffer->voxels_size.x;
+ maxBounds.x = buffer->origin_GRID.x + buffer->voxels_size.x;
+ }
+
+ if(minBounds.x > maxBounds.x)
+ std::swap(minBounds.x, maxBounds.x);
+
+
+ //Y
+ if(newY >= 0)
+ {
+ minBounds.y = buffer->origin_GRID.y;
+ maxBounds.y = newY;
+ }
+ else
+ {
+ minBounds.y = newY + buffer->voxels_size.y;
+ maxBounds.y = buffer->origin_GRID.y + buffer->voxels_size.y;
+ }
+
+ if(minBounds.y > maxBounds.y)
+ std::swap(minBounds.y, maxBounds.y);
+
+ //Z
+ minBounds.z = buffer->origin_GRID.z;
+ maxBounds.z = shiftZ;
+
+ // call kernel
+ dim3 block (32, 16);
+ dim3 grid (1, 1, 1);
+ grid.x = divUp (buffer->voxels_size.x, block.x);
+ grid.y = divUp (buffer->voxels_size.y, block.y);
+
+ clearSliceKernel<<<grid, block>>>(volume, *buffer, minBounds, maxBounds);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+ }
+ }
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+
+#ifndef PCL_GPU_KINFU_CUDA_UTILS_HPP_
+#define PCL_GPU_KINFU_CUDA_UTILS_HPP_
+//#include <boost/graph/buffer_concepts.hpp>
+
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ template <class T>
+ __device__ __host__ __forceinline__ void swap ( T& a, T& b )
+ {
+ T c(a); a=b; b=c;
+ }
+
+ template<typename T> struct numeric_limits;
+
+ template<> struct numeric_limits<float>
+ {
+ __device__ __forceinline__ static float
+ quiet_NaN() { return __int_as_float(0x7fffffff); /*CUDART_NAN_F*/ };
+ __device__ __forceinline__ static float
+ epsilon() { return 1.192092896e-07f/*FLT_EPSILON*/; };
+
+ __device__ __forceinline__ static float
+ min() { return 1.175494351e-38f/*FLT_MIN*/; };
+ __device__ __forceinline__ static float
+ max() { return 3.402823466e+38f/*FLT_MAX*/; };
+ };
+
+ template<> struct numeric_limits<short>
+ {
+ __device__ __forceinline__ static short
+ max() { return SHRT_MAX; };
+ };
+
+ __device__ __forceinline__ float
+ dot(const float3& v1, const float3& v2)
+ {
+ return v1.x * v2.x + v1.y*v2.y + v1.z*v2.z;
+ }
+
+ __device__ __forceinline__ float3&
+ operator+=(float3& vec, const float& v)
+ {
+ vec.x += v; vec.y += v; vec.z += v; return vec;
+ }
+
+ __device__ __forceinline__ float3
+ operator+(const float3& v1, const float3& v2)
+ {
+ return make_float3(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
+ }
+
+ __device__ __forceinline__ float3&
+ operator*=(float3& vec, const float& v)
+ {
+ vec.x *= v; vec.y *= v; vec.z *= v; return vec;
+ }
+
+ __device__ __forceinline__ float3
+ operator-(const float3& v1, const float3& v2)
+ {
+ return make_float3(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
+ }
+
+ __device__ __forceinline__ float3
+ operator*(const float3& v1, const float& v)
+ {
+ return make_float3(v1.x * v, v1.y * v, v1.z * v);
+ }
+
+ __device__ __forceinline__ float
+ norm(const float3& v)
+ {
+ return sqrt(dot(v, v));
+ }
+
+ __device__ __forceinline__ float3
+ normalized(const float3& v)
+ {
+ return v * rsqrt(dot(v, v));
+ }
+
+ __device__ __host__ __forceinline__ float3
+ cross(const float3& v1, const float3& v2)
+ {
+ return make_float3(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x);
+ }
+
+ __device__ __forceinline__ void computeRoots2(const float& b, const float& c, float3& roots)
+ {
+ roots.x = 0.f;
+ float d = b * b - 4.f * c;
+ if (d < 0.f) // no real roots!!!! THIS SHOULD NOT HAPPEN!
+ d = 0.f;
+
+ float sd = sqrtf(d);
+
+ roots.z = 0.5f * (b + sd);
+ roots.y = 0.5f * (b - sd);
+ }
+
+ __device__ __forceinline__ void
+ computeRoots3(float c0, float c1, float c2, float3& roots)
+ {
+ if ( fabsf(c0) < numeric_limits<float>::epsilon())// one root is 0 -> quadratic equation
+ {
+ computeRoots2 (c2, c1, roots);
+ }
+ else
+ {
+ const float s_inv3 = 1.f/3.f;
+ const float s_sqrt3 = sqrtf(3.f);
+ // Construct the parameters used in classifying the roots of the equation
+ // and in solving the equation for the roots in closed form.
+ float c2_over_3 = c2 * s_inv3;
+ float a_over_3 = (c1 - c2*c2_over_3)*s_inv3;
+ if (a_over_3 > 0.f)
+ a_over_3 = 0.f;
+
+ float half_b = 0.5f * (c0 + c2_over_3 * (2.f * c2_over_3 * c2_over_3 - c1));
+
+ float q = half_b * half_b + a_over_3 * a_over_3 * a_over_3;
+ if (q > 0.f)
+ q = 0.f;
+
+ // Compute the eigenvalues by solving for the roots of the polynomial.
+ float rho = sqrtf(-a_over_3);
+ float theta = atan2f (sqrtf (-q), half_b)*s_inv3;
+ float cos_theta = __cosf (theta);
+ float sin_theta = __sinf (theta);
+ roots.x = c2_over_3 + 2.f * rho * cos_theta;
+ roots.y = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
+ roots.z = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
+
+ // Sort in increasing order.
+ if (roots.x >= roots.y)
+ swap(roots.x, roots.y);
+
+ if (roots.y >= roots.z)
+ {
+ swap(roots.y, roots.z);
+
+ if (roots.x >= roots.y)
+ swap (roots.x, roots.y);
+ }
+ if (roots.x <= 0) // eigenval for symetric positive semi-definite matrix can not be negative! Set it to 0
+ computeRoots2 (c2, c1, roots);
+ }
+ }
+
+ struct Eigen33
+ {
+ public:
+ template<int Rows>
+ struct MiniMat
+ {
+ float3 data[Rows];
+ __device__ __host__ __forceinline__ float3& operator[](int i) { return data[i]; }
+ __device__ __host__ __forceinline__ const float3& operator[](int i) const { return data[i]; }
+ };
+ typedef MiniMat<3> Mat33;
+ typedef MiniMat<4> Mat43;
+
+
+ static __forceinline__ __device__ float3
+ unitOrthogonal (const float3& src)
+ {
+ float3 perp;
+ /* Let us compute the crossed product of *this with a vector
+ * that is not too close to being colinear to *this.
+ */
+
+ /* unless the x and y coords are both close to zero, we can
+ * simply take ( -y, x, 0 ) and normalize it.
+ */
+ if(!isMuchSmallerThan(src.x, src.z) || !isMuchSmallerThan(src.y, src.z))
+ {
+ float invnm = rsqrtf(src.x*src.x + src.y*src.y);
+ perp.x = -src.y * invnm;
+ perp.y = src.x * invnm;
+ perp.z = 0.0f;
+ }
+ /* if both x and y are close to zero, then the vector is close
+ * to the z-axis, so it's far from colinear to the x-axis for instance.
+ * So we take the crossed product with (1,0,0) and normalize it.
+ */
+ else
+ {
+ float invnm = rsqrtf(src.z * src.z + src.y * src.y);
+ perp.x = 0.0f;
+ perp.y = -src.z * invnm;
+ perp.z = src.y * invnm;
+ }
+
+ return perp;
+ }
+
+ __device__ __forceinline__
+ Eigen33(volatile float* mat_pkg_arg) : mat_pkg(mat_pkg_arg) {}
+ __device__ __forceinline__ void
+ compute(Mat33& tmp, Mat33& vec_tmp, Mat33& evecs, float3& evals)
+ {
+ // Scale the matrix so its entries are in [-1,1]. The scaling is applied
+ // only when at least one matrix entry has magnitude larger than 1.
+
+ float max01 = fmaxf( fabsf(mat_pkg[0]), fabsf(mat_pkg[1]) );
+ float max23 = fmaxf( fabsf(mat_pkg[2]), fabsf(mat_pkg[3]) );
+ float max45 = fmaxf( fabsf(mat_pkg[4]), fabsf(mat_pkg[5]) );
+ float m0123 = fmaxf( max01, max23);
+ float scale = fmaxf( max45, m0123);
+
+ if (scale <= numeric_limits<float>::min())
+ scale = 1.f;
+
+ mat_pkg[0] /= scale;
+ mat_pkg[1] /= scale;
+ mat_pkg[2] /= scale;
+ mat_pkg[3] /= scale;
+ mat_pkg[4] /= scale;
+ mat_pkg[5] /= scale;
+
+ // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The
+ // eigenvalues are the roots to this equation, all guaranteed to be
+ // real-valued, because the matrix is symmetric.
+ float c0 = m00() * m11() * m22()
+ + 2.f * m01() * m02() * m12()
+ - m00() * m12() * m12()
+ - m11() * m02() * m02()
+ - m22() * m01() * m01();
+ float c1 = m00() * m11() -
+ m01() * m01() +
+ m00() * m22() -
+ m02() * m02() +
+ m11() * m22() -
+ m12() * m12();
+ float c2 = m00() + m11() + m22();
+
+ computeRoots3(c0, c1, c2, evals);
+
+ if(evals.z - evals.x <= numeric_limits<float>::epsilon())
+ {
+ evecs[0] = make_float3(1.f, 0.f, 0.f);
+ evecs[1] = make_float3(0.f, 1.f, 0.f);
+ evecs[2] = make_float3(0.f, 0.f, 1.f);
+ }
+ else if (evals.y - evals.x <= numeric_limits<float>::epsilon() )
+ {
+ // first and second equal
+ tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
+ tmp[0].x -= evals.z; tmp[1].y -= evals.z; tmp[2].z -= evals.z;
+
+ vec_tmp[0] = cross(tmp[0], tmp[1]);
+ vec_tmp[1] = cross(tmp[0], tmp[2]);
+ vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+ float len1 = dot (vec_tmp[0], vec_tmp[0]);
+ float len2 = dot (vec_tmp[1], vec_tmp[1]);
+ float len3 = dot (vec_tmp[2], vec_tmp[2]);
+
+ if (len1 >= len2 && len1 >= len3)
+ {
+ evecs[2] = vec_tmp[0] * rsqrtf (len1);
+ }
+ else if (len2 >= len1 && len2 >= len3)
+ {
+ evecs[2] = vec_tmp[1] * rsqrtf (len2);
+ }
+ else
+ {
+ evecs[2] = vec_tmp[2] * rsqrtf (len3);
+ }
+
+ evecs[1] = unitOrthogonal(evecs[2]);
+ evecs[0] = cross(evecs[1], evecs[2]);
+ }
+ else if (evals.z - evals.y <= numeric_limits<float>::epsilon() )
+ {
+ // second and third equal
+ tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
+ tmp[0].x -= evals.x; tmp[1].y -= evals.x; tmp[2].z -= evals.x;
+
+ vec_tmp[0] = cross(tmp[0], tmp[1]);
+ vec_tmp[1] = cross(tmp[0], tmp[2]);
+ vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+ float len1 = dot(vec_tmp[0], vec_tmp[0]);
+ float len2 = dot(vec_tmp[1], vec_tmp[1]);
+ float len3 = dot(vec_tmp[2], vec_tmp[2]);
+
+ if (len1 >= len2 && len1 >= len3)
+ {
+ evecs[0] = vec_tmp[0] * rsqrtf(len1);
+ }
+ else if (len2 >= len1 && len2 >= len3)
+ {
+ evecs[0] = vec_tmp[1] * rsqrtf(len2);
+ }
+ else
+ {
+ evecs[0] = vec_tmp[2] * rsqrtf(len3);
+ }
+
+ evecs[1] = unitOrthogonal( evecs[0] );
+ evecs[2] = cross(evecs[0], evecs[1]);
+ }
+ else
+ {
+
+ tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
+ tmp[0].x -= evals.z; tmp[1].y -= evals.z; tmp[2].z -= evals.z;
+
+ vec_tmp[0] = cross(tmp[0], tmp[1]);
+ vec_tmp[1] = cross(tmp[0], tmp[2]);
+ vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+ float len1 = dot(vec_tmp[0], vec_tmp[0]);
+ float len2 = dot(vec_tmp[1], vec_tmp[1]);
+ float len3 = dot(vec_tmp[2], vec_tmp[2]);
+
+ float mmax[3];
+
+ unsigned int min_el = 2;
+ unsigned int max_el = 2;
+ if (len1 >= len2 && len1 >= len3)
+ {
+ mmax[2] = len1;
+ evecs[2] = vec_tmp[0] * rsqrtf (len1);
+ }
+ else if (len2 >= len1 && len2 >= len3)
+ {
+ mmax[2] = len2;
+ evecs[2] = vec_tmp[1] * rsqrtf (len2);
+ }
+ else
+ {
+ mmax[2] = len3;
+ evecs[2] = vec_tmp[2] * rsqrtf (len3);
+ }
+
+ tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
+ tmp[0].x -= evals.y; tmp[1].y -= evals.y; tmp[2].z -= evals.y;
+
+ vec_tmp[0] = cross(tmp[0], tmp[1]);
+ vec_tmp[1] = cross(tmp[0], tmp[2]);
+ vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+ len1 = dot(vec_tmp[0], vec_tmp[0]);
+ len2 = dot(vec_tmp[1], vec_tmp[1]);
+ len3 = dot(vec_tmp[2], vec_tmp[2]);
+
+ if (len1 >= len2 && len1 >= len3)
+ {
+ mmax[1] = len1;
+ evecs[1] = vec_tmp[0] * rsqrtf (len1);
+ min_el = len1 <= mmax[min_el] ? 1 : min_el;
+ max_el = len1 > mmax[max_el] ? 1 : max_el;
+ }
+ else if (len2 >= len1 && len2 >= len3)
+ {
+ mmax[1] = len2;
+ evecs[1] = vec_tmp[1] * rsqrtf (len2);
+ min_el = len2 <= mmax[min_el] ? 1 : min_el;
+ max_el = len2 > mmax[max_el] ? 1 : max_el;
+ }
+ else
+ {
+ mmax[1] = len3;
+ evecs[1] = vec_tmp[2] * rsqrtf (len3);
+ min_el = len3 <= mmax[min_el] ? 1 : min_el;
+ max_el = len3 > mmax[max_el] ? 1 : max_el;
+ }
+
+ tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
+ tmp[0].x -= evals.x; tmp[1].y -= evals.x; tmp[2].z -= evals.x;
+
+ vec_tmp[0] = cross(tmp[0], tmp[1]);
+ vec_tmp[1] = cross(tmp[0], tmp[2]);
+ vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+ len1 = dot (vec_tmp[0], vec_tmp[0]);
+ len2 = dot (vec_tmp[1], vec_tmp[1]);
+ len3 = dot (vec_tmp[2], vec_tmp[2]);
+
+
+ if (len1 >= len2 && len1 >= len3)
+ {
+ mmax[0] = len1;
+ evecs[0] = vec_tmp[0] * rsqrtf (len1);
+ min_el = len3 <= mmax[min_el] ? 0 : min_el;
+ max_el = len3 > mmax[max_el] ? 0 : max_el;
+ }
+ else if (len2 >= len1 && len2 >= len3)
+ {
+ mmax[0] = len2;
+ evecs[0] = vec_tmp[1] * rsqrtf (len2);
+ min_el = len3 <= mmax[min_el] ? 0 : min_el;
+ max_el = len3 > mmax[max_el] ? 0 : max_el;
+ }
+ else
+ {
+ mmax[0] = len3;
+ evecs[0] = vec_tmp[2] * rsqrtf (len3);
+ min_el = len3 <= mmax[min_el] ? 0 : min_el;
+ max_el = len3 > mmax[max_el] ? 0 : max_el;
+ }
+
+ unsigned mid_el = 3 - min_el - max_el;
+ evecs[min_el] = normalized( cross( evecs[(min_el+1) % 3], evecs[(min_el+2) % 3] ) );
+ evecs[mid_el] = normalized( cross( evecs[(mid_el+1) % 3], evecs[(mid_el+2) % 3] ) );
+ }
+ // Rescale back to the original size.
+ evals *= scale;
+ }
+ private:
+ volatile float* mat_pkg;
+
+ __device__ __forceinline__ float m00() const { return mat_pkg[0]; }
+ __device__ __forceinline__ float m01() const { return mat_pkg[1]; }
+ __device__ __forceinline__ float m02() const { return mat_pkg[2]; }
+ __device__ __forceinline__ float m10() const { return mat_pkg[1]; }
+ __device__ __forceinline__ float m11() const { return mat_pkg[3]; }
+ __device__ __forceinline__ float m12() const { return mat_pkg[4]; }
+ __device__ __forceinline__ float m20() const { return mat_pkg[2]; }
+ __device__ __forceinline__ float m21() const { return mat_pkg[4]; }
+ __device__ __forceinline__ float m22() const { return mat_pkg[5]; }
+
+ __device__ __forceinline__ float3 row0() const { return make_float3( m00(), m01(), m02() ); }
+ __device__ __forceinline__ float3 row1() const { return make_float3( m10(), m11(), m12() ); }
+ __device__ __forceinline__ float3 row2() const { return make_float3( m20(), m21(), m22() ); }
+
+ __device__ __forceinline__ static bool isMuchSmallerThan (float x, float y)
+ {
+ // copied from <eigen>/include/Eigen/src/Core/NumTraits.h
+ const float prec_sqr = numeric_limits<float>::epsilon() * numeric_limits<float>::epsilon();
+ return x * x <= prec_sqr * y * y;
+ }
+ };
+
+ struct Block
+ {
+ static __device__ __forceinline__ unsigned int stride()
+ {
+ return blockDim.x * blockDim.y * blockDim.z;
+ }
+
+ static __device__ __forceinline__ int
+ flattenedThreadId()
+ {
+ return threadIdx.z * blockDim.x * blockDim.y + threadIdx.y * blockDim.x + threadIdx.x;
+ }
+
+ template<int CTA_SIZE, typename T, class BinOp>
+ static __device__ __forceinline__ void reduce(volatile T* buffer, BinOp op)
+ {
+ int tid = flattenedThreadId();
+ T val = buffer[tid];
+
+ if (CTA_SIZE >= 1024) { if (tid < 512) buffer[tid] = val = op(val, buffer[tid + 512]); __syncthreads(); }
+ if (CTA_SIZE >= 512) { if (tid < 256) buffer[tid] = val = op(val, buffer[tid + 256]); __syncthreads(); }
+ if (CTA_SIZE >= 256) { if (tid < 128) buffer[tid] = val = op(val, buffer[tid + 128]); __syncthreads(); }
+ if (CTA_SIZE >= 128) { if (tid < 64) buffer[tid] = val = op(val, buffer[tid + 64]); __syncthreads(); }
+
+ if (tid < 32)
+ {
+ if (CTA_SIZE >= 64) { buffer[tid] = val = op(val, buffer[tid + 32]); }
+ if (CTA_SIZE >= 32) { buffer[tid] = val = op(val, buffer[tid + 16]); }
+ if (CTA_SIZE >= 16) { buffer[tid] = val = op(val, buffer[tid + 8]); }
+ if (CTA_SIZE >= 8) { buffer[tid] = val = op(val, buffer[tid + 4]); }
+ if (CTA_SIZE >= 4) { buffer[tid] = val = op(val, buffer[tid + 2]); }
+ if (CTA_SIZE >= 2) { buffer[tid] = val = op(val, buffer[tid + 1]); }
+ }
+ }
+
+ template<int CTA_SIZE, typename T, class BinOp>
+ static __device__ __forceinline__ T reduce(volatile T* buffer, T init, BinOp op)
+ {
+ int tid = flattenedThreadId();
+ T val = buffer[tid] = init;
+ __syncthreads();
+
+ if (CTA_SIZE >= 1024) { if (tid < 512) buffer[tid] = val = op(val, buffer[tid + 512]); __syncthreads(); }
+ if (CTA_SIZE >= 512) { if (tid < 256) buffer[tid] = val = op(val, buffer[tid + 256]); __syncthreads(); }
+ if (CTA_SIZE >= 256) { if (tid < 128) buffer[tid] = val = op(val, buffer[tid + 128]); __syncthreads(); }
+ if (CTA_SIZE >= 128) { if (tid < 64) buffer[tid] = val = op(val, buffer[tid + 64]); __syncthreads(); }
+
+ if (tid < 32)
+ {
+ if (CTA_SIZE >= 64) { buffer[tid] = val = op(val, buffer[tid + 32]); }
+ if (CTA_SIZE >= 32) { buffer[tid] = val = op(val, buffer[tid + 16]); }
+ if (CTA_SIZE >= 16) { buffer[tid] = val = op(val, buffer[tid + 8]); }
+ if (CTA_SIZE >= 8) { buffer[tid] = val = op(val, buffer[tid + 4]); }
+ if (CTA_SIZE >= 4) { buffer[tid] = val = op(val, buffer[tid + 2]); }
+ if (CTA_SIZE >= 2) { buffer[tid] = val = op(val, buffer[tid + 1]); }
+ }
+ __syncthreads();
+ return buffer[0];
+ }
+ };
+
+ struct Warp
+ {
+ enum
+ {
+ LOG_WARP_SIZE = 5,
+ WARP_SIZE = 1 << LOG_WARP_SIZE,
+ STRIDE = WARP_SIZE
+ };
+
+ /** \brief Returns the warp lane ID of the calling thread. */
+ static __device__ __forceinline__ unsigned int
+ laneId()
+ {
+ unsigned int ret;
+ asm("mov.u32 %0, %laneid;" : "=r"(ret) );
+ return ret;
+ }
+
+ static __device__ __forceinline__ unsigned int id()
+ {
+ int tid = threadIdx.z * blockDim.x * blockDim.y + threadIdx.y * blockDim.x + threadIdx.x;
+ return tid >> LOG_WARP_SIZE;
+ }
+
+ static __device__ __forceinline__
+ int laneMaskLt()
+ {
+ #if (__CUDA_ARCH__ >= 200)
+ unsigned int ret;
+ asm("mov.u32 %0, %lanemask_lt;" : "=r"(ret) );
+ return ret;
+ #else
+ return 0xFFFFFFFF >> (32 - laneId());
+ #endif
+ }
+
+ static __device__ __forceinline__ int binaryExclScan(int ballot_mask)
+ {
+ return __popc(Warp::laneMaskLt() & ballot_mask);
+ }
+ };
+
+
+ struct Emulation
+ {
+ static __device__ __forceinline__ int
+ warp_reduce ( volatile int *ptr , const unsigned int tid)
+ {
+ const unsigned int lane = tid & 31; // index of thread in warp (0..31)
+
+ if (lane < 16)
+ {
+ int partial = ptr[tid];
+
+ ptr[tid] = partial = partial + ptr[tid + 16];
+ ptr[tid] = partial = partial + ptr[tid + 8];
+ ptr[tid] = partial = partial + ptr[tid + 4];
+ ptr[tid] = partial = partial + ptr[tid + 2];
+ ptr[tid] = partial = partial + ptr[tid + 1];
+ }
+ return ptr[tid - lane];
+ }
+
+ static __forceinline__ __device__ int
+ Ballot(int predicate, volatile int* cta_buffer)
+ {
+ #if __CUDA_ARCH__ >= 200
+ (void)cta_buffer;
+ return __ballot(predicate);
+ #else
+ int tid = Block::flattenedThreadId();
+ cta_buffer[tid] = predicate ? (1 << (tid & 31)) : 0;
+ return warp_reduce(cta_buffer, tid);
+ #endif
+ }
+
+ static __forceinline__ __device__ bool
+ All(int predicate, volatile int* cta_buffer)
+ {
+ #if __CUDA_ARCH__ >= 200
+ (void)cta_buffer;
+ return __all(predicate);
+ #else
+ int tid = Block::flattenedThreadId();
+ cta_buffer[tid] = predicate ? 1 : 0;
+ return warp_reduce(cta_buffer, tid) == 32;
+ #endif
+ }
+ };
+ }
+ }
+}
+
+#endif /* PCL_GPU_KINFU_CUDA_UTILS_HPP_ */
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+
+#include <pcl/gpu/kinfu_large_scale/cyclical_buffer.h>
+#include <pcl/common/distances.h>
+#include "internal.h"
+
+
+
+bool
+pcl::gpu::kinfuLS::CyclicalBuffer::checkForShift (const TsdfVolume::Ptr volume, const Eigen::Affine3f &cam_pose, const double distance_camera_target, const bool perform_shift, const bool last_shift, const bool force_shift)
+{
+ bool result = false;
+
+ // project the target point in the cube
+ pcl::PointXYZ targetPoint;
+ targetPoint.x = 0.0f;
+ targetPoint.y = 0.0f;
+ targetPoint.z = distance_camera_target; // place the point at camera position + distance_camera_target on Z
+ targetPoint = pcl::transformPoint (targetPoint, cam_pose);
+
+ // check distance from the cube's center
+ pcl::PointXYZ center_cube;
+ center_cube.x = buffer_.origin_metric.x + buffer_.volume_size.x/2.0f;
+ center_cube.y = buffer_.origin_metric.y + buffer_.volume_size.y/2.0f;
+ center_cube.z = buffer_.origin_metric.z + buffer_.volume_size.z/2.0f;
+
+ if (pcl::euclideanDistance (targetPoint, center_cube) > distance_threshold_)
+ result = true;
+
+ if (!perform_shift && !force_shift)
+ return (result);
+
+ // perform shifting operations
+ if (result || force_shift)
+ performShift (volume, targetPoint, last_shift);
+
+ return (result);
+}
+
+
+void
+pcl::gpu::kinfuLS::CyclicalBuffer::performShift (const TsdfVolume::Ptr volume, const pcl::PointXYZ &target_point, const bool last_shift)
+{
+ // compute new origin and offsets
+ int offset_x, offset_y, offset_z;
+ computeAndSetNewCubeMetricOrigin (target_point, offset_x, offset_y, offset_z);
+
+ // extract current slice from the TSDF volume (coordinates are in indices! (see fetchSliceAsCloud() )
+ DeviceArray<PointXYZ> points;
+ DeviceArray<float> intensities;
+ int size;
+ if(!last_shift)
+ {
+ size = volume->fetchSliceAsCloud (cloud_buffer_device_xyz_, cloud_buffer_device_intensities_, &buffer_, offset_x, offset_y, offset_z);
+ }
+ else
+ {
+ size = volume->fetchSliceAsCloud (cloud_buffer_device_xyz_, cloud_buffer_device_intensities_, &buffer_, buffer_.voxels_size.x - 1, buffer_.voxels_size.y - 1, buffer_.voxels_size.z - 1);
+ }
+ points = DeviceArray<PointXYZ> (cloud_buffer_device_xyz_.ptr (), size);
+ intensities = DeviceArray<float> (cloud_buffer_device_intensities_.ptr(), size);
+
+ PointCloud<PointXYZI>::Ptr current_slice (new PointCloud<PointXYZI>);
+ PointCloud<PointXYZ>::Ptr current_slice_xyz (new PointCloud<PointXYZ>);
+ PointCloud<PointIntensity>::Ptr current_slice_intensities (new PointCloud<PointIntensity>);
+
+ // Retrieving XYZ
+ points.download (current_slice_xyz->points);
+ current_slice_xyz->width = (int) current_slice_xyz->points.size ();
+ current_slice_xyz->height = 1;
+
+ // Retrieving intensities
+ // TODO change this mechanism by using PointIntensity directly (in spite of float)
+ // when tried, this lead to wrong intenisty values being extracted by fetchSliceAsCloud () (padding pbls?)
+ std::vector<float , Eigen::aligned_allocator<float> > intensities_vector;
+ intensities.download (intensities_vector);
+ current_slice_intensities->points.resize (current_slice_xyz->points.size ());
+ for(int i = 0 ; i < current_slice_intensities->points.size () ; ++i)
+ current_slice_intensities->points[i].intensity = intensities_vector[i];
+
+ current_slice_intensities->width = (int) current_slice_intensities->points.size ();
+ current_slice_intensities->height = 1;
+
+ // Concatenating XYZ and Intensities
+ pcl::concatenateFields (*current_slice_xyz, *current_slice_intensities, *current_slice);
+ current_slice->width = (int) current_slice->points.size ();
+ current_slice->height = 1;
+
+ // transform the slice from local to global coordinates
+ Eigen::Affine3f global_cloud_transformation;
+ global_cloud_transformation.translation ()[0] = buffer_.origin_GRID_global.x;
+ global_cloud_transformation.translation ()[1] = buffer_.origin_GRID_global.y;
+ global_cloud_transformation.translation ()[2] = buffer_.origin_GRID_global.z;
+ global_cloud_transformation.linear () = Eigen::Matrix3f::Identity ();
+ transformPointCloud (*current_slice, *current_slice, global_cloud_transformation);
+
+ // retrieve existing data from the world model
+ PointCloud<PointXYZI>::Ptr previously_existing_slice (new PointCloud<PointXYZI>);
+ double min_bound_x = buffer_.origin_GRID_global.x + buffer_.voxels_size.x - 1;
+ double new_origin_x = buffer_.origin_GRID_global.x + offset_x;
+ double new_origin_y = buffer_.origin_GRID_global.y + offset_y;
+ double new_origin_z = buffer_.origin_GRID_global.z + offset_z;
+
+ world_model_.getExistingData (buffer_.origin_GRID_global.x, buffer_.origin_GRID_global.y, buffer_.origin_GRID_global.z,
+ offset_x, offset_y, offset_z,
+ buffer_.voxels_size.x - 1, buffer_.voxels_size.y - 1, buffer_.voxels_size.z - 1,
+ *previously_existing_slice);
+
+ //replace world model data with values extracted from the TSDF buffer slice
+ world_model_.setSliceAsNans (buffer_.origin_GRID_global.x, buffer_.origin_GRID_global.y, buffer_.origin_GRID_global.z,
+ offset_x, offset_y, offset_z,
+ buffer_.voxels_size.x, buffer_.voxels_size.y, buffer_.voxels_size.z);
+
+
+ PCL_INFO ("world contains %d points after update\n", world_model_.getWorldSize ());
+ world_model_.cleanWorldFromNans ();
+ PCL_INFO ("world contains %d points after cleaning\n", world_model_.getWorldSize ());
+
+ // clear buffer slice and update the world model
+ pcl::device::kinfuLS::clearTSDFSlice (volume->data (), &buffer_, offset_x, offset_y, offset_z);
+
+ // insert current slice in the world if it contains any points
+ if (current_slice->points.size () != 0) {
+ world_model_.addSlice(current_slice);
+ }
+
+ // shift buffer addresses
+ shiftOrigin (volume, offset_x, offset_y, offset_z);
+
+ // push existing data in the TSDF buffer
+ if (previously_existing_slice->points.size () != 0 ) {
+ volume->pushSlice(previously_existing_slice, getBuffer () );
+ }
+}
+
+void
+pcl::gpu::kinfuLS::CyclicalBuffer::computeAndSetNewCubeMetricOrigin (const pcl::PointXYZ &target_point, int &shiftX, int &shiftY, int &shiftZ)
+{
+ // compute new origin for the cube, based on the target point
+ float3 new_cube_origin_meters;
+ new_cube_origin_meters.x = target_point.x - buffer_.volume_size.x/2.0f;
+ new_cube_origin_meters.y = target_point.y - buffer_.volume_size.y/2.0f;
+ new_cube_origin_meters.z = target_point.z - buffer_.volume_size.z/2.0f;
+ PCL_INFO ("The old cube's metric origin was (%f, %f, %f).\n", buffer_.origin_metric.x, buffer_.origin_metric.y, buffer_.origin_metric.z);
+ PCL_INFO ("The new cube's metric origin is now (%f, %f, %f).\n", new_cube_origin_meters.x, new_cube_origin_meters.y, new_cube_origin_meters.z);
+
+ // deduce each shift in indices
+ shiftX = (int)( (new_cube_origin_meters.x - buffer_.origin_metric.x) * ( buffer_.voxels_size.x / (float) (buffer_.volume_size.x) ) );
+ shiftY = (int)( (new_cube_origin_meters.y - buffer_.origin_metric.y) * ( buffer_.voxels_size.y / (float) (buffer_.volume_size.y) ) );
+ shiftZ = (int)( (new_cube_origin_meters.z - buffer_.origin_metric.z) * ( buffer_.voxels_size.z / (float) (buffer_.volume_size.z) ) );
+
+ // update the cube's metric origin
+ buffer_.origin_metric = new_cube_origin_meters;
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_GPU_KINFU_LARGE_SCALE_CUDA_ESTIMATE_COMBINED_H_
+#define PCL_GPU_KINFU_LARGE_SCALE_CUDA_ESTIMATE_COMBINED_H_
+
+#define ESTIMATE_COMBINED_CUDA_GRID_X 32
+#define ESTIMATE_COMBINED_CUDA_GRID_Y 8
+
+#endif // PCL_GPU_KINFU_LARGE_SCALE_CUDA_ESTIMATE_COMBINED_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_KINFU_INTERNAL_HPP_
+#define PCL_KINFU_INTERNAL_HPP_
+
+#include <pcl/gpu/utils/safe_call.hpp>
+#include <pcl/gpu/kinfu_large_scale/device.h>
+
+//using namespace pcl::gpu;
+
+namespace pcl
+{
+ namespace device
+ {
+ namespace kinfuLS
+ {
+ /** \brief Light source collection
+ */
+ struct LightSource
+ {
+ float3 pos[1];
+ int number;
+ };
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Maps
+
+ /** \brief Perfoms bilateral filtering of disparity map
+ * \param[in] src soruce map
+ * \param[out] dst output map
+ */
+ void
+ bilateralFilter (const DepthMap& src, DepthMap& dst);
+
+ /** \brief Computes depth pyramid
+ * \param[in] src source
+ * \param[out] dst destination
+ */
+ void
+ pyrDown (const DepthMap& src, DepthMap& dst);
+
+ /** \brief Computes vertex map
+ * \param[in] intr depth camera intrinsics
+ * \param[in] depth depth
+ * \param[out] vmap vertex map
+ */
+ void
+ createVMap (const Intr& intr, const DepthMap& depth, MapArr& vmap);
+
+ /** \brief Computes normal map using cross product
+ * \param[in] vmap vertex map
+ * \param[out] nmap normal map
+ */
+ void
+ createNMap (const MapArr& vmap, MapArr& nmap);
+
+ /** \brief Computes normal map using Eigen/PCA approach
+ * \param[in] vmap vertex map
+ * \param[out] nmap normal map
+ */
+ void
+ computeNormalsEigen (const MapArr& vmap, MapArr& nmap);
+
+ /** \brief Performs affine tranform of vertex and normal maps
+ * \param[in] vmap_src source vertex map
+ * \param[in] nmap_src source vertex map
+ * \param[in] Rmat Rotation mat
+ * \param[in] tvec translation
+ * \param[out] vmap_dst destination vertex map
+ * \param[out] nmap_dst destination vertex map
+ */
+ void
+ transformMaps (const MapArr& vmap_src, const MapArr& nmap_src, const Mat33& Rmat, const float3& tvec, MapArr& vmap_dst, MapArr& nmap_dst);
+
+ /** \brief Performs depth truncation
+ * \param[out] depth depth map to truncation
+ * \param[in] max_distance truncation threshold, values that are higher than the threshold are reset to zero (means not measurement)
+ */
+ void
+ truncateDepth(DepthMap& depth, float max_distance);
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // ICP
+
+ /** \brief (now it's exra code) Computes corespondances map
+ * \param[in] vmap_g_curr current vertex map in global coo space
+ * \param[in] nmap_g_curr current normals map in global coo space
+ * \param[in] Rprev_inv inverse camera rotation at previous pose
+ * \param[in] tprev camera translation at previous pose
+ * \param[in] intr camera intrinsics
+ * \param[in] vmap_g_prev previous vertex map in global coo space
+ * \param[in] nmap_g_prev previous vertex map in global coo space
+ * \param[in] distThres distance filtering threshold
+ * \param[in] angleThres angle filtering threshold. Represents sine of angle between normals
+ * \param[out] coresp
+ */
+ void
+ findCoresp (const MapArr& vmap_g_curr, const MapArr& nmap_g_curr, const Mat33& Rprev_inv, const float3& tprev, const Intr& intr,
+ const MapArr& vmap_g_prev, const MapArr& nmap_g_prev, float distThres, float angleThres, PtrStepSz<short2> coresp);
+
+ /** \brief (now it's exra code) Computation Ax=b for ICP iteration
+ * \param[in] v_dst destination vertex map (previous frame cloud)
+ * \param[in] n_dst destination normal map (previous frame normals)
+ * \param[in] v_src source normal map (current frame cloud)
+ * \param[in] coresp Corespondances
+ * \param[out] gbuf temp buffer for GPU reduction
+ * \param[out] mbuf output GPU buffer for matrix computed
+ * \param[out] matrixA_host A
+ * \param[out] vectorB_host b
+ */
+ void
+ estimateTransform (const MapArr& v_dst, const MapArr& n_dst, const MapArr& v_src, const PtrStepSz<short2>& coresp,
+ DeviceArray2D<float>& gbuf, DeviceArray<float>& mbuf, float* matrixA_host, float* vectorB_host);
+
+
+ /** \brief Computation Ax=b for ICP iteration
+ * \param[in] Rcurr Rotation of current camera pose guess
+ * \param[in] tcurr translation of current camera pose guess
+ * \param[in] vmap_curr current vertex map in camera coo space
+ * \param[in] nmap_curr current vertex map in camera coo space
+ * \param[in] Rprev_inv inverse camera rotation at previous pose
+ * \param[in] tprev camera translation at previous pose
+ * \param[in] intr camera intrinsics
+ * \param[in] vmap_g_prev previous vertex map in global coo space
+ * \param[in] nmap_g_prev previous vertex map in global coo space
+ * \param[in] distThres distance filtering threshold
+ * \param[in] angleThres angle filtering threshold. Represents sine of angle between normals
+ * \param[out] gbuf temp buffer for GPU reduction
+ * \param[out] mbuf output GPU buffer for matrix computed
+ * \param[out] matrixA_host A
+ * \param[out] vectorB_host b
+ */
+ void
+ estimateCombined (const Mat33& Rcurr, const float3& tcurr, const MapArr& vmap_curr, const MapArr& nmap_curr, const Mat33& Rprev_inv, const float3& tprev, const Intr& intr,
+ const MapArr& vmap_g_prev, const MapArr& nmap_g_prev, float distThres, float angleThres,
+ DeviceArray2D<float>& gbuf, DeviceArray<float>& mbuf, float* matrixA_host, float* vectorB_host);
+
+ /** \brief Computation Ax=b for ICP iteration
+ * \param[in] Rcurr Rotation of current camera pose guess
+ * \param[in] tcurr translation of current camera pose guess
+ * \param[in] vmap_curr current vertex map in camera coo space
+ * \param[in] nmap_curr current vertex map in camera coo space
+ * \param[in] Rprev_inv inverse camera rotation at previous pose
+ * \param[in] tprev camera translation at previous pose
+ * \param[in] intr camera intrinsics
+ * \param[in] vmap_g_prev previous vertex map in global coo space
+ * \param[in] nmap_g_prev previous vertex map in global coo space
+ * \param[in] distThres distance filtering threshold
+ * \param[in] angleThres angle filtering threshold. Represents sine of angle between normals
+ * \param[out] gbuf temp buffer for GPU reduction
+ * \param[out] mbuf output GPU buffer for matrix computed
+ * \param[out] matrixA_host A
+ * \param[out] vectorB_host b
+ */
+ void
+ estimateCombined (const Mat33& Rcurr, const float3& tcurr, const MapArr& vmap_curr, const MapArr& nmap_curr, const Mat33& Rprev_inv, const float3& tprev, const Intr& intr,
+ const MapArr& vmap_g_prev, const MapArr& nmap_g_prev, float distThres, float angleThres,
+ DeviceArray2D<double>& gbuf, DeviceArray<double>& mbuf, double* matrixA_host, double* vectorB_host);
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // TSDF volume functions
+
+ /** \brief Perform tsdf volume initialization
+ * \param[out] array volume to be initialized
+ */
+ PCL_EXPORTS void
+ initVolume(PtrStep<short2> array);
+
+ //first version
+ /** \brief Performs Tsfg volume uptation (extra obsolete now)
+ * \param[in] depth_raw Kinect depth image
+ * \param[in] intr camera intrinsics
+ * \param[in] volume_size size of volume in mm
+ * \param[in] Rcurr_inv inverse rotation for current camera pose
+ * \param[in] tcurr translation for current camera pose
+ * \param[in] tranc_dist tsdf truncation distance
+ * \param[in] volume tsdf volume to be updated
+ */
+ void
+ integrateTsdfVolume (const PtrStepSz<ushort>& depth_raw, const Intr& intr, const float3& volume_size,
+ const Mat33& Rcurr_inv, const float3& tcurr, float tranc_dist, PtrStep<short2> volume);
+
+ //second version
+ /** \brief Function that integrates volume if volume element contains: 2 bytes for round(tsdf*SHORT_MAX) and 2 bytes for integer weight.
+ * \param[in] depth Kinect depth image
+ * \param[in] intr camera intrinsics
+ * \param[in] volume_size size of volume in mm
+ * \param[in] Rcurr_inv inverse rotation for current camera pose
+ * \param[in] tcurr translation for current camera pose
+ * \param[in] tranc_dist tsdf truncation distance
+ * \param[in] volume tsdf volume to be updated
+ * \param[in] buffer cyclical buffer structure
+ * \param[out] depthScaled Buffer for scaled depth along ray
+ */
+ PCL_EXPORTS void
+ integrateTsdfVolume (const PtrStepSz<ushort>& depth, const Intr& intr, const float3& volume_size,
+ const Mat33& Rcurr_inv, const float3& tcurr, float tranc_dist, PtrStep<short2> volume, const pcl::gpu::kinfuLS::tsdf_buffer* buffer, DeviceArray2D<float>& depthScaled);
+
+ /** \brief Function that clears the TSDF values. The clearing takes place from the origin (in indices) to an offset in X,Y,Z values accordingly
+ * \param[in] volume Pointer to TSDF volume in GPU
+ * \param[in] buffer Pointer to the buffer struct that contains information about memory addresses of the tsdf volume memory block, which are used for the cyclic buffer.
+ * \param[in] shiftX Offset in indices that will be cleared from the TSDF volume. The clearing start from buffer.OriginX and stops in OriginX + shiftX
+ * \param[in] shiftY Offset in indices that will be cleared from the TSDF volume. The clearing start from buffer.OriginY and stops in OriginY + shiftY
+ * \param[in] shiftZ Offset in indices that will be cleared from the TSDF volume. The clearing start from buffer.OriginZ and stops in OriginZ + shiftZ
+ */
+ PCL_EXPORTS void
+ clearTSDFSlice (PtrStep<short2> volume, pcl::gpu::kinfuLS::tsdf_buffer* buffer, int shiftX, int shiftY, int shiftZ);
+
+ /** \brief Initialzied color volume
+ * \param[out] color_volume color volume for initialization
+ */
+ void
+ initColorVolume(PtrStep<uchar4> color_volume);
+
+ /** \brief Performs integration in color volume
+ * \param[in] intr Depth camera intrionsics structure
+ * \param[in] tranc_dist tsdf truncation distance
+ * \param[in] R_inv Inverse camera rotation
+ * \param[in] t camera translation
+ * \param[in] vmap Raycasted vertex map
+ * \param[in] colors RGB colors for current frame
+ * \param[in] volume_size volume size in meters
+ * \param[in] color_volume color volume to be integrated
+ * \param[in] max_weight max weight for running color average. Zero means not average, one means average with prev value, etc.
+ */
+ void
+ updateColorVolume(const Intr& intr, float tranc_dist, const Mat33& R_inv, const float3& t, const MapArr& vmap,
+ const PtrStepSz<uchar3>& colors, const float3& volume_size, PtrStep<uchar4> color_volume, int max_weight = 1);
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Raycast and view generation
+ /** \brief Generation vertex and normal maps from volume for current camera pose
+ * \param[in] intr camera intrinsices
+ * \param[in] Rcurr current rotation
+ * \param[in] tcurr current translation
+ * \param[in] tranc_dist volume truncation distance
+ * \param[in] volume_size volume size in mm
+ * \param[in] volume tsdf volume
+ * \param[in] buffer cyclical buffer structure
+ * \param[out] vmap output vertex map
+ * \param[out] nmap output normals map
+ */
+ void
+ raycast (const Intr& intr, const Mat33& Rcurr, const float3& tcurr, float tranc_dist, const float3& volume_size,
+ const PtrStep<short2>& volume, const pcl::gpu::kinfuLS::tsdf_buffer* buffer, MapArr& vmap, MapArr& nmap);
+
+ /** \brief Renders 3D image of the scene
+ * \param[in] vmap vertex map
+ * \param[in] nmap normals map
+ * \param[in] light pose of light source
+ * \param[out] dst buffer where image is generated
+ */
+ void
+ generateImage (const MapArr& vmap, const MapArr& nmap, const LightSource& light, PtrStepSz<uchar3> dst);
+
+
+ /** \brief Renders depth image from give pose
+ * \param[in] R_inv inverse camera rotation
+ * \param[in] t camera translation
+ * \param[in] vmap vertex map
+ * \param[out] dst buffer where depth is generated
+ */
+ void
+ generateDepth (const Mat33& R_inv, const float3& t, const MapArr& vmap, DepthMap& dst);
+
+ /** \brief Paints 3D view with color map
+ * \param[in] colors rgb color frame from OpenNI
+ * \param[out] dst output 3D view
+ * \param[in] colors_weight weight for colors
+ */
+ void
+ paint3DView(const PtrStep<uchar3>& colors, PtrStepSz<uchar3> dst, float colors_weight = 0.5f);
+
+ /** \brief Performs resize of vertex map to next pyramid level by averaging each four points
+ * \param[in] input vertext map
+ * \param[out] output resized vertex map
+ */
+ void
+ resizeVMap (const MapArr& input, MapArr& output);
+
+ /** \brief Performs resize of vertex map to next pyramid level by averaging each four normals
+ * \param[in] input normal map
+ * \param[out] output vertex map
+ */
+ void
+ resizeNMap (const MapArr& input, MapArr& output);
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Push data to TSDF
+
+ /** \brief Loads the values of a tsdf point cloud to the tsdf volume in GPU
+ * \param[in] volume tsdf volume
+ * \param[in] cloud_gpu contains the data to be pushed to the tsdf volume
+ * \param[in] buffer Pointer to the buffer struct that contains information about memory addresses of the tsdf volume memory block, which are used for the cyclic buffer.
+ */
+ /*PCL_EXPORTS*/ void
+ pushCloudAsSliceGPU (const PtrStep<short2>& volume, pcl::gpu::DeviceArray<PointType> cloud_gpu, const pcl::gpu::kinfuLS::tsdf_buffer* buffer);
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Cloud extraction
+
+ /** \brief Perform point cloud extraction from tsdf volume
+ * \param[in] volume tsdf volume
+ * \param[in] volume_size size of the volume
+ * \param[out] output buffer large enought to store point cloud
+ * \return number of point stored to passed buffer
+ */
+ PCL_EXPORTS size_t
+ extractCloud (const PtrStep<short2>& volume, const float3& volume_size, PtrSz<PointType> output);
+
+ /** \brief Perform point cloud extraction of a slice from tsdf volume
+ * \param[in] volume tsdf volume on GPU
+ * \param[in] volume_size size of the volume
+ * \param[in] buffer Pointer to the buffer struct that contains information about memory addresses of the tsdf volume memory block, which are used for the cyclic buffer.
+ * \param[in] shiftX Offset in indices that will be cleared from the TSDF volume. The clearing start from buffer.OriginX and stops in OriginX + shiftX
+ * \param[in] shiftY Offset in indices that will be cleared from the TSDF volume. The clearing start from buffer.OriginY and stops in OriginY + shiftY
+ * \param[in] shiftZ Offset in indices that will be cleared from the TSDF volume. The clearing start from buffer.OriginZ and stops in OriginZ + shiftZ
+ * \param[out] output_xyz buffer large enought to store point cloud xyz values
+ * \param[out] output_intensities buffer large enought to store point cloud intensity values
+ * \return number of point stored to passed buffer
+ */
+ PCL_EXPORTS size_t
+ extractSliceAsCloud (const PtrStep<short2>& volume, const float3& volume_size, const pcl::gpu::kinfuLS::tsdf_buffer* buffer, const int shiftX, const int shiftY, const int shiftZ, PtrSz<PointType> output_xyz, PtrSz<float> output_intensities);
+
+ /** \brief Performs normals computation for given poins using tsdf volume
+ * \param[in] volume tsdf volume
+ * \param[in] volume_size volume size
+ * \param[in] input points where normals are computed
+ * \param[out] output normals. Could be float4 or float8. If for a point normal can't be computed, such normal is marked as nan.
+ */
+ template<typename NormalType>
+ void
+ extractNormals (const PtrStep<short2>& volume, const float3& volume_size, const PtrSz<PointType>& input, NormalType* output);
+
+ /** \brief Performs colors exctraction from color volume
+ * \param[in] color_volume color volume
+ * \param[in] volume_size volume size
+ * \param[in] points points for which color are computed
+ * \param[out] colors output array with colors.
+ */
+ void
+ exctractColors(const PtrStep<uchar4>& color_volume, const float3& volume_size, const PtrSz<PointType>& points, uchar4* colors);
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Utility
+ struct float8 { float x, y, z, w, c1, c2, c3, c4; };
+ struct float12 { float x, y, z, w, normal_x, normal_y, normal_z, n4, c1, c2, c3, c4; };
+
+ /** \brief Conversion from SOA to AOS
+ * \param[in] vmap SOA map
+ * \param[out] output Array of 3D points. Can be float4 or float8.
+ */
+ template<typename T>
+ void
+ convert (const MapArr& vmap, DeviceArray2D<T>& output);
+
+ /** \brief Merges pcl::PointXYZ and pcl::Normal to PointNormal
+ * \param[in] cloud points cloud
+ * \param[in] normals normals cloud
+ * \param[out] output array of PointNomals.
+ */
+ void
+ mergePointNormal(const DeviceArray<float4>& cloud, const DeviceArray<float8>& normals, const DeviceArray<float12>& output);
+
+ /** \brief Check for qnan (unused now)
+ * \param[in] value
+ */
+ inline bool
+ valid_host (float value)
+ {
+ return *reinterpret_cast<int*>(&value) != 0x7fffffff; //QNAN
+ }
+
+ /** \brief synchronizes CUDA execution */
+ inline
+ void
+ sync () { cudaSafeCall (cudaDeviceSynchronize ()); }
+
+
+ template<class D, class Matx> D&
+ device_cast (Matx& matx)
+ {
+ return (*reinterpret_cast<D*>(matx.data ()));
+ }
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Marching cubes implementation
+
+ /** \brief Binds marching cubes tables to texture references */
+ void
+ bindTextures(const int *edgeBuf, const int *triBuf, const int *numVertsBuf);
+
+ /** \brief Unbinds */
+ void
+ unbindTextures();
+
+ /** \brief Scans tsdf volume and retrieves occuped voxes
+ * \param[in] volume tsdf volume
+ * \param[out] occupied_voxels buffer for occuped voxels. The function fulfills first row with voxel ids and second row with number of vertextes.
+ * \return number of voxels in the buffer
+ */
+ int
+ getOccupiedVoxels(const PtrStep<short2>& volume, DeviceArray2D<int>& occupied_voxels);
+
+ /** \brief Computes total number of vertexes for all voxels and offsets of vertexes in final triangle array
+ * \param[out] occupied_voxels buffer with occuped voxels. The function fulfills 3nd only with offsets
+ * \return total number of vertexes
+ */
+ int
+ computeOffsetsAndTotalVertexes(DeviceArray2D<int>& occupied_voxels);
+
+ /** \brief Generates final triangle array
+ * \param[in] volume tsdf volume
+ * \param[in] occupied_voxels occuped voxel ids (first row), number of vertexes(second row), offsets(third row).
+ * \param[in] volume_size volume size in meters
+ * \param[out] output triangle array
+ */
+ void
+ generateTriangles(const PtrStep<short2>& volume, const DeviceArray2D<int>& occupied_voxels, const float3& volume_size, DeviceArray<PointType>& output);
+ }
+ }
+}
+
+#endif /* PCL_KINFU_INTERNAL_HPP_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <iostream>
+#include <algorithm>
+
+#include <pcl/common/time.h>
+#include <pcl/gpu/kinfu_large_scale/kinfu.h>
+#include "estimate_combined.h"
+#include "internal.h"
+
+#include <Eigen/Core>
+#include <Eigen/SVD>
+#include <Eigen/Cholesky>
+#include <Eigen/Geometry>
+#include <Eigen/LU>
+
+#ifdef HAVE_OPENCV
+ #include <opencv2/opencv.hpp>
+ //~ #include <opencv2/gpu/gpu.hpp>
+#endif
+
+using namespace std;
+using namespace pcl::device::kinfuLS;
+
+using pcl::device::kinfuLS::device_cast;
+using Eigen::AngleAxisf;
+using Eigen::Array3f;
+using Eigen::Vector3i;
+using Eigen::Vector3f;
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace kinfuLS
+ {
+ Eigen::Vector3f rodrigues2(const Eigen::Matrix3f& matrix);
+ }
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::gpu::kinfuLS::KinfuTracker::KinfuTracker (const Eigen::Vector3f &volume_size, const float shiftingDistance, int rows, int cols) :
+ cyclical_( DISTANCE_THRESHOLD, VOLUME_SIZE, VOLUME_X), rows_(rows), cols_(cols), global_time_(0), max_icp_distance_(0), integration_metric_threshold_(0.f), perform_last_scan_ (false), finished_(false), lost_ (false), disable_icp_ (false)
+{
+ //const Vector3f volume_size = Vector3f::Constant (VOLUME_SIZE);
+ const Vector3i volume_resolution (VOLUME_X, VOLUME_Y, VOLUME_Z);
+
+ volume_size_ = volume_size(0);
+
+ tsdf_volume_ = TsdfVolume::Ptr ( new TsdfVolume(volume_resolution) );
+ tsdf_volume_->setSize (volume_size);
+
+ shifting_distance_ = shiftingDistance;
+
+ // set cyclical buffer values
+ cyclical_.setDistanceThreshold (shifting_distance_);
+ cyclical_.setVolumeSize (volume_size_, volume_size_, volume_size_);
+
+ setDepthIntrinsics (FOCAL_LENGTH, FOCAL_LENGTH); // default values, can be overwritten
+
+ init_Rcam_ = Eigen::Matrix3f::Identity ();// * AngleAxisf(-30.f/180*3.1415926, Vector3f::UnitX());
+ init_tcam_ = volume_size * 0.5f - Vector3f (0, 0, volume_size (2) / 2 * 1.2f);
+
+ const int iters[] = {10, 5, 4};
+ std::copy (iters, iters + LEVELS, icp_iterations_);
+
+ const float default_distThres = 0.10f; //meters
+ const float default_angleThres = sin (20.f * 3.14159254f / 180.f);
+ const float default_tranc_dist = 0.03f; //meters
+
+ setIcpCorespFilteringParams (default_distThres, default_angleThres);
+ tsdf_volume_->setTsdfTruncDist (default_tranc_dist);
+
+ allocateBufffers (rows, cols);
+
+ rmats_.reserve (30000);
+ tvecs_.reserve (30000);
+
+ reset ();
+
+ // initialize cyclical buffer
+ cyclical_.initBuffer(tsdf_volume_);
+
+ last_estimated_rotation_= Eigen::Matrix3f::Identity ();
+ last_estimated_translation_= volume_size * 0.5f - Vector3f (0, 0, volume_size (2) / 2 * 1.2f);
+
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::kinfuLS::KinfuTracker::setDepthIntrinsics (float fx, float fy, float cx, float cy)
+{
+ fx_ = fx;
+ fy_ = fy;
+ cx_ = (cx == -1) ? cols_/2-0.5f : cx;
+ cy_ = (cy == -1) ? rows_/2-0.5f : cy;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::kinfuLS::KinfuTracker::setInitialCameraPose (const Eigen::Affine3f& pose)
+{
+ init_Rcam_ = pose.rotation ();
+ init_tcam_ = pose.translation ();
+ //reset (); // (already called in constructor)
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::kinfuLS::KinfuTracker::setDepthTruncationForICP (float max_icp_distance)
+{
+ max_icp_distance_ = max_icp_distance;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::kinfuLS::KinfuTracker::setCameraMovementThreshold(float threshold)
+{
+ integration_metric_threshold_ = threshold;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::kinfuLS::KinfuTracker::setIcpCorespFilteringParams (float distThreshold, float sineOfAngle)
+{
+ distThres_ = distThreshold; //mm
+ angleThres_ = sineOfAngle;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+int
+pcl::gpu::kinfuLS::KinfuTracker::cols ()
+{
+ return (cols_);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+int
+pcl::gpu::kinfuLS::KinfuTracker::rows ()
+{
+ return (rows_);
+}
+
+void
+pcl::gpu::kinfuLS::KinfuTracker::extractAndSaveWorld ()
+{
+
+ //extract current volume to world model
+ PCL_INFO("Extracting current volume...");
+ cyclical_.checkForShift(tsdf_volume_, getCameraPose (), 0.6 * volume_size_, true, true, true); // this will force the extraction of the whole cube.
+ PCL_INFO("Done\n");
+
+ finished_ = true; // TODO maybe we could add a bool param to prevent kinfuLS from exiting after we saved the current world model
+
+ int cloud_size = 0;
+
+ cloud_size = cyclical_.getWorldModel ()->getWorld ()->points.size();
+
+ if (cloud_size <= 0)
+ {
+ PCL_WARN ("World model currently has no points. Skipping save procedure.\n");
+ return;
+ }
+ else
+ {
+ PCL_INFO ("Saving current world to world.pcd with %d points.\n", cloud_size);
+ pcl::io::savePCDFile<pcl::PointXYZI> ("world.pcd", *(cyclical_.getWorldModel ()->getWorld ()), true);
+ return;
+ }
+
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::kinfuLS::KinfuTracker::reset ()
+{
+ cout << "in reset function!" << std::endl;
+
+ if (global_time_)
+ PCL_WARN ("Reset\n");
+
+ // dump current world to a pcd file
+ /*
+ if (global_time_)
+ {
+ PCL_INFO ("Saving current world to current_world.pcd\n");
+ pcl::io::savePCDFile<pcl::PointXYZI> ("current_world.pcd", *(cyclical_.getWorldModel ()->getWorld ()), true);
+ // clear world model
+ cyclical_.getWorldModel ()->reset ();
+ }
+ */
+
+ // clear world model
+ cyclical_.getWorldModel ()->reset ();
+
+
+ global_time_ = 0;
+ rmats_.clear ();
+ tvecs_.clear ();
+
+ rmats_.push_back (init_Rcam_);
+ tvecs_.push_back (init_tcam_);
+
+ tsdf_volume_->reset ();
+
+ // reset cyclical buffer as well
+ cyclical_.resetBuffer (tsdf_volume_);
+
+ if (color_volume_) // color integration mode is enabled
+ color_volume_->reset ();
+
+ // reset estimated pose
+ last_estimated_rotation_= Eigen::Matrix3f::Identity ();
+ last_estimated_translation_= Vector3f (volume_size_, volume_size_, volume_size_) * 0.5f - Vector3f (0, 0, volume_size_ / 2 * 1.2f);
+
+
+ lost_=false;
+ has_shifted_=false;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::kinfuLS::KinfuTracker::allocateBufffers (int rows, int cols)
+{
+ depths_curr_.resize (LEVELS);
+ vmaps_g_curr_.resize (LEVELS);
+ nmaps_g_curr_.resize (LEVELS);
+
+ vmaps_g_prev_.resize (LEVELS);
+ nmaps_g_prev_.resize (LEVELS);
+
+ vmaps_curr_.resize (LEVELS);
+ nmaps_curr_.resize (LEVELS);
+
+ vmaps_prev_.resize (LEVELS);
+ nmaps_prev_.resize (LEVELS);
+
+ coresps_.resize (LEVELS);
+
+ for (int i = 0; i < LEVELS; ++i)
+ {
+ int pyr_rows = rows >> i;
+ int pyr_cols = cols >> i;
+
+ depths_curr_[i].create (pyr_rows, pyr_cols);
+
+ vmaps_g_curr_[i].create (pyr_rows*3, pyr_cols);
+ nmaps_g_curr_[i].create (pyr_rows*3, pyr_cols);
+
+ vmaps_g_prev_[i].create (pyr_rows*3, pyr_cols);
+ nmaps_g_prev_[i].create (pyr_rows*3, pyr_cols);
+
+ vmaps_curr_[i].create (pyr_rows*3, pyr_cols);
+ nmaps_curr_[i].create (pyr_rows*3, pyr_cols);
+
+ vmaps_prev_[i].create (pyr_rows*3, pyr_cols);
+ nmaps_prev_[i].create (pyr_rows*3, pyr_cols);
+
+ coresps_[i].create (pyr_rows, pyr_cols);
+ }
+ depthRawScaled_.create (rows, cols);
+ // see estimate tranform for the magic numbers
+ int r = (int)ceil ( ((float)rows) / ESTIMATE_COMBINED_CUDA_GRID_Y );
+ int c = (int)ceil ( ((float)cols) / ESTIMATE_COMBINED_CUDA_GRID_X );
+ gbuf_.create (27, r * c);
+ sumbuf_.create (27);
+}
+
+inline void
+pcl::gpu::kinfuLS::KinfuTracker::convertTransforms (Matrix3frm& rotation_in_1, Matrix3frm& rotation_in_2, Vector3f& translation_in_1, Vector3f& translation_in_2, Mat33& rotation_out_1, Mat33& rotation_out_2, float3& translation_out_1, float3& translation_out_2)
+{
+ rotation_out_1 = device_cast<Mat33> (rotation_in_1);
+ rotation_out_2 = device_cast<Mat33> (rotation_in_2);
+ translation_out_1 = device_cast<float3>(translation_in_1);
+ translation_out_2 = device_cast<float3>(translation_in_2);
+}
+
+inline void
+pcl::gpu::kinfuLS::KinfuTracker::convertTransforms (Matrix3frm& rotation_in_1, Matrix3frm& rotation_in_2, Vector3f& translation_in, Mat33& rotation_out_1, Mat33& rotation_out_2, float3& translation_out)
+{
+ rotation_out_1 = device_cast<Mat33> (rotation_in_1);
+ rotation_out_2 = device_cast<Mat33> (rotation_in_2);
+ translation_out = device_cast<float3>(translation_in);
+}
+
+inline void
+pcl::gpu::kinfuLS::KinfuTracker::convertTransforms (Matrix3frm& rotation_in, Vector3f& translation_in, Mat33& rotation_out, float3& translation_out)
+{
+ rotation_out = device_cast<Mat33> (rotation_in);
+ translation_out = device_cast<float3>(translation_in);
+}
+
+inline void
+pcl::gpu::kinfuLS::KinfuTracker::prepareMaps (const DepthMap& depth_raw, const Intr& cam_intrinsics)
+{
+ // blur raw map
+ bilateralFilter (depth_raw, depths_curr_[0]);
+
+ // optionally remove points that are farther than a threshold
+ if (max_icp_distance_ > 0)
+ truncateDepth(depths_curr_[0], max_icp_distance_);
+
+ // downsample map for each pyramid level
+ for (int i = 1; i < LEVELS; ++i)
+ pyrDown (depths_curr_[i-1], depths_curr_[i]);
+
+ // create vertex and normal maps for each pyramid level
+ for (int i = 0; i < LEVELS; ++i)
+ {
+ createVMap (cam_intrinsics(i), depths_curr_[i], vmaps_curr_[i]);
+ computeNormalsEigen (vmaps_curr_[i], nmaps_curr_[i]);
+ }
+
+}
+
+inline void
+pcl::gpu::kinfuLS::KinfuTracker::saveCurrentMaps()
+{
+ Matrix3frm rot_id = Eigen::Matrix3f::Identity ();
+ Mat33 identity_rotation = device_cast<Mat33> (rot_id);
+
+ // save vertex and normal maps for each level, keeping camera coordinates (i.e. no transform)
+ for (int i = 0; i < LEVELS; ++i)
+ {
+ transformMaps (vmaps_curr_[i], nmaps_curr_[i],identity_rotation, make_float3(0,0,0), vmaps_prev_[i], nmaps_prev_[i]);
+ }
+}
+
+inline bool
+pcl::gpu::kinfuLS::KinfuTracker::performICP(const Intr& cam_intrinsics, Matrix3frm& previous_global_rotation, Vector3f& previous_global_translation, Matrix3frm& current_global_rotation , Vector3f& current_global_translation)
+{
+
+ if(disable_icp_)
+ {
+ lost_=true;
+ return (false);
+ }
+
+ // Compute inverse rotation
+ Matrix3frm previous_global_rotation_inv = previous_global_rotation.inverse (); // Rprev.t();
+
+ ///////////////////////////////////////////////
+ // Convert pose to device type
+ Mat33 device_cam_rot_local_prev_inv;
+ float3 device_cam_trans_local_prev;
+ convertTransforms(previous_global_rotation_inv, previous_global_translation, device_cam_rot_local_prev_inv, device_cam_trans_local_prev);
+ device_cam_trans_local_prev -= getCyclicalBufferStructure ()->origin_metric; ;
+
+ // Use temporary pose, so that we modify the current global pose only if ICP converged
+ Matrix3frm resulting_rotation;
+ Vector3f resulting_translation;
+
+ // Initialize output pose to current pose
+ current_global_rotation = previous_global_rotation;
+ current_global_translation = previous_global_translation;
+
+ ///////////////////////////////////////////////
+ // Run ICP
+ {
+ //ScopeTime time("icp-all");
+ for (int level_index = LEVELS-1; level_index>=0; --level_index)
+ {
+ int iter_num = icp_iterations_[level_index];
+
+ // current vertex and normal maps
+ MapArr& vmap_curr = vmaps_curr_[level_index];
+ MapArr& nmap_curr = nmaps_curr_[level_index];
+
+ // previous vertex and normal maps
+ MapArr& vmap_g_prev = vmaps_g_prev_[level_index];
+ MapArr& nmap_g_prev = nmaps_g_prev_[level_index];
+
+ // We need to transform the maps from global to local coordinates
+ Mat33& rotation_id = device_cast<Mat33> (rmats_[0]); // Identity Rotation Matrix. Because we only need translation
+ float3 cube_origin = (getCyclicalBufferStructure ())->origin_metric;
+ cube_origin = -cube_origin;
+
+ MapArr& vmap_temp = vmap_g_prev;
+ MapArr& nmap_temp = nmap_g_prev;
+ transformMaps (vmap_temp, nmap_temp, rotation_id, cube_origin, vmap_g_prev, nmap_g_prev);
+
+ // run ICP for iter_num iterations (return false when lost)
+ for (int iter = 0; iter < iter_num; ++iter)
+ {
+ //CONVERT POSES TO DEVICE TYPES
+ // CURRENT LOCAL POSE
+ Mat33 device_current_rotation = device_cast<Mat33> (current_global_rotation); // We do not deal with changes in rotations
+ float3 device_current_translation_local = device_cast<float3> (current_global_translation);
+ device_current_translation_local -= getCyclicalBufferStructure ()->origin_metric;
+
+ Eigen::Matrix<double, 6, 6, Eigen::RowMajor> A;
+ Eigen::Matrix<double, 6, 1> b;
+
+ // call the ICP function (see paper by Kok-Lim Low "Linear Least-squares Optimization for Point-to-Plane ICP Surface Registration")
+ estimateCombined (device_current_rotation, device_current_translation_local, vmap_curr, nmap_curr, device_cam_rot_local_prev_inv, device_cam_trans_local_prev, cam_intrinsics (level_index),
+ vmap_g_prev, nmap_g_prev, distThres_, angleThres_, gbuf_, sumbuf_, A.data (), b.data ());
+
+ // checking nullspace
+ double det = A.determinant ();
+
+ if ( fabs (det) < 100000 /*1e-15*/ || pcl_isnan (det) ) //TODO find a threshold that makes ICP track well, but prevents it from generating wrong transforms
+ {
+ if (pcl_isnan (det)) cout << "qnan" << endl;
+ if(lost_ == false)
+ PCL_ERROR ("ICP LOST... PLEASE COME BACK TO THE LAST VALID POSE (green)\n");
+ //reset (); //GUI will now show the user that ICP is lost. User needs to press "R" to reset the volume
+ lost_ = true;
+ return (false);
+ }
+
+ Eigen::Matrix<float, 6, 1> result = A.llt ().solve (b).cast<float>();
+ float alpha = result (0);
+ float beta = result (1);
+ float gamma = result (2);
+
+ // deduce incremental rotation and translation from ICP's results
+ Eigen::Matrix3f cam_rot_incremental = (Eigen::Matrix3f)AngleAxisf (gamma, Vector3f::UnitZ ()) * AngleAxisf (beta, Vector3f::UnitY ()) * AngleAxisf (alpha, Vector3f::UnitX ());
+ Vector3f cam_trans_incremental = result.tail<3> ();
+
+ //compose global pose
+ current_global_translation = cam_rot_incremental * current_global_translation + cam_trans_incremental;
+ current_global_rotation = cam_rot_incremental * current_global_rotation;
+ }
+ }
+ }
+ // ICP has converged
+ lost_ = false;
+ return (true);
+}
+
+inline bool
+pcl::gpu::kinfuLS::KinfuTracker::performPairWiseICP(const Intr cam_intrinsics, Matrix3frm& resulting_rotation , Vector3f& resulting_translation)
+{
+ // we assume that both v and n maps are in the same coordinate space
+ // initialize rotation and translation to respectively identity and zero
+ Matrix3frm previous_rotation = Eigen::Matrix3f::Identity ();
+ Matrix3frm previous_rotation_inv = previous_rotation.inverse ();
+ Vector3f previous_translation = Vector3f(0,0,0);
+
+ ///////////////////////////////////////////////
+ // Convert pose to device type
+ Mat33 device_cam_rot_prev_inv;
+ float3 device_cam_trans_prev;
+ convertTransforms(previous_rotation_inv, previous_translation, device_cam_rot_prev_inv, device_cam_trans_prev);
+
+ // Initialize output pose to current pose (i.e. identity and zero translation)
+ Matrix3frm current_rotation = previous_rotation;
+ Vector3f current_translation = previous_translation;
+
+ ///////////////////////////////////////////////
+ // Run ICP
+ {
+ //ScopeTime time("icp-all");
+ for (int level_index = LEVELS-1; level_index>=0; --level_index)
+ {
+ int iter_num = icp_iterations_[level_index];
+
+ // current vertex and normal maps
+ MapArr& vmap_curr = vmaps_curr_[level_index];
+ MapArr& nmap_curr = nmaps_curr_[level_index];
+
+ // previous vertex and normal maps
+ MapArr& vmap_prev = vmaps_prev_[level_index];
+ MapArr& nmap_prev = nmaps_prev_[level_index];
+
+ // no need to transform maps from global to local since they are both in camera coordinates
+
+ // run ICP for iter_num iterations (return false when lost)
+ for (int iter = 0; iter < iter_num; ++iter)
+ {
+ //CONVERT POSES TO DEVICE TYPES
+ // CURRENT LOCAL POSE
+ Mat33 device_current_rotation = device_cast<Mat33> (current_rotation);
+ float3 device_current_translation_local = device_cast<float3> (current_translation);
+
+ Eigen::Matrix<double, 6, 6, Eigen::RowMajor> A;
+ Eigen::Matrix<double, 6, 1> b;
+
+ // call the ICP function (see paper by Kok-Lim Low "Linear Least-squares Optimization for Point-to-Plane ICP Surface Registration")
+ estimateCombined (device_current_rotation, device_current_translation_local, vmap_curr, nmap_curr, device_cam_rot_prev_inv, device_cam_trans_prev, cam_intrinsics (level_index),
+ vmap_prev, nmap_prev, distThres_, angleThres_, gbuf_, sumbuf_, A.data (), b.data ());
+
+ // checking nullspace
+ double det = A.determinant ();
+
+ if ( fabs (det) < 1e-15 || pcl_isnan (det) )
+ {
+ if (pcl_isnan (det)) cout << "qnan" << endl;
+
+ PCL_WARN ("ICP PairWise LOST...\n");
+ //reset ();
+ return (false);
+ }
+
+ Eigen::Matrix<float, 6, 1> result = A.llt ().solve (b).cast<float>();
+ float alpha = result (0);
+ float beta = result (1);
+ float gamma = result (2);
+
+ // deduce incremental rotation and translation from ICP's results
+ Eigen::Matrix3f cam_rot_incremental = (Eigen::Matrix3f)AngleAxisf (gamma, Vector3f::UnitZ ()) * AngleAxisf (beta, Vector3f::UnitY ()) * AngleAxisf (alpha, Vector3f::UnitX ());
+ Vector3f cam_trans_incremental = result.tail<3> ();
+
+ //compose global pose
+ current_translation = cam_rot_incremental * current_translation + cam_trans_incremental;
+ current_rotation = cam_rot_incremental * current_rotation;
+ }
+ }
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // since raw depthmaps are quite noisy, we make sure the estimated transform is big enought to be taken into account
+ float rnorm = rodrigues2(current_rotation).norm();
+ float tnorm = (current_translation).norm();
+ const float alpha = 1.f;
+ bool integrate = (rnorm + alpha * tnorm)/2 >= integration_metric_threshold_ * 2.0f;
+
+ if(integrate)
+ {
+ resulting_rotation = current_rotation;
+ resulting_translation = current_translation;
+ }
+ else
+ {
+ resulting_rotation = Eigen::Matrix3f::Identity ();
+ resulting_translation = Vector3f(0,0,0);
+ }
+ // ICP has converged
+ return (true);
+}
+
+bool
+pcl::gpu::kinfuLS::KinfuTracker::operator() (const DepthMap& depth_raw)
+{
+ // Intrisics of the camera
+ Intr intr (fx_, fy_, cx_, cy_);
+
+ // Physical volume size (meters)
+ float3 device_volume_size = device_cast<const float3> (tsdf_volume_->getSize());
+
+ // process the incoming raw depth map
+ prepareMaps (depth_raw, intr);
+
+ // sync GPU device
+ pcl::device::kinfuLS::sync ();
+
+ ///////////////////////////////////////////////////////////////////////////////////////////
+ // Initialization at first frame
+ if (global_time_ == 0) // this is the frist frame, the tsdf volume needs to be initialized
+ {
+ // Initial rotation
+ Matrix3frm initial_cam_rot = rmats_[0]; // [Ri|ti] - pos of camera
+ Matrix3frm initial_cam_rot_inv = initial_cam_rot.inverse ();
+ // Initial translation
+ Vector3f initial_cam_trans = tvecs_[0]; // transform from camera to global coo space for (i-1)th camera pose
+
+ // Convert pose to device types
+ Mat33 device_initial_cam_rot, device_initial_cam_rot_inv;
+ float3 device_initial_cam_trans;
+ convertTransforms (initial_cam_rot, initial_cam_rot_inv, initial_cam_trans, device_initial_cam_rot, device_initial_cam_rot_inv, device_initial_cam_trans);
+
+ // integrate current depth map into tsdf volume, from default initial pose.
+ integrateTsdfVolume(depth_raw, intr, device_volume_size, device_initial_cam_rot_inv, device_initial_cam_trans, tsdf_volume_->getTsdfTruncDist(), tsdf_volume_->data(), getCyclicalBufferStructure (), depthRawScaled_);
+
+ // transform vertex and normal maps for each pyramid level
+ for (int i = 0; i < LEVELS; ++i)
+ transformMaps (vmaps_curr_[i], nmaps_curr_[i], device_initial_cam_rot, device_initial_cam_trans, vmaps_g_prev_[i], nmaps_g_prev_[i]);
+
+ if(perform_last_scan_)
+ finished_ = true;
+
+ ++global_time_;
+
+ // save current vertex and normal maps
+ saveCurrentMaps ();
+ // return and wait for next frame
+ return (false);
+ }
+
+ ///////////////////////////////////////////////////////////////////////////////////////////
+ // Iterative Closest Point
+ // Get the last-known pose
+ Matrix3frm last_known_global_rotation = rmats_[global_time_ - 1]; // [Ri|ti] - pos of camera, i.e.
+ Vector3f last_known_global_translation = tvecs_[global_time_ - 1]; // transform from camera to global coo space for (i-1)th camera pose
+ // Declare variables to host ICP results
+ Matrix3frm current_global_rotation;
+ Vector3f current_global_translation;
+ // Call ICP
+ if(!performICP(intr, last_known_global_rotation, last_known_global_translation, current_global_rotation, current_global_translation))
+ {
+ // ICP based on synthetic maps failed -> try to estimate the current camera pose based on previous and current raw maps
+ Matrix3frm delta_rotation;
+ Vector3f delta_translation;
+ if(!performPairWiseICP(intr, delta_rotation, delta_translation))
+ {
+ // save current vertex and normal maps
+ saveCurrentMaps ();
+ return (false);
+ }
+
+ // Pose estimation succeeded -> update estimated pose
+ last_estimated_translation_ = delta_rotation * last_estimated_translation_ + delta_translation;
+ last_estimated_rotation_ = delta_rotation * last_estimated_rotation_;
+ // save current vertex and normal maps
+ saveCurrentMaps ();
+ return (true);
+ }
+ else
+ {
+ // ICP based on synthetic maps succeeded
+ // Save newly-computed pose
+ rmats_.push_back (current_global_rotation);
+ tvecs_.push_back (current_global_translation);
+ // Update last estimated pose to current pairwise ICP result
+ last_estimated_translation_ = current_global_translation;
+ last_estimated_rotation_ = current_global_rotation;
+ }
+
+ ///////////////////////////////////////////////////////////////////////////////////////////
+ // check if we need to shift
+ has_shifted_ = cyclical_.checkForShift(tsdf_volume_, getCameraPose (), 0.6 * volume_size_, true, perform_last_scan_); // TODO make target distance from camera a param
+ if(has_shifted_)
+ PCL_WARN ("SHIFTING\n");
+
+ ///////////////////////////////////////////////////////////////////////////////////////////
+ // get the NEW local pose as device types
+ Mat33 device_current_rotation_inv, device_current_rotation;
+ float3 device_current_translation_local;
+ Matrix3frm cam_rot_local_curr_inv = current_global_rotation.inverse (); //rotation (local = global)
+ convertTransforms(cam_rot_local_curr_inv, current_global_rotation, current_global_translation, device_current_rotation_inv, device_current_rotation, device_current_translation_local);
+ device_current_translation_local -= getCyclicalBufferStructure()->origin_metric; // translation (local translation = global translation - origin of cube)
+
+ ///////////////////////////////////////////////////////////////////////////////////////////
+ // Integration check - We do not integrate volume if camera does not move far enought.
+ {
+ float rnorm = rodrigues2(current_global_rotation.inverse() * last_known_global_rotation).norm();
+ float tnorm = (current_global_translation - last_known_global_translation).norm();
+ const float alpha = 1.f;
+ bool integrate = (rnorm + alpha * tnorm)/2 >= integration_metric_threshold_;
+ ///////////////////////////////////////////////////////////////////////////////////////////
+ // Volume integration
+ if (integrate)
+ {
+ integrateTsdfVolume (depth_raw, intr, device_volume_size, device_current_rotation_inv, device_current_translation_local, tsdf_volume_->getTsdfTruncDist (), tsdf_volume_->data (), getCyclicalBufferStructure (), depthRawScaled_);
+ }
+ }
+
+ ///////////////////////////////////////////////////////////////////////////////////////////
+ // Ray casting
+ {
+ // generate synthetic vertex and normal maps from newly-found pose.
+ raycast (intr, device_current_rotation, device_current_translation_local, tsdf_volume_->getTsdfTruncDist (), device_volume_size, tsdf_volume_->data (), getCyclicalBufferStructure (), vmaps_g_prev_[0], nmaps_g_prev_[0]);
+
+ // POST-PROCESSING: We need to transform the newly raycasted maps into the global space.
+ Mat33& rotation_id = device_cast<Mat33> (rmats_[0]); /// Identity Rotation Matrix. Because we never rotate our volume
+ float3 cube_origin = (getCyclicalBufferStructure ())->origin_metric;
+ MapArr& vmap_temp = vmaps_g_prev_[0];
+ MapArr& nmap_temp = nmaps_g_prev_[0];
+ transformMaps (vmap_temp, nmap_temp, rotation_id, cube_origin, vmaps_g_prev_[0], nmaps_g_prev_[0]);
+
+ // create pyramid levels for vertex and normal maps
+ for (int i = 1; i < LEVELS; ++i)
+ {
+ resizeVMap (vmaps_g_prev_[i-1], vmaps_g_prev_[i]);
+ resizeNMap (nmaps_g_prev_[i-1], nmaps_g_prev_[i]);
+ }
+ pcl::device::kinfuLS::sync ();
+ }
+
+ if(has_shifted_ && perform_last_scan_)
+ extractAndSaveWorld ();
+
+
+ ++global_time_;
+ // save current vertex and normal maps
+ saveCurrentMaps ();
+ return (true);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Eigen::Affine3f
+pcl::gpu::kinfuLS::KinfuTracker::getCameraPose (int time) const
+{
+ if (time > (int)rmats_.size () || time < 0)
+ time = rmats_.size () - 1;
+
+ Eigen::Affine3f aff;
+ aff.linear () = rmats_[time];
+ aff.translation () = tvecs_[time];
+ return (aff);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+Eigen::Affine3f
+pcl::gpu::kinfuLS::KinfuTracker::getLastEstimatedPose () const
+{
+ Eigen::Affine3f aff;
+ aff.linear () = last_estimated_rotation_;
+ aff.translation () = last_estimated_translation_;
+ return (aff);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+size_t
+pcl::gpu::kinfuLS::KinfuTracker::getNumberOfPoses () const
+{
+ return rmats_.size();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+const pcl::gpu::kinfuLS::TsdfVolume&
+pcl::gpu::kinfuLS::KinfuTracker::volume() const
+{
+ return *tsdf_volume_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::gpu::kinfuLS::TsdfVolume&
+pcl::gpu::kinfuLS::KinfuTracker::volume()
+{
+ return *tsdf_volume_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+const pcl::gpu::kinfuLS::ColorVolume&
+pcl::gpu::kinfuLS::KinfuTracker::colorVolume() const
+{
+ return *color_volume_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+pcl::gpu::kinfuLS::ColorVolume&
+pcl::gpu::kinfuLS::KinfuTracker::colorVolume()
+{
+ return *color_volume_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::kinfuLS::KinfuTracker::getImage (View& view) const
+{
+ //Eigen::Vector3f light_source_pose = tsdf_volume_->getSize() * (-3.f);
+ Eigen::Vector3f light_source_pose = tvecs_[tvecs_.size () - 1];
+
+ LightSource light;
+ light.number = 1;
+ light.pos[0] = device_cast<const float3>(light_source_pose);
+
+ view.create (rows_, cols_);
+ generateImage (vmaps_g_prev_[0], nmaps_g_prev_[0], light, view);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::kinfuLS::KinfuTracker::getLastFrameCloud (DeviceArray2D<PointType>& cloud) const
+{
+ cloud.create (rows_, cols_);
+ DeviceArray2D<float4>& c = (DeviceArray2D<float4>&)cloud;
+ convert (vmaps_g_prev_[0], c);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::kinfuLS::KinfuTracker::getLastFrameNormals (DeviceArray2D<NormalType>& normals) const
+{
+ normals.create (rows_, cols_);
+ DeviceArray2D<float8>& n = (DeviceArray2D<float8>&)normals;
+ convert (nmaps_g_prev_[0], n);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::kinfuLS::KinfuTracker::initColorIntegration(int max_weight)
+{
+ color_volume_ = pcl::gpu::kinfuLS::ColorVolume::Ptr( new ColorVolume(*tsdf_volume_, max_weight) );
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::gpu::kinfuLS::KinfuTracker::operator() (const DepthMap& depth, const View& colors)
+{
+ bool res = (*this)(depth);
+
+ if (res && color_volume_)
+ {
+ const float3 device_volume_size = device_cast<const float3> (tsdf_volume_->getSize());
+ Intr intr(fx_, fy_, cx_, cy_);
+
+ Matrix3frm R_inv = rmats_.back().inverse();
+ Vector3f t = tvecs_.back();
+
+ Mat33& device_Rcurr_inv = device_cast<Mat33> (R_inv);
+ float3& device_tcurr = device_cast<float3> (t);
+
+ updateColorVolume(intr, tsdf_volume_->getTsdfTruncDist(), device_Rcurr_inv, device_tcurr, vmaps_g_prev_[0],
+ colors, device_volume_size, color_volume_->data(), color_volume_->getMaxWeight());
+ }
+
+ return res;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace kinfuLS
+ {
+ PCL_EXPORTS void
+ paint3DView(const KinfuTracker::View& rgb24, KinfuTracker::View& view, float colors_weight = 0.5f)
+ {
+ pcl::device::kinfuLS::paint3DView(rgb24, view, colors_weight);
+ }
+
+ PCL_EXPORTS void
+ mergePointNormal(const DeviceArray<PointXYZ>& cloud, const DeviceArray<Normal>& normals, DeviceArray<PointNormal>& output)
+ {
+ const size_t size = min(cloud.size(), normals.size());
+ output.create(size);
+
+ const DeviceArray<float4>& c = (const DeviceArray<float4>&)cloud;
+ const DeviceArray<float8>& n = (const DeviceArray<float8>&)normals;
+ const DeviceArray<float12>& o = (const DeviceArray<float12>&)output;
+ pcl::device::kinfuLS::mergePointNormal(c, n, o);
+ }
+
+ Eigen::Vector3f rodrigues2(const Eigen::Matrix3f& matrix)
+ {
+ Eigen::JacobiSVD<Eigen::Matrix3f> svd(matrix, Eigen::ComputeFullV | Eigen::ComputeFullU);
+ Eigen::Matrix3f R = svd.matrixU() * svd.matrixV().transpose();
+
+ double rx = R(2, 1) - R(1, 2);
+ double ry = R(0, 2) - R(2, 0);
+ double rz = R(1, 0) - R(0, 1);
+
+ double s = sqrt((rx*rx + ry*ry + rz*rz)*0.25);
+ double c = (R.trace() - 1) * 0.5;
+ c = c > 1. ? 1. : c < -1. ? -1. : c;
+
+ double theta = acos(c);
+
+ if( s < 1e-5 )
+ {
+ double t;
+
+ if( c > 0 )
+ rx = ry = rz = 0;
+ else
+ {
+ t = (R(0, 0) + 1)*0.5;
+ rx = sqrt( std::max(t, 0.0) );
+ t = (R(1, 1) + 1)*0.5;
+ ry = sqrt( std::max(t, 0.0) ) * (R(0, 1) < 0 ? -1.0 : 1.0);
+ t = (R(2, 2) + 1)*0.5;
+ rz = sqrt( std::max(t, 0.0) ) * (R(0, 2) < 0 ? -1.0 : 1.0);
+
+ if( fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R(1, 2) > 0) != (ry*rz > 0) )
+ rz = -rz;
+ theta /= sqrt(rx*rx + ry*ry + rz*rz);
+ rx *= theta;
+ ry *= theta;
+ rz *= theta;
+ }
+ }
+ else
+ {
+ double vth = 1/(2*s);
+ vth *= theta;
+ rx *= vth; ry *= vth; rz *= vth;
+ }
+ return Eigen::Vector3d(rx, ry, rz).cast<float>();
+ }
+ }
+ }
+}
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Point Cloud Library (PCL) - www.pointclouds.org
+* Copyright (c) 2011, Willow Garage, Inc.
+*
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+*/
+
+#include <pcl/gpu/kinfu_large_scale/kinfu.h>
+#include <pcl/gpu/kinfu_large_scale/marching_cubes.h>
+#include "internal.h"
+
+using namespace pcl;
+using pcl::device::kinfuLS::device_cast;
+using pcl::gpu::DeviceArray;
+
+extern const int edgeTable[256];
+extern const int triTable[256][16];
+extern const int numVertsTable[256];
+
+pcl::gpu::kinfuLS::MarchingCubes::MarchingCubes()
+{
+ edgeTable_.upload(edgeTable, 256);
+ numVertsTable_.upload(numVertsTable, 256);
+ triTable_.upload(&triTable[0][0], 256 * 16);
+}
+
+pcl::gpu::kinfuLS::MarchingCubes::~MarchingCubes() {}
+
+DeviceArray<pcl::gpu::kinfuLS::MarchingCubes::PointType>
+pcl::gpu::kinfuLS::MarchingCubes::run(const TsdfVolume& tsdf, DeviceArray<PointType>& triangles_buffer)
+{
+ if (triangles_buffer.empty())
+ triangles_buffer.create(DEFAULT_TRIANGLES_BUFFER_SIZE);
+ occupied_voxels_buffer_.create(3, triangles_buffer.size() / 3);
+
+ pcl::device::kinfuLS::bindTextures(edgeTable_, triTable_, numVertsTable_);
+
+ int active_voxels = pcl::device::kinfuLS::getOccupiedVoxels(tsdf.data(), occupied_voxels_buffer_);
+ if(!active_voxels)
+ {
+ pcl::device::kinfuLS::unbindTextures();
+ return DeviceArray<PointType>();
+ }
+
+ DeviceArray2D<int> occupied_voxels(3, active_voxels, occupied_voxels_buffer_.ptr(), occupied_voxels_buffer_.step());
+
+ int total_vertexes = pcl::device::kinfuLS::computeOffsetsAndTotalVertexes(occupied_voxels);
+
+ float3 volume_size = device_cast<const float3>(tsdf.getSize());
+ pcl::device::kinfuLS::generateTriangles(tsdf.data(), occupied_voxels, volume_size, (DeviceArray<pcl::device::kinfuLS::PointType>&)triangles_buffer);
+
+ pcl::device::kinfuLS::unbindTextures();
+ return DeviceArray<PointType>(triangles_buffer.ptr(), total_vertexes);
+}
+
+
+// edge table maps 8-bit flag representing which cube vertices are inside
+// the isosurface to 12-bit number indicating which edges are intersected
+const int edgeTable[256] =
+{
+ 0x0 , 0x109, 0x203, 0x30a, 0x406, 0x50f, 0x605, 0x70c,
+ 0x80c, 0x905, 0xa0f, 0xb06, 0xc0a, 0xd03, 0xe09, 0xf00,
+ 0x190, 0x99 , 0x393, 0x29a, 0x596, 0x49f, 0x795, 0x69c,
+ 0x99c, 0x895, 0xb9f, 0xa96, 0xd9a, 0xc93, 0xf99, 0xe90,
+ 0x230, 0x339, 0x33 , 0x13a, 0x636, 0x73f, 0x435, 0x53c,
+ 0xa3c, 0xb35, 0x83f, 0x936, 0xe3a, 0xf33, 0xc39, 0xd30,
+ 0x3a0, 0x2a9, 0x1a3, 0xaa , 0x7a6, 0x6af, 0x5a5, 0x4ac,
+ 0xbac, 0xaa5, 0x9af, 0x8a6, 0xfaa, 0xea3, 0xda9, 0xca0,
+ 0x460, 0x569, 0x663, 0x76a, 0x66 , 0x16f, 0x265, 0x36c,
+ 0xc6c, 0xd65, 0xe6f, 0xf66, 0x86a, 0x963, 0xa69, 0xb60,
+ 0x5f0, 0x4f9, 0x7f3, 0x6fa, 0x1f6, 0xff , 0x3f5, 0x2fc,
+ 0xdfc, 0xcf5, 0xfff, 0xef6, 0x9fa, 0x8f3, 0xbf9, 0xaf0,
+ 0x650, 0x759, 0x453, 0x55a, 0x256, 0x35f, 0x55 , 0x15c,
+ 0xe5c, 0xf55, 0xc5f, 0xd56, 0xa5a, 0xb53, 0x859, 0x950,
+ 0x7c0, 0x6c9, 0x5c3, 0x4ca, 0x3c6, 0x2cf, 0x1c5, 0xcc ,
+ 0xfcc, 0xec5, 0xdcf, 0xcc6, 0xbca, 0xac3, 0x9c9, 0x8c0,
+ 0x8c0, 0x9c9, 0xac3, 0xbca, 0xcc6, 0xdcf, 0xec5, 0xfcc,
+ 0xcc , 0x1c5, 0x2cf, 0x3c6, 0x4ca, 0x5c3, 0x6c9, 0x7c0,
+ 0x950, 0x859, 0xb53, 0xa5a, 0xd56, 0xc5f, 0xf55, 0xe5c,
+ 0x15c, 0x55 , 0x35f, 0x256, 0x55a, 0x453, 0x759, 0x650,
+ 0xaf0, 0xbf9, 0x8f3, 0x9fa, 0xef6, 0xfff, 0xcf5, 0xdfc,
+ 0x2fc, 0x3f5, 0xff , 0x1f6, 0x6fa, 0x7f3, 0x4f9, 0x5f0,
+ 0xb60, 0xa69, 0x963, 0x86a, 0xf66, 0xe6f, 0xd65, 0xc6c,
+ 0x36c, 0x265, 0x16f, 0x66 , 0x76a, 0x663, 0x569, 0x460,
+ 0xca0, 0xda9, 0xea3, 0xfaa, 0x8a6, 0x9af, 0xaa5, 0xbac,
+ 0x4ac, 0x5a5, 0x6af, 0x7a6, 0xaa , 0x1a3, 0x2a9, 0x3a0,
+ 0xd30, 0xc39, 0xf33, 0xe3a, 0x936, 0x83f, 0xb35, 0xa3c,
+ 0x53c, 0x435, 0x73f, 0x636, 0x13a, 0x33 , 0x339, 0x230,
+ 0xe90, 0xf99, 0xc93, 0xd9a, 0xa96, 0xb9f, 0x895, 0x99c,
+ 0x69c, 0x795, 0x49f, 0x596, 0x29a, 0x393, 0x99 , 0x190,
+ 0xf00, 0xe09, 0xd03, 0xc0a, 0xb06, 0xa0f, 0x905, 0x80c,
+ 0x70c, 0x605, 0x50f, 0x406, 0x30a, 0x203, 0x109, 0x0
+};
+
+// triangle table maps same cube vertex index to a list of up to 5 triangles
+// which are built from the interpolated edge vertices
+const int triTable[256][16] =
+{
+ {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 1, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 8, 3, 9, 8, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 2, 10, 0, 2, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {2, 8, 3, 2, 10, 8, 10, 9, 8, -1, -1, -1, -1, -1, -1, -1},
+ {3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 11, 2, 8, 11, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 9, 0, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 11, 2, 1, 9, 11, 9, 8, 11, -1, -1, -1, -1, -1, -1, -1},
+ {3, 10, 1, 11, 10, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 10, 1, 0, 8, 10, 8, 11, 10, -1, -1, -1, -1, -1, -1, -1},
+ {3, 9, 0, 3, 11, 9, 11, 10, 9, -1, -1, -1, -1, -1, -1, -1},
+ {9, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 3, 0, 7, 3, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 1, 9, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 1, 9, 4, 7, 1, 7, 3, 1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {3, 4, 7, 3, 0, 4, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1},
+ {9, 2, 10, 9, 0, 2, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1},
+ {2, 10, 9, 2, 9, 7, 2, 7, 3, 7, 9, 4, -1, -1, -1, -1},
+ {8, 4, 7, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {11, 4, 7, 11, 2, 4, 2, 0, 4, -1, -1, -1, -1, -1, -1, -1},
+ {9, 0, 1, 8, 4, 7, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1},
+ {4, 7, 11, 9, 4, 11, 9, 11, 2, 9, 2, 1, -1, -1, -1, -1},
+ {3, 10, 1, 3, 11, 10, 7, 8, 4, -1, -1, -1, -1, -1, -1, -1},
+ {1, 11, 10, 1, 4, 11, 1, 0, 4, 7, 11, 4, -1, -1, -1, -1},
+ {4, 7, 8, 9, 0, 11, 9, 11, 10, 11, 0, 3, -1, -1, -1, -1},
+ {4, 7, 11, 4, 11, 9, 9, 11, 10, -1, -1, -1, -1, -1, -1, -1},
+ {9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 5, 4, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 5, 4, 1, 5, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {8, 5, 4, 8, 3, 5, 3, 1, 5, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {3, 0, 8, 1, 2, 10, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1},
+ {5, 2, 10, 5, 4, 2, 4, 0, 2, -1, -1, -1, -1, -1, -1, -1},
+ {2, 10, 5, 3, 2, 5, 3, 5, 4, 3, 4, 8, -1, -1, -1, -1},
+ {9, 5, 4, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 11, 2, 0, 8, 11, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1},
+ {0, 5, 4, 0, 1, 5, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1},
+ {2, 1, 5, 2, 5, 8, 2, 8, 11, 4, 8, 5, -1, -1, -1, -1},
+ {10, 3, 11, 10, 1, 3, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1},
+ {4, 9, 5, 0, 8, 1, 8, 10, 1, 8, 11, 10, -1, -1, -1, -1},
+ {5, 4, 0, 5, 0, 11, 5, 11, 10, 11, 0, 3, -1, -1, -1, -1},
+ {5, 4, 8, 5, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1},
+ {9, 7, 8, 5, 7, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 3, 0, 9, 5, 3, 5, 7, 3, -1, -1, -1, -1, -1, -1, -1},
+ {0, 7, 8, 0, 1, 7, 1, 5, 7, -1, -1, -1, -1, -1, -1, -1},
+ {1, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 7, 8, 9, 5, 7, 10, 1, 2, -1, -1, -1, -1, -1, -1, -1},
+ {10, 1, 2, 9, 5, 0, 5, 3, 0, 5, 7, 3, -1, -1, -1, -1},
+ {8, 0, 2, 8, 2, 5, 8, 5, 7, 10, 5, 2, -1, -1, -1, -1},
+ {2, 10, 5, 2, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1},
+ {7, 9, 5, 7, 8, 9, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1},
+ {9, 5, 7, 9, 7, 2, 9, 2, 0, 2, 7, 11, -1, -1, -1, -1},
+ {2, 3, 11, 0, 1, 8, 1, 7, 8, 1, 5, 7, -1, -1, -1, -1},
+ {11, 2, 1, 11, 1, 7, 7, 1, 5, -1, -1, -1, -1, -1, -1, -1},
+ {9, 5, 8, 8, 5, 7, 10, 1, 3, 10, 3, 11, -1, -1, -1, -1},
+ {5, 7, 0, 5, 0, 9, 7, 11, 0, 1, 0, 10, 11, 10, 0, -1},
+ {11, 10, 0, 11, 0, 3, 10, 5, 0, 8, 0, 7, 5, 7, 0, -1},
+ {11, 10, 5, 7, 11, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 0, 1, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 8, 3, 1, 9, 8, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1},
+ {1, 6, 5, 2, 6, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 6, 5, 1, 2, 6, 3, 0, 8, -1, -1, -1, -1, -1, -1, -1},
+ {9, 6, 5, 9, 0, 6, 0, 2, 6, -1, -1, -1, -1, -1, -1, -1},
+ {5, 9, 8, 5, 8, 2, 5, 2, 6, 3, 2, 8, -1, -1, -1, -1},
+ {2, 3, 11, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {11, 0, 8, 11, 2, 0, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1},
+ {0, 1, 9, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1},
+ {5, 10, 6, 1, 9, 2, 9, 11, 2, 9, 8, 11, -1, -1, -1, -1},
+ {6, 3, 11, 6, 5, 3, 5, 1, 3, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 11, 0, 11, 5, 0, 5, 1, 5, 11, 6, -1, -1, -1, -1},
+ {3, 11, 6, 0, 3, 6, 0, 6, 5, 0, 5, 9, -1, -1, -1, -1},
+ {6, 5, 9, 6, 9, 11, 11, 9, 8, -1, -1, -1, -1, -1, -1, -1},
+ {5, 10, 6, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 3, 0, 4, 7, 3, 6, 5, 10, -1, -1, -1, -1, -1, -1, -1},
+ {1, 9, 0, 5, 10, 6, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1},
+ {10, 6, 5, 1, 9, 7, 1, 7, 3, 7, 9, 4, -1, -1, -1, -1},
+ {6, 1, 2, 6, 5, 1, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 5, 5, 2, 6, 3, 0, 4, 3, 4, 7, -1, -1, -1, -1},
+ {8, 4, 7, 9, 0, 5, 0, 6, 5, 0, 2, 6, -1, -1, -1, -1},
+ {7, 3, 9, 7, 9, 4, 3, 2, 9, 5, 9, 6, 2, 6, 9, -1},
+ {3, 11, 2, 7, 8, 4, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1},
+ {5, 10, 6, 4, 7, 2, 4, 2, 0, 2, 7, 11, -1, -1, -1, -1},
+ {0, 1, 9, 4, 7, 8, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1},
+ {9, 2, 1, 9, 11, 2, 9, 4, 11, 7, 11, 4, 5, 10, 6, -1},
+ {8, 4, 7, 3, 11, 5, 3, 5, 1, 5, 11, 6, -1, -1, -1, -1},
+ {5, 1, 11, 5, 11, 6, 1, 0, 11, 7, 11, 4, 0, 4, 11, -1},
+ {0, 5, 9, 0, 6, 5, 0, 3, 6, 11, 6, 3, 8, 4, 7, -1},
+ {6, 5, 9, 6, 9, 11, 4, 7, 9, 7, 11, 9, -1, -1, -1, -1},
+ {10, 4, 9, 6, 4, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 10, 6, 4, 9, 10, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1},
+ {10, 0, 1, 10, 6, 0, 6, 4, 0, -1, -1, -1, -1, -1, -1, -1},
+ {8, 3, 1, 8, 1, 6, 8, 6, 4, 6, 1, 10, -1, -1, -1, -1},
+ {1, 4, 9, 1, 2, 4, 2, 6, 4, -1, -1, -1, -1, -1, -1, -1},
+ {3, 0, 8, 1, 2, 9, 2, 4, 9, 2, 6, 4, -1, -1, -1, -1},
+ {0, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {8, 3, 2, 8, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1},
+ {10, 4, 9, 10, 6, 4, 11, 2, 3, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 2, 2, 8, 11, 4, 9, 10, 4, 10, 6, -1, -1, -1, -1},
+ {3, 11, 2, 0, 1, 6, 0, 6, 4, 6, 1, 10, -1, -1, -1, -1},
+ {6, 4, 1, 6, 1, 10, 4, 8, 1, 2, 1, 11, 8, 11, 1, -1},
+ {9, 6, 4, 9, 3, 6, 9, 1, 3, 11, 6, 3, -1, -1, -1, -1},
+ {8, 11, 1, 8, 1, 0, 11, 6, 1, 9, 1, 4, 6, 4, 1, -1},
+ {3, 11, 6, 3, 6, 0, 0, 6, 4, -1, -1, -1, -1, -1, -1, -1},
+ {6, 4, 8, 11, 6, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {7, 10, 6, 7, 8, 10, 8, 9, 10, -1, -1, -1, -1, -1, -1, -1},
+ {0, 7, 3, 0, 10, 7, 0, 9, 10, 6, 7, 10, -1, -1, -1, -1},
+ {10, 6, 7, 1, 10, 7, 1, 7, 8, 1, 8, 0, -1, -1, -1, -1},
+ {10, 6, 7, 10, 7, 1, 1, 7, 3, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 6, 1, 6, 8, 1, 8, 9, 8, 6, 7, -1, -1, -1, -1},
+ {2, 6, 9, 2, 9, 1, 6, 7, 9, 0, 9, 3, 7, 3, 9, -1},
+ {7, 8, 0, 7, 0, 6, 6, 0, 2, -1, -1, -1, -1, -1, -1, -1},
+ {7, 3, 2, 6, 7, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {2, 3, 11, 10, 6, 8, 10, 8, 9, 8, 6, 7, -1, -1, -1, -1},
+ {2, 0, 7, 2, 7, 11, 0, 9, 7, 6, 7, 10, 9, 10, 7, -1},
+ {1, 8, 0, 1, 7, 8, 1, 10, 7, 6, 7, 10, 2, 3, 11, -1},
+ {11, 2, 1, 11, 1, 7, 10, 6, 1, 6, 7, 1, -1, -1, -1, -1},
+ {8, 9, 6, 8, 6, 7, 9, 1, 6, 11, 6, 3, 1, 3, 6, -1},
+ {0, 9, 1, 11, 6, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {7, 8, 0, 7, 0, 6, 3, 11, 0, 11, 6, 0, -1, -1, -1, -1},
+ {7, 11, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {3, 0, 8, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 1, 9, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {8, 1, 9, 8, 3, 1, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1},
+ {10, 1, 2, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, 3, 0, 8, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1},
+ {2, 9, 0, 2, 10, 9, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1},
+ {6, 11, 7, 2, 10, 3, 10, 8, 3, 10, 9, 8, -1, -1, -1, -1},
+ {7, 2, 3, 6, 2, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {7, 0, 8, 7, 6, 0, 6, 2, 0, -1, -1, -1, -1, -1, -1, -1},
+ {2, 7, 6, 2, 3, 7, 0, 1, 9, -1, -1, -1, -1, -1, -1, -1},
+ {1, 6, 2, 1, 8, 6, 1, 9, 8, 8, 7, 6, -1, -1, -1, -1},
+ {10, 7, 6, 10, 1, 7, 1, 3, 7, -1, -1, -1, -1, -1, -1, -1},
+ {10, 7, 6, 1, 7, 10, 1, 8, 7, 1, 0, 8, -1, -1, -1, -1},
+ {0, 3, 7, 0, 7, 10, 0, 10, 9, 6, 10, 7, -1, -1, -1, -1},
+ {7, 6, 10, 7, 10, 8, 8, 10, 9, -1, -1, -1, -1, -1, -1, -1},
+ {6, 8, 4, 11, 8, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {3, 6, 11, 3, 0, 6, 0, 4, 6, -1, -1, -1, -1, -1, -1, -1},
+ {8, 6, 11, 8, 4, 6, 9, 0, 1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 4, 6, 9, 6, 3, 9, 3, 1, 11, 3, 6, -1, -1, -1, -1},
+ {6, 8, 4, 6, 11, 8, 2, 10, 1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, 3, 0, 11, 0, 6, 11, 0, 4, 6, -1, -1, -1, -1},
+ {4, 11, 8, 4, 6, 11, 0, 2, 9, 2, 10, 9, -1, -1, -1, -1},
+ {10, 9, 3, 10, 3, 2, 9, 4, 3, 11, 3, 6, 4, 6, 3, -1},
+ {8, 2, 3, 8, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1},
+ {0, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 9, 0, 2, 3, 4, 2, 4, 6, 4, 3, 8, -1, -1, -1, -1},
+ {1, 9, 4, 1, 4, 2, 2, 4, 6, -1, -1, -1, -1, -1, -1, -1},
+ {8, 1, 3, 8, 6, 1, 8, 4, 6, 6, 10, 1, -1, -1, -1, -1},
+ {10, 1, 0, 10, 0, 6, 6, 0, 4, -1, -1, -1, -1, -1, -1, -1},
+ {4, 6, 3, 4, 3, 8, 6, 10, 3, 0, 3, 9, 10, 9, 3, -1},
+ {10, 9, 4, 6, 10, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 9, 5, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, 4, 9, 5, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1},
+ {5, 0, 1, 5, 4, 0, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1},
+ {11, 7, 6, 8, 3, 4, 3, 5, 4, 3, 1, 5, -1, -1, -1, -1},
+ {9, 5, 4, 10, 1, 2, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1},
+ {6, 11, 7, 1, 2, 10, 0, 8, 3, 4, 9, 5, -1, -1, -1, -1},
+ {7, 6, 11, 5, 4, 10, 4, 2, 10, 4, 0, 2, -1, -1, -1, -1},
+ {3, 4, 8, 3, 5, 4, 3, 2, 5, 10, 5, 2, 11, 7, 6, -1},
+ {7, 2, 3, 7, 6, 2, 5, 4, 9, -1, -1, -1, -1, -1, -1, -1},
+ {9, 5, 4, 0, 8, 6, 0, 6, 2, 6, 8, 7, -1, -1, -1, -1},
+ {3, 6, 2, 3, 7, 6, 1, 5, 0, 5, 4, 0, -1, -1, -1, -1},
+ {6, 2, 8, 6, 8, 7, 2, 1, 8, 4, 8, 5, 1, 5, 8, -1},
+ {9, 5, 4, 10, 1, 6, 1, 7, 6, 1, 3, 7, -1, -1, -1, -1},
+ {1, 6, 10, 1, 7, 6, 1, 0, 7, 8, 7, 0, 9, 5, 4, -1},
+ {4, 0, 10, 4, 10, 5, 0, 3, 10, 6, 10, 7, 3, 7, 10, -1},
+ {7, 6, 10, 7, 10, 8, 5, 4, 10, 4, 8, 10, -1, -1, -1, -1},
+ {6, 9, 5, 6, 11, 9, 11, 8, 9, -1, -1, -1, -1, -1, -1, -1},
+ {3, 6, 11, 0, 6, 3, 0, 5, 6, 0, 9, 5, -1, -1, -1, -1},
+ {0, 11, 8, 0, 5, 11, 0, 1, 5, 5, 6, 11, -1, -1, -1, -1},
+ {6, 11, 3, 6, 3, 5, 5, 3, 1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, 9, 5, 11, 9, 11, 8, 11, 5, 6, -1, -1, -1, -1},
+ {0, 11, 3, 0, 6, 11, 0, 9, 6, 5, 6, 9, 1, 2, 10, -1},
+ {11, 8, 5, 11, 5, 6, 8, 0, 5, 10, 5, 2, 0, 2, 5, -1},
+ {6, 11, 3, 6, 3, 5, 2, 10, 3, 10, 5, 3, -1, -1, -1, -1},
+ {5, 8, 9, 5, 2, 8, 5, 6, 2, 3, 8, 2, -1, -1, -1, -1},
+ {9, 5, 6, 9, 6, 0, 0, 6, 2, -1, -1, -1, -1, -1, -1, -1},
+ {1, 5, 8, 1, 8, 0, 5, 6, 8, 3, 8, 2, 6, 2, 8, -1},
+ {1, 5, 6, 2, 1, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 3, 6, 1, 6, 10, 3, 8, 6, 5, 6, 9, 8, 9, 6, -1},
+ {10, 1, 0, 10, 0, 6, 9, 5, 0, 5, 6, 0, -1, -1, -1, -1},
+ {0, 3, 8, 5, 6, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {10, 5, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {11, 5, 10, 7, 5, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {11, 5, 10, 11, 7, 5, 8, 3, 0, -1, -1, -1, -1, -1, -1, -1},
+ {5, 11, 7, 5, 10, 11, 1, 9, 0, -1, -1, -1, -1, -1, -1, -1},
+ {10, 7, 5, 10, 11, 7, 9, 8, 1, 8, 3, 1, -1, -1, -1, -1},
+ {11, 1, 2, 11, 7, 1, 7, 5, 1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, 1, 2, 7, 1, 7, 5, 7, 2, 11, -1, -1, -1, -1},
+ {9, 7, 5, 9, 2, 7, 9, 0, 2, 2, 11, 7, -1, -1, -1, -1},
+ {7, 5, 2, 7, 2, 11, 5, 9, 2, 3, 2, 8, 9, 8, 2, -1},
+ {2, 5, 10, 2, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1},
+ {8, 2, 0, 8, 5, 2, 8, 7, 5, 10, 2, 5, -1, -1, -1, -1},
+ {9, 0, 1, 5, 10, 3, 5, 3, 7, 3, 10, 2, -1, -1, -1, -1},
+ {9, 8, 2, 9, 2, 1, 8, 7, 2, 10, 2, 5, 7, 5, 2, -1},
+ {1, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 7, 0, 7, 1, 1, 7, 5, -1, -1, -1, -1, -1, -1, -1},
+ {9, 0, 3, 9, 3, 5, 5, 3, 7, -1, -1, -1, -1, -1, -1, -1},
+ {9, 8, 7, 5, 9, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {5, 8, 4, 5, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1},
+ {5, 0, 4, 5, 11, 0, 5, 10, 11, 11, 3, 0, -1, -1, -1, -1},
+ {0, 1, 9, 8, 4, 10, 8, 10, 11, 10, 4, 5, -1, -1, -1, -1},
+ {10, 11, 4, 10, 4, 5, 11, 3, 4, 9, 4, 1, 3, 1, 4, -1},
+ {2, 5, 1, 2, 8, 5, 2, 11, 8, 4, 5, 8, -1, -1, -1, -1},
+ {0, 4, 11, 0, 11, 3, 4, 5, 11, 2, 11, 1, 5, 1, 11, -1},
+ {0, 2, 5, 0, 5, 9, 2, 11, 5, 4, 5, 8, 11, 8, 5, -1},
+ {9, 4, 5, 2, 11, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {2, 5, 10, 3, 5, 2, 3, 4, 5, 3, 8, 4, -1, -1, -1, -1},
+ {5, 10, 2, 5, 2, 4, 4, 2, 0, -1, -1, -1, -1, -1, -1, -1},
+ {3, 10, 2, 3, 5, 10, 3, 8, 5, 4, 5, 8, 0, 1, 9, -1},
+ {5, 10, 2, 5, 2, 4, 1, 9, 2, 9, 4, 2, -1, -1, -1, -1},
+ {8, 4, 5, 8, 5, 3, 3, 5, 1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 4, 5, 1, 0, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {8, 4, 5, 8, 5, 3, 9, 0, 5, 0, 3, 5, -1, -1, -1, -1},
+ {9, 4, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 11, 7, 4, 9, 11, 9, 10, 11, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, 4, 9, 7, 9, 11, 7, 9, 10, 11, -1, -1, -1, -1},
+ {1, 10, 11, 1, 11, 4, 1, 4, 0, 7, 4, 11, -1, -1, -1, -1},
+ {3, 1, 4, 3, 4, 8, 1, 10, 4, 7, 4, 11, 10, 11, 4, -1},
+ {4, 11, 7, 9, 11, 4, 9, 2, 11, 9, 1, 2, -1, -1, -1, -1},
+ {9, 7, 4, 9, 11, 7, 9, 1, 11, 2, 11, 1, 0, 8, 3, -1},
+ {11, 7, 4, 11, 4, 2, 2, 4, 0, -1, -1, -1, -1, -1, -1, -1},
+ {11, 7, 4, 11, 4, 2, 8, 3, 4, 3, 2, 4, -1, -1, -1, -1},
+ {2, 9, 10, 2, 7, 9, 2, 3, 7, 7, 4, 9, -1, -1, -1, -1},
+ {9, 10, 7, 9, 7, 4, 10, 2, 7, 8, 7, 0, 2, 0, 7, -1},
+ {3, 7, 10, 3, 10, 2, 7, 4, 10, 1, 10, 0, 4, 0, 10, -1},
+ {1, 10, 2, 8, 7, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 9, 1, 4, 1, 7, 7, 1, 3, -1, -1, -1, -1, -1, -1, -1},
+ {4, 9, 1, 4, 1, 7, 0, 8, 1, 8, 7, 1, -1, -1, -1, -1},
+ {4, 0, 3, 7, 4, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 8, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {3, 0, 9, 3, 9, 11, 11, 9, 10, -1, -1, -1, -1, -1, -1, -1},
+ {0, 1, 10, 0, 10, 8, 8, 10, 11, -1, -1, -1, -1, -1, -1, -1},
+ {3, 1, 10, 11, 3, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 11, 1, 11, 9, 9, 11, 8, -1, -1, -1, -1, -1, -1, -1},
+ {3, 0, 9, 3, 9, 11, 1, 2, 9, 2, 11, 9, -1, -1, -1, -1},
+ {0, 2, 11, 8, 0, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {3, 2, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {2, 3, 8, 2, 8, 10, 10, 8, 9, -1, -1, -1, -1, -1, -1, -1},
+ {9, 10, 2, 0, 9, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {2, 3, 8, 2, 8, 10, 0, 1, 8, 1, 10, 8, -1, -1, -1, -1},
+ {1, 10, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 3, 8, 9, 1, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 9, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 3, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}
+};
+
+// number of vertices for each case above
+const int numVertsTable[256] =
+{
+ 0,
+ 3,
+ 3,
+ 6,
+ 3,
+ 6,
+ 6,
+ 9,
+ 3,
+ 6,
+ 6,
+ 9,
+ 6,
+ 9,
+ 9,
+ 6,
+ 3,
+ 6,
+ 6,
+ 9,
+ 6,
+ 9,
+ 9,
+ 12,
+ 6,
+ 9,
+ 9,
+ 12,
+ 9,
+ 12,
+ 12,
+ 9,
+ 3,
+ 6,
+ 6,
+ 9,
+ 6,
+ 9,
+ 9,
+ 12,
+ 6,
+ 9,
+ 9,
+ 12,
+ 9,
+ 12,
+ 12,
+ 9,
+ 6,
+ 9,
+ 9,
+ 6,
+ 9,
+ 12,
+ 12,
+ 9,
+ 9,
+ 12,
+ 12,
+ 9,
+ 12,
+ 15,
+ 15,
+ 6,
+ 3,
+ 6,
+ 6,
+ 9,
+ 6,
+ 9,
+ 9,
+ 12,
+ 6,
+ 9,
+ 9,
+ 12,
+ 9,
+ 12,
+ 12,
+ 9,
+ 6,
+ 9,
+ 9,
+ 12,
+ 9,
+ 12,
+ 12,
+ 15,
+ 9,
+ 12,
+ 12,
+ 15,
+ 12,
+ 15,
+ 15,
+ 12,
+ 6,
+ 9,
+ 9,
+ 12,
+ 9,
+ 12,
+ 6,
+ 9,
+ 9,
+ 12,
+ 12,
+ 15,
+ 12,
+ 15,
+ 9,
+ 6,
+ 9,
+ 12,
+ 12,
+ 9,
+ 12,
+ 15,
+ 9,
+ 6,
+ 12,
+ 15,
+ 15,
+ 12,
+ 15,
+ 6,
+ 12,
+ 3,
+ 3,
+ 6,
+ 6,
+ 9,
+ 6,
+ 9,
+ 9,
+ 12,
+ 6,
+ 9,
+ 9,
+ 12,
+ 9,
+ 12,
+ 12,
+ 9,
+ 6,
+ 9,
+ 9,
+ 12,
+ 9,
+ 12,
+ 12,
+ 15,
+ 9,
+ 6,
+ 12,
+ 9,
+ 12,
+ 9,
+ 15,
+ 6,
+ 6,
+ 9,
+ 9,
+ 12,
+ 9,
+ 12,
+ 12,
+ 15,
+ 9,
+ 12,
+ 12,
+ 15,
+ 12,
+ 15,
+ 15,
+ 12,
+ 9,
+ 12,
+ 12,
+ 9,
+ 12,
+ 15,
+ 15,
+ 12,
+ 12,
+ 9,
+ 15,
+ 6,
+ 15,
+ 12,
+ 6,
+ 3,
+ 6,
+ 9,
+ 9,
+ 12,
+ 9,
+ 12,
+ 12,
+ 15,
+ 9,
+ 12,
+ 12,
+ 15,
+ 6,
+ 9,
+ 9,
+ 6,
+ 9,
+ 12,
+ 12,
+ 15,
+ 12,
+ 15,
+ 15,
+ 6,
+ 12,
+ 9,
+ 15,
+ 12,
+ 9,
+ 6,
+ 12,
+ 3,
+ 9,
+ 12,
+ 12,
+ 15,
+ 12,
+ 15,
+ 9,
+ 12,
+ 12,
+ 15,
+ 15,
+ 6,
+ 9,
+ 12,
+ 6,
+ 3,
+ 6,
+ 9,
+ 9,
+ 6,
+ 9,
+ 12,
+ 6,
+ 3,
+ 9,
+ 6,
+ 12,
+ 3,
+ 6,
+ 3,
+ 3,
+ 0,
+};
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/gpu/kinfu_large_scale/raycaster.h>
+#include <pcl/gpu/kinfu_large_scale/tsdf_volume.h>
+#include "internal.h"
+
+using namespace pcl;
+using namespace Eigen;
+//using namespace pcl::gpu::kinfuLS;
+//using namespace pcl::device::kinfuLS;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+pcl::gpu::kinfuLS::RayCaster::RayCaster(int rows_arg, int cols_arg, float fx, float fy, float cx, float cy)
+ : cols(cols_arg), rows(rows_arg), fx_(fx), fy_(fy), cx_(cx < 0 ? cols/2 : cx), cy_(cy < 0 ? rows/2 : cy)
+{
+ vertex_map_.create(rows * 3, cols);
+ normal_map_.create(rows * 3, cols);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+pcl::gpu::kinfuLS::RayCaster::~RayCaster()
+{
+
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::kinfuLS::RayCaster::setIntrinsics(float fx, float fy, float cx, float cy)
+{
+ fx_ = fx;
+ fy_ = fy;
+ cx_ = cx < 0 ? cols/2 : cx;
+ cy_ = cy < 0 ? rows/2 : cy;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::kinfuLS::RayCaster::run(const TsdfVolume& volume, const Affine3f& camera_pose, tsdf_buffer* buffer)
+{
+ camera_pose_.linear() = camera_pose.linear();
+ camera_pose_.translation() = camera_pose.translation();
+ volume_size_ = volume.getSize();
+ pcl::device::kinfuLS::Intr intr (fx_, fy_, cx_, cy_);
+
+ vertex_map_.create(rows * 3, cols);
+ normal_map_.create(rows * 3, cols);
+
+ typedef Matrix<float, 3, 3, RowMajor> Matrix3f;
+
+ Matrix3f R = camera_pose_.linear();
+ Vector3f t = camera_pose_.translation();
+
+ const pcl::device::kinfuLS::Mat33& device_R = pcl::device::kinfuLS::device_cast<const pcl::device::kinfuLS::Mat33>(R);
+ // const float3& device_t = device_cast<const float3>(t);
+
+ float3& device_t = pcl::device::kinfuLS::device_cast<float3>(t);
+
+ device_t.x -= buffer->origin_metric.x;
+ device_t.y -= buffer->origin_metric.y;
+ device_t.z -= buffer->origin_metric.z;
+
+ float tranc_dist = volume.getTsdfTruncDist();
+ pcl::device::kinfuLS::raycast (intr, device_R, device_t, tranc_dist, pcl::device::kinfuLS::device_cast<const float3>(volume_size_), volume.data(), buffer, vertex_map_, normal_map_);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::kinfuLS::RayCaster::generateSceneView(View& view) const
+{
+ generateSceneView(view, volume_size_ * (-3.f));
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::kinfuLS::RayCaster::generateSceneView(View& view, const Vector3f& light_source_pose) const
+{
+ pcl::device::kinfuLS::LightSource light;
+ light.number = 1;
+ light.pos[0] = pcl::device::kinfuLS::device_cast<const float3>(light_source_pose);
+
+ view.create(rows, cols);
+ pcl::device::kinfuLS::generateImage (vertex_map_, normal_map_, light, view);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::gpu::kinfuLS::RayCaster::generateDepthImage(Depth& depth) const
+{
+ pcl::device::kinfuLS::Intr intr (fx_, fy_, cx_, cy_);
+
+ depth.create(rows, cols);
+
+ Matrix<float, 3, 3, RowMajor> R_inv = camera_pose_.linear().inverse();
+ Vector3f t = camera_pose_.translation();
+
+ pcl::device::kinfuLS::generateDepth(pcl::device::kinfuLS::device_cast<pcl::device::kinfuLS::Mat33>(R_inv), pcl::device::kinfuLS::device_cast<const float3>(t), vertex_map_, depth);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::gpu::kinfuLS::RayCaster::MapArr
+pcl::gpu::kinfuLS::RayCaster::getVertexMap() const
+{
+ return vertex_map_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::gpu::kinfuLS::RayCaster::MapArr
+pcl::gpu::kinfuLS::RayCaster::getNormalMap() const
+{
+ return normal_map_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace kinfuLS
+ {
+ template<> PCL_EXPORTS void
+ convertMapToOranizedCloud<pcl::PointXYZ>(const RayCaster::MapArr& map, DeviceArray2D<pcl::PointXYZ>& cloud)
+ {
+ cloud.create (map.rows()/3, map.cols());
+ DeviceArray2D<float4>& c = (DeviceArray2D<float4>&)cloud;
+ pcl::device::kinfuLS::convert (map, c);
+ }
+
+ template<> PCL_EXPORTS void
+ convertMapToOranizedCloud<pcl::Normal> (const RayCaster::MapArr& map, DeviceArray2D<pcl::Normal>& cloud)
+ {
+ cloud.create (map.rows()/3, map.cols());
+ DeviceArray2D<pcl::device::kinfuLS::float8>& n = (DeviceArray2D<pcl::device::kinfuLS::float8>&)cloud;
+ pcl::device::kinfuLS::convert(map, n);
+ }
+ }
+ }
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Francisco Heredia, Technical University Eindhoven, (f.j.mysurname.soriano < aT > tue.nl)
+ */
+
+
+
+#ifndef PCL_SCREENSHOT_MANAGER_CPP_
+#define PCL_SCREENSHOT_MANAGER_CPP_
+#include <pcl/gpu/kinfu_large_scale/screenshot_manager.h>
+
+namespace pcl
+{
+ namespace kinfuLS
+ {
+ ScreenshotManager::ScreenshotManager()
+ {
+ boost::filesystem::path p ("KinFuSnapshots");
+ boost::filesystem::create_directory (p);
+ screenshot_counter = 0;
+ setCameraIntrinsics();
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ void
+ ScreenshotManager::saveImage(const Eigen::Affine3f &camPose, pcl::gpu::PtrStepSz<const PixelRGB> rgb24)
+ {
+
+ PCL_WARN ("[o] [o] [o] [o] Saving screenshot [o] [o] [o] [o]\n");
+
+ std::string file_extension_image = ".png";
+ std::string file_extension_pose = ".txt";
+ std::string filename_image = "KinFuSnapshots/";
+ std::string filename_pose = "KinFuSnapshots/";
+
+ // Get Pose
+ Eigen::Matrix<float, 3, 3, Eigen::RowMajor> erreMats = camPose.linear ();
+ Eigen::Vector3f teVecs = camPose.translation ();
+
+ // Create filenames
+ filename_pose = filename_pose + boost::lexical_cast<std::string> (screenshot_counter) + file_extension_pose;
+ filename_image = filename_image + boost::lexical_cast<std::string> (screenshot_counter) + file_extension_image;
+
+ // Write files
+ writePose (filename_pose, teVecs, erreMats);
+
+ // Save Image
+ pcl::io::saveRgbPNGFile (filename_image, (unsigned char*)rgb24.data, 640,480);
+
+ screenshot_counter++;
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ void
+ ScreenshotManager::setCameraIntrinsics (float focal, float height, float width)
+ {
+ focal_ = focal;
+ height_ = height;
+ width_ = width;
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ void
+ ScreenshotManager::writePose(const std::string &filename_pose, const Eigen::Vector3f &teVecs, const Eigen::Matrix<float, 3, 3, Eigen::RowMajor> &erreMats)
+ {
+ std::ofstream poseFile;
+ poseFile.open (filename_pose.c_str());
+
+ if (poseFile.is_open())
+ {
+ poseFile << "TVector" << std::endl << teVecs << std::endl << std::endl
+ << "RMatrix" << std::endl << erreMats << std::endl << std::endl
+ << "Camera Intrinsics: focal height width" << std::endl << focal_ << " " << height_ << " " << width_ << std::endl << std::endl;
+ poseFile.close ();
+ }
+ else
+ {
+ PCL_WARN ("Unable to open/create output file for camera pose!\n");
+ }
+ }
+
+ } // namespace kinfuLS
+} //namespace pcl
+
+#endif // PCL_SCREENSHOT_MANAGER_CPP_
--- /dev/null
+ /*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/point_types.h>
+#include <pcl/impl/instantiate.hpp>
+#include <pcl/gpu/kinfu_large_scale/standalone_marching_cubes.h>
+#include <pcl/gpu/kinfu_large_scale/impl/standalone_marching_cubes.hpp>
+
+//~ PCL_INSTANTIATE(StandaloneMarchingCubes, (pcl::PointXYZI));
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/gpu/kinfu_large_scale/tsdf_volume.h>
+#include "internal.h"
+#include <algorithm>
+#include <Eigen/Core>
+
+#include <iostream>
+
+using namespace pcl;
+using namespace pcl::gpu;
+using namespace Eigen;
+using pcl::device::kinfuLS::device_cast;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+pcl::gpu::kinfuLS::TsdfVolume::TsdfVolume(const Vector3i& resolution) : resolution_(resolution), volume_host_ (new std::vector<float>), weights_host_ (new std::vector<short>)
+{
+ int volume_x = resolution_(0);
+ int volume_y = resolution_(1);
+ int volume_z = resolution_(2);
+
+ volume_.create (volume_y * volume_z, volume_x);
+
+ const Vector3f default_volume_size = Vector3f::Constant (3.f); //meters
+ const float default_tranc_dist = 0.03f; //meters
+
+ setSize(default_volume_size);
+ setTsdfTruncDist(default_tranc_dist);
+
+ reset();
+
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::kinfuLS::TsdfVolume::setSize(const Vector3f& size)
+{
+ size_ = size;
+ setTsdfTruncDist(tranc_dist_);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::kinfuLS::TsdfVolume::setTsdfTruncDist (float distance)
+{
+ float cx = size_(0) / resolution_(0);
+ float cy = size_(1) / resolution_(1);
+ float cz = size_(2) / resolution_(2);
+
+ tranc_dist_ = std::max (distance, 2.1f * std::max (cx, std::max (cy, cz)));
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+pcl::gpu::DeviceArray2D<int>
+pcl::gpu::kinfuLS::TsdfVolume::data() const
+{
+ return volume_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+const Eigen::Vector3f&
+pcl::gpu::kinfuLS::TsdfVolume::getSize() const
+{
+ return size_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+const Eigen::Vector3i&
+pcl::gpu::kinfuLS::TsdfVolume::getResolution() const
+{
+ return resolution_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+const Eigen::Vector3f
+pcl::gpu::kinfuLS::TsdfVolume::getVoxelSize() const
+{
+ return size_.array () / resolution_.array().cast<float>();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+float
+pcl::gpu::kinfuLS::TsdfVolume::getTsdfTruncDist () const
+{
+ return tranc_dist_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::kinfuLS::TsdfVolume::reset()
+{
+ pcl::device::kinfuLS::initVolume(volume_);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::kinfuLS::TsdfVolume::fetchCloudHost (PointCloud<PointXYZI>& cloud, bool connected26) const
+{
+ PointCloud<PointXYZ>::Ptr cloud_ptr_ = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ>);
+ PointCloud<PointIntensity>::Ptr cloud_i_ptr_ = PointCloud<PointIntensity>::Ptr (new PointCloud<PointIntensity>);
+ fetchCloudHost(*cloud_ptr_);
+ pcl::concatenateFields (*cloud_ptr_, *cloud_i_ptr_, cloud);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::kinfuLS::TsdfVolume::fetchCloudHost (PointCloud<PointType>& cloud, bool connected26) const
+{
+ int volume_x = resolution_(0);
+ int volume_y = resolution_(1);
+ int volume_z = resolution_(2);
+
+ int cols;
+ std::vector<int> volume_host;
+ volume_.download (volume_host, cols);
+
+ cloud.points.clear ();
+ cloud.points.reserve (10000);
+
+ const int DIVISOR = pcl::device::kinfuLS::DIVISOR; // SHRT_MAX;
+
+#define FETCH(x, y, z) volume_host[(x) + (y) * volume_x + (z) * volume_y * volume_x]
+
+ Array3f cell_size = getVoxelSize();
+
+ for (int x = 1; x < volume_x-1; ++x)
+ {
+ for (int y = 1; y < volume_y-1; ++y)
+ {
+ for (int z = 0; z < volume_z-1; ++z)
+ {
+ int tmp = FETCH (x, y, z);
+ int W = reinterpret_cast<short2*>(&tmp)->y;
+ int F = reinterpret_cast<short2*>(&tmp)->x;
+
+ if (W == 0 || F == DIVISOR)
+ continue;
+
+ Vector3f V = ((Array3f(x, y, z) + 0.5f) * cell_size).matrix ();
+
+ if (connected26)
+ {
+ int dz = 1;
+ for (int dy = -1; dy < 2; ++dy)
+ for (int dx = -1; dx < 2; ++dx)
+ {
+ int tmp = FETCH (x+dx, y+dy, z+dz);
+
+ int Wn = reinterpret_cast<short2*>(&tmp)->y;
+ int Fn = reinterpret_cast<short2*>(&tmp)->x;
+ if (Wn == 0 || Fn == DIVISOR)
+ continue;
+
+ if ((F > 0 && Fn < 0) || (F < 0 && Fn > 0))
+ {
+ Vector3f Vn = ((Array3f (x+dx, y+dy, z+dz) + 0.5f) * cell_size).matrix ();
+ Vector3f point = (V * abs (Fn) + Vn * abs (F)) / (abs (F) + abs (Fn));
+
+ pcl::PointXYZ xyz;
+ xyz.x = point (0);
+ xyz.y = point (1);
+ xyz.z = point (2);
+
+ cloud.points.push_back (xyz);
+ }
+ }
+ dz = 0;
+ for (int dy = 0; dy < 2; ++dy)
+ for (int dx = -1; dx < dy * 2; ++dx)
+ {
+ int tmp = FETCH (x+dx, y+dy, z+dz);
+
+ int Wn = reinterpret_cast<short2*>(&tmp)->y;
+ int Fn = reinterpret_cast<short2*>(&tmp)->x;
+ if (Wn == 0 || Fn == DIVISOR)
+ continue;
+
+ if ((F > 0 && Fn < 0) || (F < 0 && Fn > 0))
+ {
+ Vector3f Vn = ((Array3f (x+dx, y+dy, z+dz) + 0.5f) * cell_size).matrix ();
+ Vector3f point = (V * abs(Fn) + Vn * abs(F))/(abs(F) + abs (Fn));
+
+ pcl::PointXYZ xyz;
+ xyz.x = point (0);
+ xyz.y = point (1);
+ xyz.z = point (2);
+
+ cloud.points.push_back (xyz);
+ }
+ }
+ }
+ else /* if (connected26) */
+ {
+ for (int i = 0; i < 3; ++i)
+ {
+ int ds[] = {0, 0, 0};
+ ds[i] = 1;
+
+ int dx = ds[0];
+ int dy = ds[1];
+ int dz = ds[2];
+
+ int tmp = FETCH (x+dx, y+dy, z+dz);
+
+ int Wn = reinterpret_cast<short2*>(&tmp)->y;
+ int Fn = reinterpret_cast<short2*>(&tmp)->x;
+ if (Wn == 0 || Fn == DIVISOR)
+ continue;
+
+ if ((F > 0 && Fn < 0) || (F < 0 && Fn > 0))
+ {
+ Vector3f Vn = ((Array3f (x+dx, y+dy, z+dz) + 0.5f) * cell_size).matrix ();
+ Vector3f point = (V * abs (Fn) + Vn * abs (F)) / (abs (F) + abs (Fn));
+
+ pcl::PointXYZ xyz;
+ xyz.x = point (0);
+ xyz.y = point (1);
+ xyz.z = point (2);
+
+ cloud.points.push_back (xyz);
+ }
+ }
+ } /* if (connected26) */
+ }
+ }
+ }
+#undef FETCH
+ cloud.width = (int)cloud.points.size ();
+ cloud.height = 1;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::gpu::DeviceArray<pcl::gpu::kinfuLS::TsdfVolume::PointType>
+pcl::gpu::kinfuLS::TsdfVolume::fetchCloud (DeviceArray<PointType>& cloud_buffer) const
+{
+ if (cloud_buffer.empty ())
+ cloud_buffer.create (DEFAULT_CLOUD_BUFFER_SIZE);
+
+ float3 device_volume_size = device_cast<const float3> (size_);
+ size_t size = pcl::device::kinfuLS::extractCloud (volume_, device_volume_size, cloud_buffer);
+ return (DeviceArray<PointType> (cloud_buffer.ptr (), size));
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::kinfuLS::TsdfVolume::fetchNormals (const DeviceArray<PointType>& cloud, DeviceArray<PointType>& normals) const
+{
+ normals.create (cloud.size ());
+ const float3 device_volume_size = device_cast<const float3> (size_);
+ pcl::device::kinfuLS::extractNormals (volume_, device_volume_size, cloud, (pcl::device::kinfuLS::PointType*)normals.ptr ());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::kinfuLS::TsdfVolume::pushSlice (PointCloud<PointXYZI>::Ptr existing_data_cloud, const pcl::gpu::kinfuLS::tsdf_buffer* buffer) const
+{
+ size_t gpu_array_size = existing_data_cloud->points.size ();
+
+ if(gpu_array_size == 0)
+ {
+ //std::cout << "[KinfuTracker](pushSlice) Existing data cloud has no points\n";//CREATE AS PCL MESSAGE
+ return;
+ }
+
+ const pcl::PointXYZI *first_point_ptr = &(existing_data_cloud->points[0]);
+
+ pcl::gpu::DeviceArray<pcl::PointXYZI> cloud_gpu;
+ cloud_gpu.upload (first_point_ptr, gpu_array_size);
+
+ DeviceArray<float4>& cloud_cast = (DeviceArray<float4>&) cloud_gpu;
+ //volume().pushCloudAsSlice (cloud_cast, &buffer_);
+ pcl::device::kinfuLS::pushCloudAsSliceGPU (volume_, cloud_cast, buffer);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+size_t
+pcl::gpu::kinfuLS::TsdfVolume::fetchSliceAsCloud (DeviceArray<PointType>& cloud_buffer_xyz, DeviceArray<float>& cloud_buffer_intensity, const pcl::gpu::kinfuLS::tsdf_buffer* buffer, int shiftX, int shiftY, int shiftZ ) const
+{
+ if (cloud_buffer_xyz.empty ())
+ cloud_buffer_xyz.create (DEFAULT_CLOUD_BUFFER_SIZE/2);
+
+ if (cloud_buffer_intensity.empty ()) {
+ cloud_buffer_intensity.create (DEFAULT_CLOUD_BUFFER_SIZE/2);
+ }
+
+ float3 device_volume_size = device_cast<const float3> (size_);
+
+ size_t size = pcl::device::kinfuLS::extractSliceAsCloud (volume_, device_volume_size, buffer, shiftX, shiftY, shiftZ, cloud_buffer_xyz, cloud_buffer_intensity);
+
+ std::cout << " SIZE IS " << size << std::endl;
+
+ return (size);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::kinfuLS::TsdfVolume::fetchNormals (const DeviceArray<PointType>& cloud, DeviceArray<NormalType>& normals) const
+{
+ normals.create (cloud.size ());
+ const float3 device_volume_size = device_cast<const float3> (size_);
+ pcl::device::kinfuLS::extractNormals (volume_, device_volume_size, cloud, (pcl::device::kinfuLS::float8*)normals.ptr ());
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::kinfuLS::TsdfVolume::convertToTsdfCloud (pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) const
+{
+ int sx = header_.resolution(0);
+ int sy = header_.resolution(1);
+ int sz = header_.resolution(2);
+
+ const int step = 2;
+ const int cloud_size = static_cast<int> (header_.getVolumeSize() / (step*step*step));
+
+ cloud->clear();
+ cloud->reserve (std::min (cloud_size/10, 500000));
+
+ int volume_idx = 0, cloud_idx = 0;
+ // #pragma omp parallel for // if used, increment over idx not possible! use index calculation
+ for (int z = 0; z < sz; z+=step)
+ for (int y = 0; y < sy; y+=step)
+ for (int x = 0; x < sx; x+=step, ++cloud_idx)
+ {
+ volume_idx = sx*sy*z + sx*y + x;
+ // pcl::PointXYZI &point = cloud->points[cloud_idx];
+
+ if (weights_host_->at(volume_idx) == 0 || volume_host_->at(volume_idx) > 0.98 )
+ continue;
+
+ pcl::PointXYZI point;
+ point.x = x; point.y = y; point.z = z;//*64;
+ point.intensity = volume_host_->at(volume_idx);
+ cloud->push_back (point);
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::kinfuLS::TsdfVolume::downloadTsdf (std::vector<float>& tsdf) const
+{
+ tsdf.resize (volume_.cols() * volume_.rows());
+ volume_.download(&tsdf[0], volume_.cols() * sizeof(int));
+
+#pragma omp parallel for
+ for(int i = 0; i < (int) tsdf.size(); ++i)
+ {
+ float tmp = reinterpret_cast<short2*>(&tsdf[i])->x;
+ tsdf[i] = tmp/pcl::device::kinfuLS::DIVISOR;
+ }
+}
+
+void
+pcl::gpu::kinfuLS::TsdfVolume::downloadTsdfLocal () const
+{
+ pcl::gpu::kinfuLS::TsdfVolume::downloadTsdf (*volume_host_);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::kinfuLS::TsdfVolume::downloadTsdfAndWeights (std::vector<float>& tsdf, std::vector<short>& weights) const
+{
+ int volumeSize = volume_.cols() * volume_.rows();
+ tsdf.resize (volumeSize);
+ weights.resize (volumeSize);
+ volume_.download(&tsdf[0], volume_.cols() * sizeof(int));
+
+ #pragma omp parallel for
+ for(int i = 0; i < (int) tsdf.size(); ++i)
+ {
+ short2 elem = *reinterpret_cast<short2*>(&tsdf[i]);
+ tsdf[i] = (float)(elem.x)/pcl::device::kinfuLS::DIVISOR;
+ weights[i] = (short)(elem.y);
+ }
+}
+
+
+void
+pcl::gpu::kinfuLS::TsdfVolume::downloadTsdfAndWeightsLocal () const
+{
+ pcl::gpu::kinfuLS::TsdfVolume::downloadTsdfAndWeights (*volume_host_, *weights_host_);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+bool
+pcl::gpu::kinfuLS::TsdfVolume::save (const std::string &filename, bool binary) const
+{
+ pcl::console::print_info ("Saving TSDF volume to "); pcl::console::print_value ("%s ... ", filename.c_str());
+ std::cout << std::flush;
+
+ std::ofstream file (filename.c_str(), binary ? std::ios_base::binary : std::ios_base::out);
+
+ if (file.is_open())
+ {
+ if (binary)
+ {
+ // HEADER
+ // write resolution and size of volume
+ file.write ((char*) &header_, sizeof (Header));
+ /* file.write ((char*) &header_.resolution, sizeof(Eigen::Vector3i));
+ file.write ((char*) &header_.volume_size, sizeof(Eigen::Vector3f));
+ // write element size
+ int volume_element_size = sizeof(VolumeT);
+ file.write ((char*) &volume_element_size, sizeof(int));
+ int weights_element_size = sizeof(WeightT);
+ file.write ((char*) &weights_element_size, sizeof(int)); */
+
+ // DATA
+ // write data
+ file.write ((char*) &(volume_host_->at(0)), volume_host_->size()*sizeof(float));
+ file.write ((char*) &(weights_host_->at(0)), weights_host_->size()*sizeof(short));
+ }
+ else
+ {
+ // write resolution and size of volume and element size
+ file << header_.resolution(0) << " " << header_.resolution(1) << " " << header_.resolution(2) << std::endl;
+ file << header_.volume_size(0) << " " << header_.volume_size(1) << " " << header_.volume_size(2) << std::endl;
+ file << sizeof (float) << " " << sizeof(short) << std::endl;
+
+ // write data
+ for (std::vector<float>::const_iterator iter = volume_host_->begin(); iter != volume_host_->end(); ++iter)
+ file << *iter << std::endl;
+ }
+
+ file.close();
+ }
+ else
+ {
+ pcl::console::print_error ("[saveTsdfVolume] Error: Couldn't open file %s.\n", filename.c_str());
+ return false;
+ }
+
+ pcl::console::print_info ("done [%d voxels]\n", this->size ());
+
+ return true;
+}
+
+
+bool
+pcl::gpu::kinfuLS::TsdfVolume::load (const std::string &filename, bool binary)
+{
+ pcl::console::print_info ("Loading TSDF volume from "); pcl::console::print_value ("%s ... ", filename.c_str());
+ std::cout << std::flush;
+
+ std::ifstream file (filename.c_str());
+
+ if (file.is_open())
+ {
+ if (binary)
+ {
+ // read HEADER
+ file.read ((char*) &header_, sizeof (Header));
+ /* file.read (&header_.resolution, sizeof(Eigen::Array3i));
+ file.read (&header_.volume_size, sizeof(Eigen::Vector3f));
+ file.read (&header_.volume_element_size, sizeof(int));
+ file.read (&header_.weights_element_size, sizeof(int)); */
+
+ // check if element size fits to data
+ if (header_.volume_element_size != sizeof(float))
+ {
+ pcl::console::print_error ("[TSDFVolume::load] Error: Given volume element size (%d) doesn't fit data (%d)", sizeof(float), header_.volume_element_size);
+ return false;
+ }
+ if ( header_.weights_element_size != sizeof(short))
+ {
+ pcl::console::print_error ("[TSDFVolume::load] Error: Given weights element size (%d) doesn't fit data (%d)", sizeof(short), header_.weights_element_size);
+ return false;
+ }
+
+ // read DATA
+ int num_elements = header_.getVolumeSize();
+ volume_host_->resize (num_elements);
+ weights_host_->resize (num_elements);
+ file.read ((char*) &(*volume_host_)[0], num_elements * sizeof(float));
+ file.read ((char*) &(*weights_host_)[0], num_elements * sizeof(short));
+ }
+ else
+ {
+ pcl::console::print_error ("[TSDFVolume::load] Error: ASCII loading not implemented.\n");
+ }
+
+ file.close ();
+ }
+ else
+ {
+ pcl::console::print_error ("[TSDFVolume::load] Error: Cloudn't read file %s.\n", filename.c_str());
+ return false;
+ }
+
+ const Eigen::Vector3i &res = this->gridResolution();
+ pcl::console::print_info ("done [%d voxels, res %dx%dx%d]\n", this->size(), res[0], res[1], res[2]);
+
+ return true;
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Raphael Favier, Technical University Eindhoven, (r.mysurname <aT> tue.nl)
+ */
+
+ #include <pcl/gpu/kinfu_large_scale/world_model.h>
+ #include <pcl/gpu/kinfu_large_scale/impl/world_model.hpp>
+
+
+
+PCL_INSTANTIATE(WorldModel, (pcl::PointXYZ)(pcl::PointXYZI));
--- /dev/null
+if(WITH_OPENNI)
+ if(NOT VTK_FOUND)
+ set(DEFAULT FALSE)
+ set(REASON "VTK was not found.")
+ else(NOT VTK_FOUND)
+ set(DEFAULT TRUE)
+ set(REASON)
+ set(VTK_USE_FILE "${VTK_USE_FILE}" CACHE INTERNAL "VTK_USE_FILE")
+ include("${VTK_USE_FILE}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
+ endif(NOT VTK_FOUND)
+
+ FILE(GLOB hdrs "*.h*")
+ include_directories(${VTK_INCLUDE_DIRS})
+
+
+ ## STANDALONE TEXTURE MAPPING
+ set(the_target pcl_kinfu_largeScale_texture_output)
+ set(srcs standalone_texture_mapping.cpp )
+
+ source_group("Source Files" FILES ${srcs} )
+
+ PCL_ADD_EXECUTABLE_OPT_BUNDLE("${the_target}" "${SUBSYS_NAME}" ${srcs} ${hdrs})
+ target_link_libraries("${the_target}" pcl_common pcl_io ${OPENNI_LIBRARIES} pcl_visualization pcl_gpu_kinfu_large_scale pcl_kdtree pcl_features pcl_surface)
+
+ ## KINECT FUSION
+ set(the_target pcl_kinfu_largeScale)
+ set(srcs kinfuLS_app.cpp capture.cpp evaluation.cpp)
+
+ source_group("Source Files" FILES ${srcs} )
+
+ PCL_ADD_EXECUTABLE_OPT_BUNDLE("${the_target}" "${SUBSYS_NAME}" ${srcs} ${hdrs})
+ target_link_libraries("${the_target}" pcl_common pcl_io ${OPENNI_LIBRARIES} pcl_visualization pcl_gpu_kinfu_large_scale pcl_octree)
+
+ ## STANDALONE MARCHING CUBES
+ set(the_target pcl_kinfu_largeScale_mesh_output)
+ set(srcs process_kinfu_large_scale_output.cpp)
+
+ PCL_ADD_EXECUTABLE_OPT_BUNDLE("${the_target}" "${SUBSYS_NAME}" ${srcs} ${hdrs})
+ target_link_libraries("${the_target}" pcl_common pcl_io ${OPENNI_LIBRARIES} pcl_visualization pcl_gpu_kinfu_large_scale pcl_filters)
+
+ ## RECORD MAPS_RGB
+ set(the_target pcl_record_kinect_maps_rgb)
+ set(srcs record_maps_rgb.cpp)
+
+ PCL_ADD_EXECUTABLE_OPT_BUNDLE("${the_target}" "${SUBSYS_NAME}" ${srcs} ${hdrs})
+ target_link_libraries("${the_target}" pcl_common pcl_io ${OPENNI_LIBRARIES} pcl_visualization pcl_gpu_kinfu_large_scale pcl_filters)
+
+
+endif()
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include <pcl/io/openni_camera/openni.h>
+
+#include "openni_capture.h"
+#include <pcl/gpu/containers/initialization.h>
+
+using namespace std;
+using namespace pcl;
+using namespace pcl::gpu;
+using namespace xn;
+
+//const std::string XMLConfig =
+//"<OpenNI>"
+// "<Licenses>"
+// "<License vendor=\"PrimeSense\" key=\"0KOIk2JeIBYClPWVnMoRKn5cdY4=\"/>"
+// "</Licenses>"
+// "<Log writeToConsole=\"false\" writeToFile=\"false\">"
+// "<LogLevel value=\"3\"/>"
+// "<Masks>"
+// "<Mask name=\"ALL\" on=\"true\"/>"
+// "</Masks>"
+// "<Dumps>"
+// "</Dumps>"
+// "</Log>"
+// "<ProductionNodes>"
+// "<Node type=\"Image\" name=\"Image1\">"
+// "<Configuration>"
+// "<MapOutputMode xRes=\"640\" yRes=\"480\" FPS=\"30\"/>"
+// "<Mirror on=\"false\"/>"
+// "</Configuration>"
+// "</Node> "
+// "<Node type=\"Depth\" name=\"Depth1\">"
+// "<Configuration>"
+// "<MapOutputMode xRes=\"640\" yRes=\"480\" FPS=\"30\"/>"
+// "<Mirror on=\"false\"/>"
+// "</Configuration>"
+// "</Node>"
+// "</ProductionNodes>"
+//"</OpenNI>";
+
+#define REPORT_ERROR(msg) pcl::gpu::error ((msg), __FILE__, __LINE__)
+
+struct pcl::gpu::kinfuLS::CaptureOpenNI::Impl
+{
+ Context context;
+ ScriptNode scriptNode;
+ DepthGenerator depth;
+ ImageGenerator image;
+ ProductionNode node;
+ DepthMetaData depthMD;
+ ImageMetaData imageMD;
+ XnChar strError[1024];
+
+ bool has_depth;
+ bool has_image;
+};
+
+pcl::gpu::kinfuLS::CaptureOpenNI::CaptureOpenNI() : depth_focal_length_VGA (0.f), baseline (0.f), shadow_value (0), no_sample_value (0), pixelSize (0.0), max_depth (0) {}
+pcl::gpu::kinfuLS::CaptureOpenNI::CaptureOpenNI(int device) {open (device); }
+pcl::gpu::kinfuLS::CaptureOpenNI::CaptureOpenNI(const string& filename) {open (filename); }
+pcl::gpu::kinfuLS::CaptureOpenNI::~CaptureOpenNI() { release (); }
+
+void
+pcl::gpu::kinfuLS::CaptureOpenNI::open (int device)
+{
+ impl_.reset ( new Impl () );
+
+ XnMapOutputMode mode;
+ mode.nXRes = XN_VGA_X_RES;
+ mode.nYRes = XN_VGA_Y_RES;
+ mode.nFPS = 30;
+
+ XnStatus rc;
+ rc = impl_->context.Init ();
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "Init failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+
+ xn::NodeInfoList devicesList;
+ rc = impl_->context.EnumerateProductionTrees ( XN_NODE_TYPE_DEVICE, NULL, devicesList, 0 );
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "Init failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+
+ xn::NodeInfoList::Iterator it = devicesList.Begin ();
+ for (int i = 0; i < device; ++i)
+ it++;
+
+ NodeInfo node = *it;
+ rc = impl_->context.CreateProductionTree ( node, impl_->node );
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "Init failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+
+ XnLicense license;
+ const char* vendor = "PrimeSense";
+ const char* key = "0KOIk2JeIBYClPWVnMoRKn5cdY4=";
+ sprintf (license.strKey, key);
+ sprintf (license.strVendor, vendor);
+
+ rc = impl_->context.AddLicense (license);
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "licence failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+
+ rc = impl_->depth.Create (impl_->context);
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "Depth generator failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+ //rc = impl_->depth.SetIntProperty("HoleFilter", 1);
+ rc = impl_->depth.SetMapOutputMode (mode);
+ impl_->has_depth = true;
+
+ rc = impl_->image.Create (impl_->context);
+ if (rc != XN_STATUS_OK)
+ {
+ printf ("Image generator creation failed: %s\n", xnGetStatusString (rc));
+ impl_->has_image = false;
+ }
+ else
+ {
+ impl_->has_image = true;
+ rc = impl_->image.SetMapOutputMode (mode);
+ }
+
+ getParams ();
+
+ rc = impl_->context.StartGeneratingAll ();
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "Start failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+}
+
+void
+pcl::gpu::kinfuLS::CaptureOpenNI::open (const std::string& filename)
+{
+ impl_.reset ( new Impl () );
+
+ XnStatus rc;
+
+ rc = impl_->context.Init ();
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "Init failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+
+ rc = impl_->context.OpenFileRecording (filename.c_str (), impl_->node);
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "Open failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+
+ rc = impl_->context.FindExistingNode (XN_NODE_TYPE_DEPTH, impl_->depth);
+ impl_->has_depth = (rc == XN_STATUS_OK);
+
+ rc = impl_->context.FindExistingNode (XN_NODE_TYPE_IMAGE, impl_->image);
+ impl_->has_image = (rc == XN_STATUS_OK);
+
+ if (!impl_->has_depth)
+ REPORT_ERROR ("No depth nodes. Check your configuration");
+
+ if (impl_->has_depth)
+ impl_->depth.GetMetaData (impl_->depthMD);
+
+ if (impl_->has_image)
+ impl_->image.GetMetaData (impl_->imageMD);
+
+ // RGB is the only image format supported.
+ if (impl_->imageMD.PixelFormat () != XN_PIXEL_FORMAT_RGB24)
+ REPORT_ERROR ("Image format must be RGB24\n");
+
+ getParams ();
+}
+
+void
+pcl::gpu::kinfuLS::CaptureOpenNI::release ()
+{
+ if (impl_)
+ {
+ impl_->context.StopGeneratingAll ();
+ impl_->context.Release ();
+ }
+
+ impl_.reset ();
+ depth_focal_length_VGA = 0;
+ baseline = 0.f;
+ shadow_value = 0;
+ no_sample_value = 0;
+ pixelSize = 0.0;
+}
+
+bool
+pcl::gpu::kinfuLS::CaptureOpenNI::grab (PtrStepSz<const unsigned short>& depth, PtrStepSz<const RGB>& rgb24)
+{
+ XnStatus rc = XN_STATUS_OK;
+
+ rc = impl_->context.WaitAndUpdateAll ();
+ if (rc != XN_STATUS_OK)
+ return printf ("Read failed: %s\n", xnGetStatusString (rc)), false;
+
+ if (impl_->has_depth)
+ {
+ impl_->depth.GetMetaData (impl_->depthMD);
+ const XnDepthPixel* pDepth = impl_->depthMD.Data ();
+ int x = impl_->depthMD.FullXRes ();
+ int y = impl_->depthMD.FullYRes ();
+ depth.cols = x;
+ depth.rows = y;
+ depth.data = pDepth;
+ depth.step = x * depth.elemSize ();
+ }
+ else
+ printf ("no depth\n");
+
+ if (impl_->has_image)
+ {
+ impl_->image.GetMetaData (impl_->imageMD);
+ const XnRGB24Pixel* pImage = impl_->imageMD.RGB24Data ();
+ int x = impl_->imageMD.FullXRes ();
+ int y = impl_->imageMD.FullYRes ();
+
+ rgb24.data = (const RGB*)pImage;
+ rgb24.cols = x;
+ rgb24.rows = y;
+ rgb24.step = x * rgb24.elemSize ();
+ }
+ else
+ {
+ printf ("no image\n");
+ rgb24.data = 0;
+ rgb24.cols = rgb24.rows = rgb24.step = 0;
+ }
+
+ return impl_->has_image || impl_->has_depth;
+}
+
+void
+pcl::gpu::kinfuLS::CaptureOpenNI::getParams ()
+{
+ XnStatus rc = XN_STATUS_OK;
+
+ max_depth = impl_->depth.GetDeviceMaxDepth ();
+
+ rc = impl_->depth.GetRealProperty ( "ZPPS", pixelSize ); // in mm
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "ZPPS failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+
+ XnUInt64 depth_focal_length_SXGA_mm; //in mm
+ rc = impl_->depth.GetIntProperty ("ZPD", depth_focal_length_SXGA_mm);
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "ZPD failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+
+ XnDouble baseline_local;
+ rc = impl_->depth.GetRealProperty ("LDDIS", baseline_local);
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "ZPD failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+
+ XnUInt64 shadow_value_local;
+ rc = impl_->depth.GetIntProperty ("ShadowValue", shadow_value_local);
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "ShadowValue failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+ shadow_value = (int)shadow_value_local;
+
+ XnUInt64 no_sample_value_local;
+ rc = impl_->depth.GetIntProperty ("NoSampleValue", no_sample_value_local);
+ if (rc != XN_STATUS_OK)
+ {
+ sprintf (impl_->strError, "NoSampleValue failed: %s\n", xnGetStatusString (rc));
+ REPORT_ERROR (impl_->strError);
+ }
+ no_sample_value = (int)no_sample_value_local;
+
+
+ // baseline from cm -> mm
+ baseline = (float)(baseline_local * 10);
+
+ //focal length from mm -> pixels (valid for 1280x1024)
+ float depth_focal_length_SXGA = static_cast<float>(depth_focal_length_SXGA_mm / pixelSize);
+ depth_focal_length_VGA = depth_focal_length_SXGA / 2;
+}
+
+bool
+pcl::gpu::kinfuLS::CaptureOpenNI::setRegistration (bool value)
+{
+ XnStatus rc = XN_STATUS_OK;
+
+ if (value)
+ {
+ if (!impl_->has_image)
+ return false;
+
+ if (impl_->depth.GetAlternativeViewPointCap ().IsViewPointAs (impl_->image) )
+ return true;
+
+ if (!impl_->depth.GetAlternativeViewPointCap ().IsViewPointSupported (impl_->image) )
+ {
+ printf ("SetRegistration failed: Unsupported viewpoint.\n");
+ return false;
+ }
+
+ rc = impl_->depth.GetAlternativeViewPointCap ().SetViewPoint (impl_->image);
+ if (rc != XN_STATUS_OK)
+ printf ("SetRegistration failed: %s\n", xnGetStatusString (rc));
+
+ }
+ else // "off"
+ {
+ rc = impl_->depth.GetAlternativeViewPointCap ().ResetViewPoint ();
+ if (rc != XN_STATUS_OK)
+ printf ("SetRegistration failed: %s\n", xnGetStatusString (rc));
+ }
+
+ getParams ();
+ return rc == XN_STATUS_OK;
+}
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef COLOR_HANDLER_H_
+#define COLOR_HANDLER_H_
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/visualization/point_cloud_handlers.h>
+
+namespace pcl
+{
+ namespace visualization
+ {
+ template <typename PointT>
+ class PointCloudColorHandlerRGBHack : public PointCloudColorHandler<PointT>
+ {
+ using PointCloudColorHandler<PointT>::capable_;
+ using PointCloudColorHandler<PointT>::cloud_;
+
+ typedef typename PointCloudColorHandler<PointT>::PointCloud::ConstPtr PointCloudConstPtr;
+ typedef typename pcl::PointCloud<RGB>::ConstPtr RgbCloudConstPtr;
+
+ public:
+ typedef boost::shared_ptr<PointCloudColorHandlerRGBHack<PointT> > Ptr;
+ typedef boost::shared_ptr<const PointCloudColorHandlerRGBHack<PointT> > ConstPtr;
+
+ PointCloudColorHandlerRGBHack (const PointCloudConstPtr& cloud, const RgbCloudConstPtr& colors) :
+ PointCloudColorHandler<PointT> (cloud), rgb_ (colors)
+ {
+ capable_ = true;
+ }
+
+ virtual bool
+ getColor (vtkSmartPointer<vtkDataArray> &scalars) const
+ {
+ if (!capable_)
+ return (false);
+
+ if (!scalars)
+ scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+ scalars->SetNumberOfComponents (3);
+
+ vtkIdType nr_points = (int)cloud_->points.size ();
+ reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
+ unsigned char* colors = reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->GetPointer (0);
+
+ // Color every point
+ if (nr_points != (int)rgb_->points.size ())
+ std::fill(colors, colors + nr_points * 3, (unsigned char)0xFF);
+ else
+ for (vtkIdType cp = 0; cp < nr_points; ++cp)
+ {
+ int idx = cp * 3;
+ colors[idx + 0] = rgb_->points[cp].r;
+ colors[idx + 1] = rgb_->points[cp].g;
+ colors[idx + 2] = rgb_->points[cp].b;
+ }
+ return (true);
+ }
+
+ private:
+ virtual std::string getFieldName () const { return ("rgb"); }
+ virtual inline std::string getName () const { return ("PointCloudColorHandlerRGBHack"); }
+ RgbCloudConstPtr rgb_;
+ };
+ }
+}
+
+
+#endif /* COLOR_HANDLER_H_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include "evaluation.h"
+
+#include<iostream>
+
+using namespace pcl::gpu;
+using namespace std;
+
+const float Evaluation::fx = 525.0f;
+const float Evaluation::fy = 525.0f;
+const float Evaluation::cx = 319.5f;
+const float Evaluation::cy = 239.5f;
+
+#ifndef HAVE_OPENCV
+
+struct Evaluation::Impl {};
+
+Evaluation::Evaluation(const std::string&) { cout << "Evaluation requires OpenCV. Please enable it in cmake-file" << endl; exit(0); }
+void Evaluation::setMatchFile(const std::string&) { }
+bool Evaluation::grab (double stamp, pcl::gpu::PtrStepSz<const RGB>& rgb24) { return false; }
+bool Evaluation::grab (double stamp, pcl::gpu::PtrStepSz<const unsigned short>& depth) { return false; }
+bool Evaluation::grab (double stamp, pcl::gpu::PtrStepSz<const unsigned short>& depth, pcl::gpu::PtrStepSz<const RGB>& rgb24) { return false; }
+void Evaluation::saveAllPoses(const pcl::gpu::kinfuLS::KinfuTracker& kinfu, int frame_number, const std::string& logfile) const {}
+
+#else
+
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include<fstream>
+
+using namespace cv;
+
+struct Evaluation::Impl
+{
+ Mat depth_buffer;
+ Mat rgb_buffer;
+};
+
+
+
+Evaluation::Evaluation(const std::string& folder) : folder_(folder), visualization_(false)
+{
+ impl_.reset( new Impl() );
+
+ if (folder_[folder_.size() - 1] != '\\' && folder_[folder_.size() - 1] != '/')
+ folder_.push_back('/');
+
+ cout << "Initializing evaluation from folder: " << folder_ << endl;
+ string depth_file = folder_ + "depth.txt";
+ string rgb_file = folder_ + "rgb.txt";
+
+ readFile(depth_file, depth_stamps_and_filenames_);
+ readFile(rgb_file, rgb_stamps_and_filenames_);
+
+ string associated_file = folder_ + "associated.txt";
+}
+
+void Evaluation::setMatchFile(const std::string& file)
+{
+ string full = folder_ + file;
+ ifstream iff(full.c_str());
+ if(!iff)
+ {
+ cout << "Can't read " << file << endl;
+ exit(1);
+ }
+
+ accociations_.clear();
+ while (!iff.eof())
+ {
+ Association acc;
+ iff >> acc.time1 >> acc.name1 >> acc.time2 >> acc.name2;
+ accociations_.push_back(acc);
+ }
+}
+
+void Evaluation::readFile(const string& file, vector< pair<double,string> >& output)
+{
+ char buffer[4096];
+ vector< pair<double,string> > tmp;
+
+ ifstream iff(file.c_str());
+ if(!iff)
+ {
+ cout << "Can't read " << file << endl;
+ exit(1);
+ }
+
+ // ignore three header lines
+ iff.getline(buffer, sizeof(buffer));
+ iff.getline(buffer, sizeof(buffer));
+ iff.getline(buffer, sizeof(buffer));
+
+ // each line consists of the timestamp and the filename of the depth image
+ while (!iff.eof())
+ {
+ double time; string name;
+ iff >> time >> name;
+ tmp.push_back(make_pair(time, name));
+ }
+ tmp.swap(output);
+}
+
+bool Evaluation::grab (double stamp, PtrStepSz<const RGB>& rgb24)
+{
+ size_t i = static_cast<size_t>(stamp); // temporary solution, now it expects only index
+ size_t total = accociations_.empty() ? rgb_stamps_and_filenames_.size() : accociations_.size();
+
+ if ( i>= total)
+ return false;
+
+ string file = folder_ + (accociations_.empty() ? rgb_stamps_and_filenames_[i].second : accociations_[i].name2);
+
+ cv::Mat bgr = cv::imread(file);
+ if(bgr.empty())
+ return false;
+
+ cv::cvtColor(bgr, impl_->rgb_buffer, CV_BGR2RGB);
+
+ rgb24.data = impl_->rgb_buffer.ptr<RGB>();
+ rgb24.cols = impl_->rgb_buffer.cols;
+ rgb24.rows = impl_->rgb_buffer.rows;
+ rgb24.step = impl_->rgb_buffer.cols*sizeof(unsigned char);
+
+ if (visualization_)
+ {
+ cv::imshow("Color channel", bgr);
+ cv::waitKey(3);
+ }
+ return true;
+}
+
+bool Evaluation::grab (double stamp, PtrStepSz<const unsigned short>& depth)
+{
+ size_t i = static_cast<size_t>(stamp); // temporary solution, now it expects only index
+ size_t total = accociations_.empty() ? depth_stamps_and_filenames_.size() : accociations_.size();
+
+ if ( i>= total)
+ return false;
+
+ string file = folder_ + (accociations_.empty() ? depth_stamps_and_filenames_[i].second : accociations_[i].name1);
+
+ cv::Mat d_img = cv::imread(file, CV_LOAD_IMAGE_ANYDEPTH | CV_LOAD_IMAGE_ANYCOLOR);
+ if(d_img.empty())
+ return false;
+
+ if (d_img.elemSize() != sizeof(unsigned short))
+ {
+ cout << "Image was not opend in 16-bit format. Please use OpenCV 2.3.1 or higher" << endl;
+ exit(1);
+ }
+
+ // Datasets are with factor 5000 (pixel to m)
+ // http://cvpr.in.tum.de/data/datasets/rgbd-dataset/file_formats#color_images_and_depth_maps
+
+ d_img.convertTo(impl_->depth_buffer, d_img.type(), 0.2);
+ depth.data = impl_->depth_buffer.ptr<ushort>();
+ depth.cols = impl_->depth_buffer.cols;
+ depth.rows = impl_->depth_buffer.rows;
+ depth.step = impl_->depth_buffer.cols*sizeof(ushort); // 1280 = 640*2
+
+ if (visualization_)
+ {
+ cv::Mat scaled = impl_->depth_buffer/5000.0*65535;
+ cv::imshow("Depth channel", scaled);
+ cv::waitKey(3);
+ }
+ return true;
+}
+
+bool Evaluation::grab (double stamp, PtrStepSz<const unsigned short>& depth, PtrStepSz<const RGB>& rgb24)
+{
+ if (accociations_.empty())
+ {
+ cout << "Please set match file" << endl;
+ exit(0);
+ }
+
+ size_t i = static_cast<size_t>(stamp); // temporary solution, now it expects only index
+
+ if ( i>= accociations_.size())
+ return false;
+
+ string depth_file = folder_ + accociations_[i].name1;
+ string color_file = folder_ + accociations_[i].name2;
+
+ cv::Mat d_img = cv::imread(depth_file, CV_LOAD_IMAGE_ANYDEPTH | CV_LOAD_IMAGE_ANYCOLOR);
+ if(d_img.empty())
+ return false;
+
+ if (d_img.elemSize() != sizeof(unsigned short))
+ {
+ cout << "Image was not opend in 16-bit format. Please use OpenCV 2.3.1 or higher" << endl;
+ exit(1);
+ }
+
+ // Datasets are with factor 5000 (pixel to m)
+ // http://cvpr.in.tum.de/data/datasets/rgbd-dataset/file_formats#color_images_and_depth_maps
+
+ d_img.convertTo(impl_->depth_buffer, d_img.type(), 0.2);
+ depth.data = impl_->depth_buffer.ptr<ushort>();
+ depth.cols = impl_->depth_buffer.cols;
+ depth.rows = impl_->depth_buffer.rows;
+ depth.step = impl_->depth_buffer.cols*depth.elemSize(); // 1280 = 640*2
+
+ cv::Mat bgr = cv::imread(color_file);
+ if(bgr.empty())
+ return false;
+
+ cv::cvtColor(bgr, impl_->rgb_buffer, CV_BGR2RGB);
+
+ rgb24.data = impl_->rgb_buffer.ptr<RGB>();
+ rgb24.cols = impl_->rgb_buffer.cols;
+ rgb24.rows = impl_->rgb_buffer.rows;
+ rgb24.step = impl_->rgb_buffer.cols*sizeof(unsigned char);
+
+ return true;
+}
+
+void Evaluation::saveAllPoses(const pcl::gpu::KinfuTracker& kinfu, int frame_number, const std::string& logfile) const
+{
+ size_t total = accociations_.empty() ? depth_stamps_and_filenames_.size() : accociations_.size();
+
+ if (frame_number < 0)
+ frame_number = (int)total;
+
+ frame_number = std::min(frame_number, (int)kinfu.getNumberOfPoses());
+
+ cout << "Writing " << frame_number << " poses to " << logfile << endl;
+
+ ofstream path_file_stream(logfile.c_str());
+ path_file_stream.setf(ios::fixed,ios::floatfield);
+
+ for(int i = 0; i < frame_number; ++i)
+ {
+ Eigen::Affine3f pose = kinfu.getCameraPose(i);
+ Eigen::Quaternionf q(pose.rotation());
+ Eigen::Vector3f t = pose.translation();
+
+ double stamp = accociations_.empty() ? depth_stamps_and_filenames_[i].first : accociations_[i].time1;
+
+ path_file_stream << stamp << " ";
+ path_file_stream << t[0] << " " << t[1] << " " << t[2] << " ";
+ path_file_stream << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;
+ }
+}
+
+
+#endif /* HAVE_OPENCV */
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#pragma once
+
+#include <string>
+#include <boost/shared_ptr.hpp>
+#include <pcl/gpu/containers/kernel_containers.h>
+#include <pcl/gpu/kinfu_large_scale/kinfu.h>
+
+
+/** \brief class for RGB-D SLAM Dataset and Benchmark
+ * \author Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+class Evaluation
+{
+public:
+ typedef boost::shared_ptr<Evaluation> Ptr;
+ typedef /*pcl::gpu::KinfuTracker::PixelRGB*/ pcl::gpu::kinfuLS::PixelRGB RGB;
+
+ Evaluation(const std::string& folder);
+
+ /** \brief Sets file with matches between depth and rgb */
+ void setMatchFile(const std::string& file);
+
+ /** \brief Reads rgb frame from the folder
+ * \param stamp index of frame to read (stamps are not implemented)
+ * \param rgb24
+ */
+ bool grab (double stamp, pcl::gpu::PtrStepSz<const RGB>& rgb24);
+
+ /** \brief Reads depth frame from the folder
+ * \param stamp index of frame to read (stamps are not implemented)
+ * \param depth
+ */
+ bool grab (double stamp, pcl::gpu::PtrStepSz<const unsigned short>& depth);
+
+ /** \brief Reads depth & rgb frame from the folder. Before calling this folder please call 'setMatchFile', or an error will be returned otherwise.
+ * \param stamp index of accociated frame pair (stamps are not implemented)
+ * \param depth
+ * \param rgb24
+ */
+ bool grab (double stamp, pcl::gpu::PtrStepSz<const unsigned short>& depth, pcl::gpu::PtrStepSz<const RGB>& rgb24);
+
+ const static float fx, fy, cx, cy;
+
+
+ void saveAllPoses(const pcl::gpu::kinfuLS::KinfuTracker& kinfu, int frame_number = -1, const std::string& logfile = "kinfu_poses.txt") const;
+
+private:
+ std::string folder_;
+ bool visualization_;
+
+ std::vector< std::pair<double, std::string> > rgb_stamps_and_filenames_;
+ std::vector< std::pair<double, std::string> > depth_stamps_and_filenames_;
+
+ struct Association
+ {
+ double time1, time2;
+ std::string name1, name2;
+ };
+
+ std::vector< Association > accociations_;
+
+ void readFile(const std::string& file, std::vector< std::pair<double, std::string> >& output);
+
+ struct Impl;
+ boost::shared_ptr<Impl> impl_;
+};
+
--- /dev/null
+/*
+Work in progress: patch by Marco (AUG,19th 2012)
+> oni fixed
+> pcl added: mostly to include rgb treatment while grabbing from PCD files obtained by pcl_openni_grab_frame -noend
+> sync issue fixed
+> volume_size issue fixed
+> world.pcd write exception on windows fixed on new trunk version
+
++ minor changes
+*/
+
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#define _CRT_SECURE_NO_DEPRECATE
+
+#include <iostream>
+
+#include <pcl/console/parse.h>
+
+#include <boost/filesystem.hpp>
+
+#include <pcl/gpu/kinfu_large_scale/kinfu.h>
+#include <pcl/gpu/kinfu_large_scale/raycaster.h>
+#include <pcl/gpu/kinfu_large_scale/marching_cubes.h>
+#include <pcl/gpu/containers/initialization.h>
+
+#include <pcl/common/time.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/visualization/image_viewer.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/vtk_io.h>
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/oni_grabber.h>
+#include <pcl/io/pcd_grabber.h>
+
+#include "openni_capture.h"
+#include "color_handler.h"
+#include "evaluation.h"
+
+#include <pcl/common/angles.h>
+
+#ifdef HAVE_OPENCV
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#endif
+typedef pcl::ScopeTime ScopeTimeT;
+
+#include <pcl/gpu/kinfu_large_scale/screenshot_manager.h>
+
+using namespace std;
+using namespace pcl;
+using namespace Eigen;
+
+using namespace pcl::gpu::kinfuLS;
+
+using pcl::gpu::DeviceArray;
+using pcl::gpu::DeviceArray2D;
+using pcl::gpu::PtrStepSz;
+
+namespace pc = pcl::console;
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace kinfuLS
+ {
+ void paint3DView (const KinfuTracker::View& rgb24, KinfuTracker::View& view, float colors_weight = 0.5f);
+ void mergePointNormal (const DeviceArray<PointXYZ>& cloud, const DeviceArray<Normal>& normals, DeviceArray<PointNormal>& output);
+ }
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+vector<string> getPcdFilesInDir(const string& directory)
+{
+ namespace fs = boost::filesystem;
+ fs::path dir(directory);
+
+ std::cout << "path: " << directory << std::endl;
+ if (directory.empty() || !fs::exists(dir) || !fs::is_directory(dir))
+ PCL_THROW_EXCEPTION (pcl::IOException, "No valid PCD directory given!\n");
+
+ vector<string> result;
+ fs::directory_iterator pos(dir);
+ fs::directory_iterator end;
+
+ for(; pos != end ; ++pos)
+ if (fs::is_regular_file(pos->status()) )
+ if (fs::extension(*pos) == ".pcd")
+ {
+#if BOOST_FILESYSTEM_VERSION == 3
+ result.push_back (pos->path ().string ());
+#else
+ result.push_back (pos->path ());
+#endif
+ cout << "added: " << result.back() << endl;
+ }
+
+ return result;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct SampledScopeTime : public StopWatch
+{
+ enum { EACH = 33 };
+ SampledScopeTime(int& time_ms) : time_ms_(time_ms) {}
+ ~SampledScopeTime()
+ {
+ static int i_ = 0;
+ static boost::posix_time::ptime starttime_ = boost::posix_time::microsec_clock::local_time();
+ time_ms_ += getTime ();
+ if (i_ % EACH == 0 && i_)
+ {
+ boost::posix_time::ptime endtime_ = boost::posix_time::microsec_clock::local_time();
+ cout << "Average frame time = " << time_ms_ / EACH << "ms ( " << 1000.f * EACH / time_ms_ << "fps )"
+ << "( real: " << 1000.f * EACH / (endtime_-starttime_).total_milliseconds() << "fps )" << endl;
+ time_ms_ = 0;
+ starttime_ = endtime_;
+ }
+ ++i_;
+ }
+private:
+ int& time_ms_;
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+setViewerPose (visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
+{
+ Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f (0, 0, 0);
+ Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f (0, 0, 1) + pos_vector;
+ Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f (0, -1, 0);
+ viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2],
+ look_at_vector[0], look_at_vector[1], look_at_vector[2],
+ up_vector[0], up_vector[1], up_vector[2]);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+Eigen::Affine3f
+getViewerPose (visualization::PCLVisualizer& viewer)
+{
+ Eigen::Affine3f pose = viewer.getViewerPose();
+ Eigen::Matrix3f rotation = pose.linear();
+
+ Matrix3f axis_reorder;
+ axis_reorder << 0, 0, 1,
+ -1, 0, 0,
+ 0, -1, 0;
+
+ rotation = rotation * axis_reorder;
+ pose.linear() = rotation;
+ return pose;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+template<typename CloudT> void
+writeCloudFile (int format, const CloudT& cloud);
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+writePolygonMeshFile (int format, const pcl::PolygonMesh& mesh);
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+template<typename MergedT, typename PointT>
+typename PointCloud<MergedT>::Ptr merge(const PointCloud<PointT>& points, const PointCloud<RGB>& colors)
+{
+ typename PointCloud<MergedT>::Ptr merged_ptr(new PointCloud<MergedT>());
+
+ pcl::copyPointCloud (points, *merged_ptr);
+ for (size_t i = 0; i < colors.size (); ++i)
+ merged_ptr->points[i].rgba = colors.points[i].rgba;
+
+ return merged_ptr;
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+boost::shared_ptr<pcl::PolygonMesh> convertToMesh(const DeviceArray<PointXYZ>& triangles)
+{
+ if (triangles.empty())
+ return boost::shared_ptr<pcl::PolygonMesh>();
+
+ pcl::PointCloud<pcl::PointXYZ> cloud;
+ cloud.width = (int)triangles.size();
+ cloud.height = 1;
+ triangles.download(cloud.points);
+
+ boost::shared_ptr<pcl::PolygonMesh> mesh_ptr( new pcl::PolygonMesh() );
+ pcl::toPCLPointCloud2(cloud, mesh_ptr->cloud);
+
+ mesh_ptr->polygons.resize (triangles.size() / 3);
+ for (size_t i = 0; i < mesh_ptr->polygons.size (); ++i)
+ {
+ pcl::Vertices v;
+ v.vertices.push_back(i*3+0);
+ v.vertices.push_back(i*3+2);
+ v.vertices.push_back(i*3+1);
+ mesh_ptr->polygons[i] = v;
+ }
+ return mesh_ptr;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct CurrentFrameCloudView
+{
+ CurrentFrameCloudView() : cloud_device_ (480, 640), cloud_viewer_ ("Frame Cloud Viewer")
+ {
+ cloud_ptr_ = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ>);
+
+ cloud_viewer_.setBackgroundColor (0, 0, 0.15);
+ cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_POINT_SIZE, 1);
+ cloud_viewer_.addCoordinateSystem (1.0, "global");
+ cloud_viewer_.initCameraParameters ();
+ cloud_viewer_.setPosition (0, 500);
+ cloud_viewer_.setSize (640, 480);
+ cloud_viewer_.setCameraClipDistances (0.01, 10.01);
+ }
+
+ void
+ show (const KinfuTracker& kinfu)
+ {
+ kinfu.getLastFrameCloud (cloud_device_);
+
+ int c;
+ cloud_device_.download (cloud_ptr_->points, c);
+ cloud_ptr_->width = cloud_device_.cols ();
+ cloud_ptr_->height = cloud_device_.rows ();
+ cloud_ptr_->is_dense = false;
+
+ cloud_viewer_.removeAllPointClouds ();
+ cloud_viewer_.addPointCloud<PointXYZ>(cloud_ptr_);
+ cloud_viewer_.spinOnce ();
+ }
+
+ void
+ setViewerPose (const Eigen::Affine3f& viewer_pose) {
+ ::setViewerPose (cloud_viewer_, viewer_pose);
+ }
+
+ PointCloud<PointXYZ>::Ptr cloud_ptr_;
+ DeviceArray2D<PointXYZ> cloud_device_;
+ visualization::PCLVisualizer cloud_viewer_;
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct ImageView
+{
+ ImageView() : paint_image_ (false), accumulate_views_ (false)
+ {
+ viewerScene_.setWindowTitle ("View3D from ray tracing");
+ viewerScene_.setPosition (0, 0);
+ viewerDepth_.setWindowTitle ("Kinect Depth stream");
+ viewerDepth_.setPosition (640, 0);
+ //viewerColor_.setWindowTitle ("Kinect RGB stream");
+ }
+
+ void
+ showScene (KinfuTracker& kinfu, const PtrStepSz<const pcl::gpu::kinfuLS::PixelRGB>& rgb24, bool registration, Eigen::Affine3f* pose_ptr = 0)
+ {
+ if (pose_ptr)
+ {
+ raycaster_ptr_->run ( kinfu.volume (), *pose_ptr, kinfu.getCyclicalBufferStructure () ); //says in cmake it does not know it
+ raycaster_ptr_->generateSceneView(view_device_);
+ }
+ else
+ {
+ kinfu.getImage (view_device_);
+ }
+
+ if (paint_image_ && registration && !pose_ptr)
+ {
+ colors_device_.upload (rgb24.data, rgb24.step, rgb24.rows, rgb24.cols);
+ paint3DView (colors_device_, view_device_);
+ }
+
+ int cols;
+ view_device_.download (view_host_, cols);
+ viewerScene_.showRGBImage (reinterpret_cast<unsigned char*> (&view_host_[0]), view_device_.cols (), view_device_.rows ());
+
+ //viewerColor_.showRGBImage ((unsigned char*)&rgb24.data, rgb24.cols, rgb24.rows);
+#ifdef HAVE_OPENCV
+ if (accumulate_views_)
+ {
+ views_.push_back (cv::Mat ());
+ cv::cvtColor (cv::Mat (480, 640, CV_8UC3, (void*)&view_host_[0]), views_.back (), CV_RGB2GRAY);
+ //cv::copy(cv::Mat(480, 640, CV_8UC3, (void*)&view_host_[0]), views_.back());
+ }
+#endif
+ }
+
+ void
+ showDepth (const PtrStepSz<const unsigned short>& depth)
+ {
+ viewerDepth_.showShortImage (depth.data, depth.cols, depth.rows, 0, 5000, true);
+ }
+
+ void
+ showGeneratedDepth (KinfuTracker& kinfu, const Eigen::Affine3f& pose)
+ {
+ raycaster_ptr_->run(kinfu.volume(), pose, kinfu.getCyclicalBufferStructure ());
+ raycaster_ptr_->generateDepthImage(generated_depth_);
+
+ int c;
+ vector<unsigned short> data;
+ generated_depth_.download(data, c);
+
+ viewerDepth_.showShortImage (&data[0], generated_depth_.cols(), generated_depth_.rows(), 0, 5000, true);
+ }
+
+ void
+ toggleImagePaint()
+ {
+ paint_image_ = !paint_image_;
+ cout << "Paint image: " << (paint_image_ ? "On (requires registration mode)" : "Off") << endl;
+ }
+
+ bool paint_image_;
+ bool accumulate_views_;
+
+ visualization::ImageViewer viewerScene_; //view the raycasted model
+ visualization::ImageViewer viewerDepth_; //view the current depth map
+ //visualization::ImageViewer viewerColor_;
+
+ KinfuTracker::View view_device_;
+ KinfuTracker::View colors_device_;
+ vector<pcl::gpu::kinfuLS::PixelRGB> view_host_;
+
+ RayCaster::Ptr raycaster_ptr_;
+
+ KinfuTracker::DepthMap generated_depth_;
+
+#ifdef HAVE_OPENCV
+ vector<cv::Mat> views_;
+#endif
+};
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// View the volume as 3D points
+struct SceneCloudView
+{
+ enum { GPU_Connected6 = 0, CPU_Connected6 = 1, CPU_Connected26 = 2 };
+
+ SceneCloudView() : extraction_mode_ (GPU_Connected6), compute_normals_ (false), valid_combined_ (false), cube_added_(false), cloud_viewer_ ("Scene Cloud Viewer")
+ {
+ cloud_ptr_ = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ>);
+ normals_ptr_ = PointCloud<Normal>::Ptr (new PointCloud<Normal>);
+ combined_ptr_ = PointCloud<PointNormal>::Ptr (new PointCloud<PointNormal>);
+ point_colors_ptr_ = PointCloud<RGB>::Ptr (new PointCloud<RGB>);
+
+ cloud_viewer_.setBackgroundColor (0, 0, 0);
+ cloud_viewer_.addCoordinateSystem (1.0, "global");
+ cloud_viewer_.initCameraParameters ();
+ cloud_viewer_.setPosition (0, 500);
+ cloud_viewer_.setSize (640, 480);
+ cloud_viewer_.setCameraClipDistances (0.01, 10.01);
+
+ cloud_viewer_.addText ("H: print help", 2, 15, 20, 34, 135, 246);
+ cloud_viewer_.addText ("ICP State: ", 450, 55, 20, 0.0, 1.0, 0.0, "icp");
+ cloud_viewer_.addText ("Press 'S' to save the current world", 450, 35, 10, 0.0, 1.0, 0.0, "icp_save");
+ cloud_viewer_.addText ("Press 'R' to reset the system", 450, 15, 10, 0.0, 1.0, 0.0, "icp_reset");
+ }
+
+ inline void
+ drawCamera (Eigen::Affine3f& pose, const string& name, double r, double g, double b)
+ {
+ double focal = 575;
+ double height = 480;
+ double width = 640;
+
+ // create a 5-point visual for each camera
+ pcl::PointXYZ p1, p2, p3, p4, p5;
+ p1.x=0; p1.y=0; p1.z=0;
+ double angleX = RAD2DEG (2.0 * atan (width / (2.0*focal)));
+ double angleY = RAD2DEG (2.0 * atan (height / (2.0*focal)));
+ double dist = 0.75;
+ double minX, minY, maxX, maxY;
+ maxX = dist*tan (atan (width / (2.0*focal)));
+ minX = -maxX;
+ maxY = dist*tan (atan (height / (2.0*focal)));
+ minY = -maxY;
+ p2.x=minX; p2.y=minY; p2.z=dist;
+ p3.x=maxX; p3.y=minY; p3.z=dist;
+ p4.x=maxX; p4.y=maxY; p4.z=dist;
+ p5.x=minX; p5.y=maxY; p5.z=dist;
+ p1=pcl::transformPoint (p1, pose);
+ p2=pcl::transformPoint (p2, pose);
+ p3=pcl::transformPoint (p3, pose);
+ p4=pcl::transformPoint (p4, pose);
+ p5=pcl::transformPoint (p5, pose);
+ std::stringstream ss;
+ ss.str ("");
+ ss << name << "_line1";
+ cloud_viewer_.addLine (p1, p2, r, g, b, ss.str ());
+ ss.str ("");
+ ss << name << "_line2";
+ cloud_viewer_.addLine (p1, p3, r, g, b, ss.str ());
+ ss.str ("");
+ ss << name << "_line3";
+ cloud_viewer_.addLine (p1, p4, r, g, b, ss.str ());
+ ss.str ("");
+ ss << name << "_line4";
+ cloud_viewer_.addLine (p1, p5, r, g, b, ss.str ());
+ ss.str ("");
+ ss << name << "_line5";
+ cloud_viewer_.addLine (p2, p5, r, g, b, ss.str ());
+ ss.str ("");
+ ss << name << "_line6";
+ cloud_viewer_.addLine (p5, p4, r, g, b, ss.str ());
+ ss.str ("");
+ ss << name << "_line7";
+ cloud_viewer_.addLine (p4, p3, r, g, b, ss.str ());
+ ss.str ("");
+ ss << name << "_line8";
+ cloud_viewer_.addLine (p3, p2, r, g, b, ss.str ());
+ }
+
+ inline void
+ removeCamera (const string& name)
+ {
+ cloud_viewer_.removeShape (name);
+ std::stringstream ss;
+ ss.str ("");
+ ss << name << "_line1";
+ cloud_viewer_.removeShape (ss.str ());
+ ss.str ("");
+ ss << name << "_line2";
+ cloud_viewer_.removeShape (ss.str ());
+ ss.str ("");
+ ss << name << "_line3";
+ cloud_viewer_.removeShape (ss.str ());
+ ss.str ("");
+ ss << name << "_line4";
+ cloud_viewer_.removeShape (ss.str ());
+ ss.str ("");
+ ss << name << "_line5";
+ cloud_viewer_.removeShape (ss.str ());
+ ss.str ("");
+ ss << name << "_line6";
+ cloud_viewer_.removeShape (ss.str ());
+ ss.str ("");
+ ss << name << "_line7";
+ cloud_viewer_.removeShape (ss.str ());
+ ss.str ("");
+ ss << name << "_line8";
+ cloud_viewer_.removeShape (ss.str ());
+ }
+
+ void
+ displayICPState (KinfuTracker& kinfu, bool was_lost_)
+ {
+ string name = "last_good_track";
+ string name_estimate = "last_good_estimate";
+ if (was_lost_ && !kinfu.icpIsLost ()) //execute only when ICP just recovered (i.e. was_lost_ == true && icpIsLost == false)
+ {
+ removeCamera(name);
+ removeCamera(name_estimate);
+ clearClouds(false);
+ cloud_viewer_.updateText ("ICP State: OK", 450, 55, 20, 0.0, 1.0, 0.0, "icp");
+ cloud_viewer_.updateText ("Press 'S' to save the current world", 450, 35, 10, 0.0, 1.0, 0.0, "icp_save");
+ cloud_viewer_.updateText ("Press 'R' to reset the system", 450, 15, 10, 0.0, 1.0, 0.0, "icp_reset");
+ }
+ else if (!was_lost_ && kinfu.icpIsLost()) //execute only when we just lost ourselves (i.e. was_lost_ = false && icpIsLost == true)
+ {
+ // draw position of the last good track
+ Eigen::Affine3f last_pose = kinfu.getCameraPose();
+ drawCamera(last_pose, name, 0.0, 1.0, 0.0);
+
+
+
+ cloud_viewer_.updateText ("ICP State: LOST", 450, 55, 20, 1.0, 0.0, 0.0, "icp");
+ cloud_viewer_.updateText ("Press 'S' to save the current world", 450, 35, 10, 1.0, 0.0, 0.0, "icp_save");
+ cloud_viewer_.updateText ("Press 'R' to reset the system", 450, 15, 10, 1.0, 0.0, 0.0, "icp_reset");
+ }
+
+
+ if( kinfu.icpIsLost() )
+ {
+ removeCamera(name_estimate);
+ // draw current camera estimate
+ Eigen::Affine3f last_pose_estimate = kinfu.getLastEstimatedPose();
+ drawCamera(last_pose_estimate, name_estimate, 1.0, 0.0, 0.0);
+ }
+
+
+
+// cout << "current estimated pose: " << kinfu.getLastEstimatedPose().translation() << std::endl << kinfu.getLastEstimatedPose().linear() << std::endl;
+//
+ }
+
+ void
+ show (KinfuTracker& kinfu, bool integrate_colors)
+ {
+ viewer_pose_ = kinfu.getCameraPose();
+
+ ScopeTimeT time ("PointCloud Extraction");
+ cout << "\nGetting cloud... " << flush;
+
+ valid_combined_ = false;
+
+ if (extraction_mode_ != GPU_Connected6) // So use CPU
+ {
+ kinfu.volume().fetchCloudHost (*cloud_ptr_, extraction_mode_ == CPU_Connected26);
+ }
+ else
+ {
+ DeviceArray<PointXYZ> extracted = kinfu.volume().fetchCloud (cloud_buffer_device_);
+
+ if (compute_normals_)
+ {
+ kinfu.volume().fetchNormals (extracted, normals_device_);
+ pcl::gpu::kinfuLS::mergePointNormal (extracted, normals_device_, combined_device_);
+ combined_device_.download (combined_ptr_->points);
+ combined_ptr_->width = (int)combined_ptr_->points.size ();
+ combined_ptr_->height = 1;
+
+ valid_combined_ = true;
+ }
+ else
+ {
+ extracted.download (cloud_ptr_->points);
+ cloud_ptr_->width = (int)cloud_ptr_->points.size ();
+ cloud_ptr_->height = 1;
+ }
+
+ if (integrate_colors)
+ {
+ kinfu.colorVolume().fetchColors(extracted, point_colors_device_);
+ point_colors_device_.download(point_colors_ptr_->points);
+ point_colors_ptr_->width = (int)point_colors_ptr_->points.size ();
+ point_colors_ptr_->height = 1;
+ }
+ else
+ point_colors_ptr_->points.clear();
+ }
+ size_t points_size = valid_combined_ ? combined_ptr_->points.size () : cloud_ptr_->points.size ();
+ cout << "Done. Cloud size: " << points_size / 1000 << "K" << endl;
+
+ cloud_viewer_.removeAllPointClouds ();
+ if (valid_combined_)
+ {
+ visualization::PointCloudColorHandlerRGBHack<PointNormal> rgb(combined_ptr_, point_colors_ptr_);
+ cloud_viewer_.addPointCloud<PointNormal> (combined_ptr_, rgb, "Cloud");
+ cloud_viewer_.addPointCloudNormals<PointNormal>(combined_ptr_, 50);
+ }
+ else
+ {
+ visualization::PointCloudColorHandlerRGBHack<PointXYZ> rgb(cloud_ptr_, point_colors_ptr_);
+ cloud_viewer_.addPointCloud<PointXYZ> (cloud_ptr_, rgb);
+ }
+ }
+
+ void
+ toggleCube(const Eigen::Vector3f& size)
+ {
+ if (cube_added_)
+ cloud_viewer_.removeShape("cube");
+ else
+ cloud_viewer_.addCube(size*0.5, Eigen::Quaternionf::Identity(), size(0), size(1), size(2));
+
+ cube_added_ = !cube_added_;
+ }
+
+ void
+ toggleExtractionMode ()
+ {
+ extraction_mode_ = (extraction_mode_ + 1) % 3;
+ switch (extraction_mode_)
+ {
+ case 0: cout << "Cloud extraction mode: GPU, Connected-6" << endl; break;
+ case 1: cout << "Cloud extraction mode: CPU, Connected-6 (requires a lot of memory)" << endl; break;
+ case 2: cout << "Cloud extraction mode: CPU, Connected-26 (requires a lot of memory)" << endl; break;
+ }
+ }
+
+ void
+ toggleNormals ()
+ {
+ compute_normals_ = !compute_normals_;
+ cout << "Compute normals: " << (compute_normals_ ? "On" : "Off") << endl;
+ }
+
+ void
+ clearClouds (bool print_message = false)
+ {
+ cloud_viewer_.removeAllPointClouds ();
+ cloud_ptr_->points.clear ();
+ normals_ptr_->points.clear ();
+ if (print_message)
+ cout << "Clouds/Meshes were cleared" << endl;
+ }
+
+ void
+ showMesh(KinfuTracker& kinfu, bool /*integrate_colors*/)
+ {
+ ScopeTimeT time ("Mesh Extraction");
+ cout << "\nGetting mesh... " << flush;
+
+ if (!marching_cubes_)
+ marching_cubes_ = MarchingCubes::Ptr( new MarchingCubes() );
+
+ DeviceArray<PointXYZ> triangles_device = marching_cubes_->run(kinfu.volume(), triangles_buffer_device_);
+ mesh_ptr_ = convertToMesh(triangles_device);
+
+ cloud_viewer_.removeAllPointClouds ();
+ if (mesh_ptr_)
+ cloud_viewer_.addPolygonMesh(*mesh_ptr_);
+
+ cout << "Done. Triangles number: " << triangles_device.size() / MarchingCubes::POINTS_PER_TRIANGLE / 1000 << "K" << endl;
+ }
+
+ int extraction_mode_;
+ bool compute_normals_;
+ bool valid_combined_;
+ bool cube_added_;
+
+ Eigen::Affine3f viewer_pose_;
+
+ visualization::PCLVisualizer cloud_viewer_;
+
+ PointCloud<PointXYZ>::Ptr cloud_ptr_;
+ PointCloud<Normal>::Ptr normals_ptr_;
+
+ DeviceArray<PointXYZ> cloud_buffer_device_;
+ DeviceArray<Normal> normals_device_;
+
+ PointCloud<PointNormal>::Ptr combined_ptr_;
+ DeviceArray<PointNormal> combined_device_;
+
+ DeviceArray<RGB> point_colors_device_;
+ PointCloud<RGB>::Ptr point_colors_ptr_;
+
+ MarchingCubes::Ptr marching_cubes_;
+ DeviceArray<PointXYZ> triangles_buffer_device_;
+
+ boost::shared_ptr<pcl::PolygonMesh> mesh_ptr_;
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct KinFuLSApp
+{
+ enum { PCD_BIN = 1, PCD_ASCII = 2, PLY = 3, MESH_PLY = 7, MESH_VTK = 8 };
+
+ KinFuLSApp(pcl::Grabber& source, float vsz, float shiftDistance, int snapshotRate) : exit_ (false), scan_ (false), scan_mesh_(false), scan_volume_ (false), independent_camera_ (false),
+ registration_ (false), integrate_colors_ (false), pcd_source_ (false), focal_length_(-1.f), capture_ (source), was_lost_(false), time_ms_ (0)
+ {
+ //Init Kinfu Tracker
+ Eigen::Vector3f volume_size = Vector3f::Constant (vsz/*meters*/);
+
+ PCL_WARN ("--- CURRENT SETTINGS ---\n");
+ PCL_INFO ("Volume size is set to %.2f meters\n", vsz);
+ PCL_INFO ("Volume will shift when the camera target point is farther than %.2f meters from the volume center\n", shiftDistance);
+ PCL_INFO ("The target point is located at [0, 0, %.2f] in camera coordinates\n", 0.6*vsz);
+ PCL_WARN ("------------------------\n");
+
+ // warning message if shifting distance is abnormally big compared to volume size
+ if(shiftDistance > 2.5 * vsz)
+ PCL_WARN ("WARNING Shifting distance (%.2f) is very large compared to the volume size (%.2f).\nYou can modify it using --shifting_distance.\n", shiftDistance, vsz);
+
+ kinfu_ = new pcl::gpu::kinfuLS::KinfuTracker(volume_size, shiftDistance);
+
+ Eigen::Matrix3f R = Eigen::Matrix3f::Identity (); // * AngleAxisf( pcl::deg2rad(-30.f), Vector3f::UnitX());
+ Eigen::Vector3f t = volume_size * 0.5f - Vector3f (0, 0, volume_size (2) / 2 * 1.2f);
+
+ Eigen::Affine3f pose = Eigen::Translation3f (t) * Eigen::AngleAxisf (R);
+
+ kinfu_->setInitialCameraPose (pose);
+ kinfu_->volume().setTsdfTruncDist (0.030f/*meters*/);
+ kinfu_->setIcpCorespFilteringParams (0.1f/*meters*/, sin ( pcl::deg2rad(20.f) ));
+ //kinfu_->setDepthTruncationForICP(3.f/*meters*/);
+ kinfu_->setCameraMovementThreshold(0.001f);
+
+ //Init KinFuLSApp
+ tsdf_cloud_ptr_ = pcl::PointCloud<pcl::PointXYZI>::Ptr (new pcl::PointCloud<pcl::PointXYZI>);
+ image_view_.raycaster_ptr_ = RayCaster::Ptr( new RayCaster(kinfu_->rows (), kinfu_->cols ()) );
+
+ scene_cloud_view_.cloud_viewer_.registerKeyboardCallback (keyboard_callback, (void*)this);
+ image_view_.viewerScene_.registerKeyboardCallback (keyboard_callback, (void*)this);
+ image_view_.viewerDepth_.registerKeyboardCallback (keyboard_callback, (void*)this);
+
+ scene_cloud_view_.toggleCube(volume_size);
+ frame_counter_ = 0;
+ enable_texture_extraction_ = false;
+
+ //~ float fx, fy, cx, cy;
+ //~ boost::shared_ptr<openni_wrapper::OpenNIDevice> d = ((pcl::OpenNIGrabber)source).getDevice ();
+ //~ kinfu_->getDepthIntrinsics (fx, fy, cx, cy);
+
+ float height = 480.0f;
+ float width = 640.0f;
+ screenshot_manager_.setCameraIntrinsics (pcl::device::kinfuLS::FOCAL_LENGTH, height, width);
+ snapshot_rate_ = snapshotRate;
+
+ Eigen::Matrix3f Rid = Eigen::Matrix3f::Identity (); // * AngleAxisf( pcl::deg2rad(-30.f), Vector3f::UnitX());
+ Eigen::Vector3f T = Vector3f (0, 0, -volume_size(0)*1.5f);
+ delta_lost_pose_ = Eigen::Translation3f (T) * Eigen::AngleAxisf (Rid);
+
+ }
+
+ ~KinFuLSApp()
+ {
+ if (evaluation_ptr_)
+ evaluation_ptr_->saveAllPoses(*kinfu_);
+ }
+
+ void
+ initCurrentFrameView ()
+ {
+ current_frame_cloud_view_ = boost::shared_ptr<CurrentFrameCloudView>(new CurrentFrameCloudView ());
+ current_frame_cloud_view_->cloud_viewer_.registerKeyboardCallback (keyboard_callback, (void*)this);
+ current_frame_cloud_view_->setViewerPose (kinfu_->getCameraPose ());
+ }
+
+ void
+ initRegistration ()
+ {
+ registration_ = capture_.providesCallback<pcl::ONIGrabber::sig_cb_openni_image_depth_image> ();
+ cout << "Registration mode: " << (registration_ ? "On" : "Off (not supported by source)") << endl;
+ }
+
+ void
+ toggleColorIntegration()
+ {
+ if(registration_)
+ {
+ const int max_color_integration_weight = 2;
+ kinfu_->initColorIntegration(max_color_integration_weight);
+ integrate_colors_ = true;
+ }
+ cout << "Color integration: " << (integrate_colors_ ? "On" : "Off ( requires registration mode )") << endl;
+ }
+
+ void
+ toggleIndependentCamera()
+ {
+ independent_camera_ = !independent_camera_;
+ cout << "Camera mode: " << (independent_camera_ ? "Independent" : "Bound to Kinect pose") << endl;
+ }
+
+ void
+ toggleEvaluationMode(const string& eval_folder, const string& match_file = string())
+ {
+ evaluation_ptr_ = Evaluation::Ptr( new Evaluation(eval_folder) );
+ if (!match_file.empty())
+ evaluation_ptr_->setMatchFile(match_file);
+
+ kinfu_->setDepthIntrinsics (evaluation_ptr_->fx, evaluation_ptr_->fy, evaluation_ptr_->cx, evaluation_ptr_->cy);
+ image_view_.raycaster_ptr_ = RayCaster::Ptr( new RayCaster(kinfu_->rows (), kinfu_->cols (), evaluation_ptr_->fx, evaluation_ptr_->fy, evaluation_ptr_->cx, evaluation_ptr_->cy) );
+ }
+
+ void execute(const PtrStepSz<const unsigned short>& depth, const PtrStepSz<const pcl::gpu::kinfuLS::PixelRGB>& rgb24, bool has_data)
+ {
+ bool has_image = false;
+ frame_counter_++;
+
+ was_lost_ = kinfu_->icpIsLost();
+
+ if (has_data)
+ {
+ depth_device_.upload (depth.data, depth.step, depth.rows, depth.cols);
+ if (integrate_colors_)
+ image_view_.colors_device_.upload (rgb24.data, rgb24.step, rgb24.rows, rgb24.cols);
+
+ {
+ SampledScopeTime fps(time_ms_);
+
+ //run kinfu algorithm
+ if (integrate_colors_)
+ has_image = (*kinfu_) (depth_device_, image_view_.colors_device_);
+ else
+ has_image = (*kinfu_) (depth_device_);
+ }
+
+ image_view_.showDepth (depth_);
+ //image_view_.showGeneratedDepth(kinfu_, kinfu_->getCameraPose());
+
+ }
+
+ if (scan_ || (!was_lost_ && kinfu_->icpIsLost ()) ) //if scan mode is OR and ICP just lost itself => show current volume as point cloud
+ {
+ scan_ = false;
+ //scene_cloud_view_.show (*kinfu_, integrate_colors_); // this triggers out of memory errors, so I comment it out for now (Raph)
+
+ if (scan_volume_)
+ {
+ cout << "Downloading TSDF volume from device ... " << flush;
+ // kinfu_->volume().downloadTsdfAndWeighs (tsdf_volume_.volumeWriteable (), tsdf_volume_.weightsWriteable ());
+ kinfu_->volume ().downloadTsdfAndWeightsLocal ();
+ // tsdf_volume_.setHeader (Eigen::Vector3i (pcl::device::kinfuLS::VOLUME_X, pcl::device::kinfuLS::VOLUME_Y, pcl::device::kinfuLS::VOLUME_Z), kinfu_->volume().getSize ());
+ kinfu_->volume ().setHeader (Eigen::Vector3i (pcl::device::kinfuLS::VOLUME_X, pcl::device::kinfuLS::VOLUME_Y, pcl::device::kinfuLS::VOLUME_Z), kinfu_->volume().getSize ());
+ // cout << "done [" << tsdf_volume_.size () << " voxels]" << endl << endl;
+ cout << "done [" << kinfu_->volume ().size () << " voxels]" << endl << endl;
+
+ cout << "Converting volume to TSDF cloud ... " << flush;
+ // tsdf_volume_.convertToTsdfCloud (tsdf_cloud_ptr_);
+ kinfu_->volume ().convertToTsdfCloud (tsdf_cloud_ptr_);
+ // cout << "done [" << tsdf_cloud_ptr_->size () << " points]" << endl << endl;
+ cout << "done [" << kinfu_->volume ().size () << " points]" << endl << endl;
+ }
+ else
+ cout << "[!] tsdf volume download is disabled" << endl << endl;
+ }
+
+
+ if (scan_mesh_)
+ {
+ scan_mesh_ = false;
+ scene_cloud_view_.showMesh(*kinfu_, integrate_colors_);
+ }
+
+ if (has_image)
+ {
+ Eigen::Affine3f viewer_pose = getViewerPose(scene_cloud_view_.cloud_viewer_);
+ image_view_.showScene (*kinfu_, rgb24, registration_, independent_camera_ ? &viewer_pose : 0);
+ }
+
+ if (current_frame_cloud_view_)
+ current_frame_cloud_view_->show (*kinfu_);
+
+ // if ICP is lost, we show the world from a farther view point
+ if(kinfu_->icpIsLost())
+ {
+ setViewerPose (scene_cloud_view_.cloud_viewer_, kinfu_->getCameraPose () * delta_lost_pose_);
+ }
+ else
+ if (!independent_camera_)
+ setViewerPose (scene_cloud_view_.cloud_viewer_, kinfu_->getCameraPose ());
+
+ if (enable_texture_extraction_ && !kinfu_->icpIsLost ()) {
+ if ( (frame_counter_ % snapshot_rate_) == 0 ) // Should be defined as a parameter. Done.
+ {
+ screenshot_manager_.saveImage (kinfu_->getCameraPose (), rgb24);
+ }
+ }
+
+ // display ICP state
+ scene_cloud_view_.displayICPState (*kinfu_, was_lost_);
+
+ }
+
+ void source_cb1(const boost::shared_ptr<openni_wrapper::DepthImage>& depth_wrapper)
+ {
+ {
+ boost::mutex::scoped_try_lock lock(data_ready_mutex_);
+ if (exit_ || !lock)
+ return;
+
+ depth_.cols = depth_wrapper->getWidth();
+ depth_.rows = depth_wrapper->getHeight();
+ depth_.step = depth_.cols * depth_.elemSize();
+
+ source_depth_data_.resize(depth_.cols * depth_.rows);
+ depth_wrapper->fillDepthImageRaw(depth_.cols, depth_.rows, &source_depth_data_[0]);
+ depth_.data = &source_depth_data_[0];
+ }
+ data_ready_cond_.notify_one();
+ }
+
+ void source_cb2(const boost::shared_ptr<openni_wrapper::Image>& image_wrapper, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_wrapper, float)
+ {
+ {
+ boost::mutex::scoped_try_lock lock(data_ready_mutex_);
+
+ if (exit_ || !lock)
+ {
+ return;
+ }
+
+ depth_.cols = depth_wrapper->getWidth();
+ depth_.rows = depth_wrapper->getHeight();
+ depth_.step = depth_.cols * depth_.elemSize();
+
+ source_depth_data_.resize(depth_.cols * depth_.rows);
+ depth_wrapper->fillDepthImageRaw(depth_.cols, depth_.rows, &source_depth_data_[0]);
+ depth_.data = &source_depth_data_[0];
+
+ rgb24_.cols = image_wrapper->getWidth();
+ rgb24_.rows = image_wrapper->getHeight();
+ rgb24_.step = rgb24_.cols * rgb24_.elemSize();
+
+ source_image_data_.resize(rgb24_.cols * rgb24_.rows);
+ image_wrapper->fillRGB(rgb24_.cols, rgb24_.rows, (unsigned char*)&source_image_data_[0]);
+ rgb24_.data = &source_image_data_[0];
+
+ }
+ data_ready_cond_.notify_one();
+ }
+
+ void source_cb3(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr & DC3)
+ {
+ {
+ //std::cout << "Giving colors1\n";
+ boost::mutex::scoped_try_lock lock(data_ready_mutex_);
+ std::cout << lock << std::endl;
+ if (exit_ || !lock)
+ return;
+ //std::cout << "Giving colors2\n";
+ int width = DC3->width;
+ int height = DC3->height;
+ depth_.cols = width;
+ depth_.rows = height;
+ depth_.step = depth_.cols * depth_.elemSize();
+ source_depth_data_.resize(depth_.cols * depth_.rows);
+
+ rgb24_.cols = width;
+ rgb24_.rows = height;
+ rgb24_.step = rgb24_.cols * rgb24_.elemSize();
+ source_image_data_.resize(rgb24_.cols * rgb24_.rows);
+
+ unsigned char *rgb = (unsigned char *) &source_image_data_[0];
+ unsigned short *depth = (unsigned short *) &source_depth_data_[0];
+
+ //std::cout << "Giving colors3\n";
+ for (int i=0; i<width*height; i++) {
+ PointXYZRGBA pt = DC3->at(i);
+ rgb[3*i +0] = pt.r;
+ rgb[3*i +1] = pt.g;
+ rgb[3*i +2] = pt.b;
+ depth[i] = pt.z/0.001;
+ }
+ //std::cout << "Giving colors4\n";
+ rgb24_.data = &source_image_data_[0];
+ depth_.data = &source_depth_data_[0];
+ }
+ data_ready_cond_.notify_one();
+ }
+
+ void
+ startMainLoop (bool triggered_capture)
+ {
+ using namespace openni_wrapper;
+ typedef boost::shared_ptr<DepthImage> DepthImagePtr;
+ typedef boost::shared_ptr<Image> ImagePtr;
+
+ boost::function<void (const ImagePtr&, const DepthImagePtr&, float constant)> func1 = boost::bind (&KinFuLSApp::source_cb2, this, _1, _2, _3);
+ boost::function<void (const DepthImagePtr&)> func2 = boost::bind (&KinFuLSApp::source_cb1, this, _1);
+ boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&) > func3 = boost::bind (&KinFuLSApp::source_cb3, this, _1);
+
+ bool need_colors = integrate_colors_ || registration_ || enable_texture_extraction_;
+
+ if ( pcd_source_ && !capture_.providesCallback<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)>() ) {
+ std::cout << "grabber doesn't provide pcl::PointCloud<pcl::PointXYZRGBA> callback !\n";
+ }
+
+ boost::signals2::connection c = pcd_source_? capture_.registerCallback (func3) : need_colors ? capture_.registerCallback (func1) : capture_.registerCallback (func2);
+
+ {
+ boost::unique_lock<boost::mutex> lock(data_ready_mutex_);
+
+ if (!triggered_capture)
+ capture_.start ();
+
+ while (!exit_ && !scene_cloud_view_.cloud_viewer_.wasStopped () && !image_view_.viewerScene_.wasStopped () && !this->kinfu_->isFinished ())
+ {
+ if (triggered_capture)
+ capture_.start(); // Triggers new frame
+
+ bool has_data = data_ready_cond_.timed_wait (lock, boost::posix_time::millisec(100));
+
+ try { this->execute (depth_, rgb24_, has_data); }
+ catch (const std::bad_alloc& /*e*/) { cout << "Bad alloc" << endl; break; }
+ catch (const std::exception& /*e*/) { cout << "Exception" << endl; break; }
+
+ scene_cloud_view_.cloud_viewer_.spinOnce (3);
+ //~ cout << "In main loop" << endl;
+ }
+ exit_ = true;
+ boost::this_thread::sleep (boost::posix_time::millisec (100));
+
+ if (!triggered_capture)
+ capture_.stop (); // Stop stream
+ }
+ c.disconnect();
+ }
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ writeCloud (int format) const
+ {
+ const SceneCloudView& view = scene_cloud_view_;
+
+ if (!view.cloud_ptr_->points.empty ())
+ {
+ if(view.point_colors_ptr_->points.empty()) // no colors
+ {
+ if (view.valid_combined_)
+ writeCloudFile (format, view.combined_ptr_);
+ else
+ writeCloudFile (format, view.cloud_ptr_);
+ }
+ else
+ {
+ if (view.valid_combined_)
+ writeCloudFile (format, merge<PointXYZRGBNormal>(*view.combined_ptr_, *view.point_colors_ptr_));
+ else
+ writeCloudFile (format, merge<PointXYZRGB>(*view.cloud_ptr_, *view.point_colors_ptr_));
+ }
+ }
+ }
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ writeMesh(int format) const
+ {
+ if (scene_cloud_view_.mesh_ptr_)
+ writePolygonMeshFile(format, *scene_cloud_view_.mesh_ptr_);
+ }
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ void
+ printHelp ()
+ {
+ cout << endl;
+ cout << "KinFu app hotkeys" << endl;
+ cout << "=================" << endl;
+ cout << " H : print this help" << endl;
+ cout << " Esc : exit" << endl;
+ cout << " T : take cloud" << endl;
+ cout << " A : take mesh" << endl;
+ cout << " M : toggle cloud exctraction mode" << endl;
+ cout << " N : toggle normals exctraction" << endl;
+ cout << " I : toggle independent camera mode" << endl;
+ cout << " B : toggle volume bounds" << endl;
+ cout << " * : toggle scene view painting ( requires registration mode )" << endl;
+ cout << " C : clear clouds" << endl;
+ cout << " 1,2,3 : save cloud to PCD(binary), PCD(ASCII), PLY(ASCII)" << endl;
+ cout << " 7,8 : save mesh to PLY, VTK" << endl;
+ cout << " X, V : TSDF volume utility" << endl;
+ cout << " L, l : On the next shift, KinFu will extract the whole current cube, extract the world and stop" << endl;
+ cout << " S, s : On the next shift, KinFu will extract the world and stop" << endl;
+ cout << endl;
+ }
+
+ bool exit_;
+ bool scan_;
+ bool scan_mesh_;
+ bool scan_volume_;
+
+ bool independent_camera_;
+ int frame_counter_;
+ bool enable_texture_extraction_;
+ pcl::kinfuLS::ScreenshotManager screenshot_manager_;
+ int snapshot_rate_;
+
+ bool registration_;
+ bool integrate_colors_;
+ bool pcd_source_;
+ float focal_length_;
+
+ pcl::Grabber& capture_;
+ KinfuTracker *kinfu_;
+
+ SceneCloudView scene_cloud_view_;
+ ImageView image_view_;
+ boost::shared_ptr<CurrentFrameCloudView> current_frame_cloud_view_;
+
+ KinfuTracker::DepthMap depth_device_;
+
+ pcl::PointCloud<pcl::PointXYZI>::Ptr tsdf_cloud_ptr_;
+
+ Evaluation::Ptr evaluation_ptr_;
+
+ boost::mutex data_ready_mutex_;
+ boost::condition_variable data_ready_cond_;
+
+ std::vector<pcl::gpu::kinfuLS::PixelRGB> source_image_data_;
+ std::vector<unsigned short> source_depth_data_;
+ PtrStepSz<const unsigned short> depth_;
+ PtrStepSz<const pcl::gpu::kinfuLS::PixelRGB> rgb24_;
+
+ Eigen::Affine3f delta_lost_pose_;
+
+ bool was_lost_;
+
+ int time_ms_;
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ static void
+ keyboard_callback (const visualization::KeyboardEvent &e, void *cookie)
+ {
+ KinFuLSApp* app = reinterpret_cast<KinFuLSApp*> (cookie);
+
+ int key = e.getKeyCode ();
+
+ if (e.keyUp ())
+ switch (key)
+ {
+ case 27: app->exit_ = true; break;
+ case (int)'t': case (int)'T': app->scan_ = true; break;
+ case (int)'a': case (int)'A': app->scan_mesh_ = true; break;
+ case (int)'h': case (int)'H': app->printHelp (); break;
+ case (int)'m': case (int)'M': app->scene_cloud_view_.toggleExtractionMode (); break;
+ case (int)'n': case (int)'N': app->scene_cloud_view_.toggleNormals (); break;
+ case (int)'c': case (int)'C': app->scene_cloud_view_.clearClouds (true); break;
+ case (int)'i': case (int)'I': app->toggleIndependentCamera (); break;
+ case (int)'b': case (int)'B': app->scene_cloud_view_.toggleCube(app->kinfu_->volume().getSize()); break;
+ case (int)'l': case (int)'L': app->kinfu_->performLastScan (); break;
+ case (int)'s': case (int)'S': app->kinfu_->extractAndSaveWorld (); break;
+ case (int)'r': case (int)'R': app->kinfu_->reset(); app->scene_cloud_view_.clearClouds(); break;
+ case (int)'7': case (int)'8': app->writeMesh (key - (int)'0'); break;
+ case (int)'1': case (int)'2': case (int)'3': app->writeCloud (key - (int)'0'); break;
+ case '*': app->image_view_.toggleImagePaint (); break;
+
+ case (int)'p': case (int)'P': app->kinfu_->setDisableICP(); break;
+
+ case (int)'x': case (int)'X':
+ app->scan_volume_ = !app->scan_volume_;
+ cout << endl << "Volume scan: " << (app->scan_volume_ ? "enabled" : "disabled") << endl << endl;
+ break;
+ case (int)'v': case (int)'V':
+ cout << "Saving TSDF volume to tsdf_volume.dat ... " << flush;
+ // app->tsdf_volume_.save ("tsdf_volume.dat", true);
+ app->kinfu_->volume ().save ("tsdf_volume.dat", true);
+ // cout << "done [" << app->tsdf_volume_.size () << " voxels]" << endl;
+ cout << "done [" << app->kinfu_->volume ().size () << " voxels]" << endl;
+ cout << "Saving TSDF volume cloud to tsdf_cloud.pcd ... " << flush;
+ pcl::io::savePCDFile<pcl::PointXYZI> ("tsdf_cloud.pcd", *app->tsdf_cloud_ptr_, true);
+ cout << "done [" << app->tsdf_cloud_ptr_->size () << " points]" << endl;
+ break;
+ default:
+ break;
+ }
+ }
+
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename CloudPtr> void
+writeCloudFile (int format, const CloudPtr& cloud_prt)
+{
+ if (format == KinFuLSApp::PCD_BIN)
+ {
+ cout << "Saving point cloud to 'cloud_bin.pcd' (binary)... " << flush;
+ pcl::io::savePCDFile ("cloud_bin.pcd", *cloud_prt, true);
+ }
+ else if (format == KinFuLSApp::PCD_ASCII)
+ {
+ cout << "Saving point cloud to 'cloud.pcd' (ASCII)... " << flush;
+ pcl::io::savePCDFile ("cloud.pcd", *cloud_prt, false);
+ }
+ else /* if (format == KinFuLSApp::PLY) */
+ {
+ cout << "Saving point cloud to 'cloud.ply' (ASCII)... " << flush;
+ pcl::io::savePLYFileASCII ("cloud.ply", *cloud_prt);
+
+ }
+ cout << "Done" << endl;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+writePolygonMeshFile (int format, const pcl::PolygonMesh& mesh)
+{
+ if (format == KinFuLSApp::MESH_PLY)
+ {
+ cout << "Saving mesh to to 'mesh.ply'... " << flush;
+ pcl::io::savePLYFile("mesh.ply", mesh);
+ }
+ else /* if (format == KinFuLSApp::MESH_VTK) */
+ {
+ cout << "Saving mesh to to 'mesh.vtk'... " << flush;
+ pcl::io::saveVTKFile("mesh.vtk", mesh);
+ }
+ cout << "Done" << endl;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+int
+print_cli_help ()
+{
+ cout << "\nKinFu parameters:" << endl;
+ cout << " --help, -h : print this message" << endl;
+ cout << " --registration, -r : try to enable registration (source needs to support this)" << endl;
+ cout << " --current-cloud, -cc : show current frame cloud" << endl;
+ cout << " --save-views, -sv : accumulate scene view and save in the end ( Requires OpenCV. Will cause 'bad_alloc' after some time )" << endl;
+ cout << " --registration, -r : enable registration mode" << endl;
+ cout << " --integrate-colors, -ic : enable color integration mode (allows to get cloud with colors)" << endl;
+ cout << " --extract-textures, -et : extract RGB PNG images to KinFuSnapshots folder." << endl;
+ cout << " --volume_size <in_meters>, -vs : define integration volume size" << endl;
+ cout << " --shifting_distance <in_meters>, -sd : define shifting threshold (distance target-point / cube center)" << endl;
+ cout << " --snapshot_rate <X_frames>, -sr : Extract RGB textures every <X_frames>. Default: 45 " << endl;
+ cout << endl << "";
+ cout << "Valid depth data sources:" << endl;
+ cout << " -dev <device> (default), -oni <oni_file>, -pcd <pcd_file or directory>" << endl;
+ cout << endl << "";
+ cout << " For RGBD benchmark (Requires OpenCV):" << endl;
+ cout << " -eval <eval_folder> [-match_file <associations_file_in_the_folder>]" << endl << endl;
+
+ return 0;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+int
+main (int argc, char* argv[])
+{
+ if (pc::find_switch (argc, argv, "--help") || pc::find_switch (argc, argv, "-h"))
+ return print_cli_help ();
+
+ int device = 0;
+ pc::parse_argument (argc, argv, "-gpu", device);
+ pcl::gpu::setDevice (device);
+ pcl::gpu::printShortCudaDeviceInfo (device);
+
+ // if (checkIfPreFermiGPU(device))
+ // return cout << endl << "Kinfu is supported only for Fermi and Kepler arhitectures. It is not even compiled for pre-Fermi by default. Exiting..." << endl, 1;
+
+ boost::shared_ptr<pcl::Grabber> capture;
+ bool triggered_capture = false;
+ bool pcd_input = false;
+
+ std::string eval_folder, match_file, openni_device, oni_file, pcd_dir;
+ try
+ {
+ if (pc::parse_argument (argc, argv, "-dev", openni_device) > 0)
+ {
+ capture.reset (new pcl::OpenNIGrabber (openni_device));
+ }
+ else if (pc::parse_argument (argc, argv, "-oni", oni_file) > 0)
+ {
+ triggered_capture = true;
+ bool repeat = false; // Only run ONI file once
+ capture.reset (new pcl::ONIGrabber (oni_file, repeat, !triggered_capture));
+ }
+ else if (pc::parse_argument (argc, argv, "-pcd", pcd_dir) > 0)
+ {
+ float fps_pcd = 15.0f;
+ pc::parse_argument (argc, argv, "-pcd_fps", fps_pcd);
+
+ vector<string> pcd_files = getPcdFilesInDir(pcd_dir);
+ // Sort the read files by name
+ sort (pcd_files.begin (), pcd_files.end ());
+ capture.reset (new pcl::PCDGrabber<pcl::PointXYZRGBA> (pcd_files, fps_pcd, false));
+ triggered_capture = true;
+ pcd_input = true;
+ }
+ else if (pc::parse_argument (argc, argv, "-eval", eval_folder) > 0)
+ {
+ //init data source latter
+ pc::parse_argument (argc, argv, "-match_file", match_file);
+ }
+ else
+ {
+ capture.reset( new pcl::OpenNIGrabber() );
+
+ //capture.reset( new pcl::ONIGrabber("d:/onis/20111013-224932.oni", true, true) );
+ //capture.reset( new pcl::ONIGrabber("d:/onis/reg20111229-180846.oni, true, true) );
+ //capture.reset( new pcl::ONIGrabber("/media/Main/onis/20111013-224932.oni", true, true) );
+ //capture.reset( new pcl::ONIGrabber("d:/onis/20111013-224551.oni", true, true) );
+ //capture.reset( new pcl::ONIGrabber("d:/onis/20111013-224719.oni", true, true) );
+ }
+ }
+ catch (const pcl::PCLException& /*e*/) { return cout << "Can't open depth source" << endl, -1; }
+
+ float volume_size = pcl::device::kinfuLS::VOLUME_SIZE;
+ pc::parse_argument (argc, argv, "--volume_size", volume_size);
+ pc::parse_argument (argc, argv, "-vs", volume_size);
+
+ float shift_distance = pcl::device::kinfuLS::DISTANCE_THRESHOLD;
+ pc::parse_argument (argc, argv, "--shifting_distance", shift_distance);
+ pc::parse_argument (argc, argv, "-sd", shift_distance);
+
+ int snapshot_rate = pcl::device::kinfuLS::SNAPSHOT_RATE; // defined in device.h
+ pc::parse_argument (argc, argv, "--snapshot_rate", snapshot_rate);
+ pc::parse_argument (argc, argv, "-sr", snapshot_rate);
+
+ KinFuLSApp app (*capture, volume_size, shift_distance, snapshot_rate);
+
+ if (pc::parse_argument (argc, argv, "-eval", eval_folder) > 0)
+ app.toggleEvaluationMode(eval_folder, match_file);
+
+ if (pc::find_switch (argc, argv, "--current-cloud") || pc::find_switch (argc, argv, "-cc"))
+ app.initCurrentFrameView ();
+
+ if (pc::find_switch (argc, argv, "--save-views") || pc::find_switch (argc, argv, "-sv"))
+ app.image_view_.accumulate_views_ = true; //will cause bad alloc after some time
+
+ if (pc::find_switch (argc, argv, "--registration") || pc::find_switch (argc, argv, "-r")) {
+ if (pcd_input) {
+ app.pcd_source_ = true;
+ app.registration_ = true; // since pcd provides registered rgbd
+ } else {
+ app.initRegistration();
+ }
+ }
+
+ if (pc::find_switch (argc, argv, "--integrate-colors") || pc::find_switch (argc, argv, "-ic"))
+ app.toggleColorIntegration();
+
+ if (pc::find_switch (argc, argv, "--extract-textures") || pc::find_switch (argc, argv, "-et"))
+ app.enable_texture_extraction_ = true;
+
+ // executing
+ if (triggered_capture)
+ std::cout << "Capture mode: triggered\n";
+ else
+ std::cout << "Capture mode: stream\n";
+
+ // set verbosity level
+ pcl::console::setVerbosityLevel(pcl::console::L_VERBOSE);
+ try { app.startMainLoop (triggered_capture); }
+ catch (const pcl::PCLException& /*e*/) { cout << "PCLException" << endl; }
+ catch (const std::bad_alloc& /*e*/) { cout << "Bad alloc" << endl; }
+ catch (const std::exception& /*e*/) { cout << "Exception" << endl; }
+
+ //~ #ifdef HAVE_OPENCV
+ //~ for (size_t t = 0; t < app.image_view_.views_.size (); ++t)
+ //~ {
+ //~ if (t == 0)
+ //~ {
+ //~ cout << "Saving depth map of first view." << endl;
+ //~ cv::imwrite ("./depthmap_1stview.png", app.image_view_.views_[0]);
+ //~ cout << "Saving sequence of (" << app.image_view_.views_.size () << ") views." << endl;
+ //~ }
+ //~ char buf[4096];
+ //~ sprintf (buf, "./%06d.png", (int)t);
+ //~ cv::imwrite (buf, app.image_view_.views_[t]);
+ //~ printf ("writing: %s\n", buf);
+ //~ }
+ //~ #endif
+ std::cout << "pcl_kinfu_largeScale exiting...\n";
+ return 0;
+}
--- /dev/null
+// Hacked version of kinfu_app.cpp
+// Simulates depth images
+// using pcl::simulation. these views are of a camera flying
+// around a short range object e.g. a tabletop from >3m aways
+// these are then fed into kinfu and a mesh reconstruction is made
+//
+// to run:
+// kinfu_app_sim -plyfile ~/projects/kmcl/kmcl/models/table_models/meta_model.ply
+// Maurice Fallon - mfallon <AT> mit.edu april 2012
+
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#define _CRT_SECURE_NO_DEPRECATE
+#include <iostream>
+
+// need to include GLEW net the top to avoid linking errors FOR PCL::SIMULATION:
+#include <GL/glew.h>
+
+#include <pcl/pcl_config.h>
+#ifdef OPENGL_IS_A_FRAMEWORK
+# include <OpenGL/gl.h>
+# include <OpenGL/glu.h>
+#else
+# include <GL/gl.h>
+# include <GL/glu.h>
+#endif
+#ifdef GLUT_IS_A_FRAMEWORK
+# include <GLUT/glut.h>
+#else
+# include <GL/glut.h>
+#endif
+
+#include <pcl/console/parse.h>
+#include <pcl/gpu/kinfu_large_scale/kinfu.h>
+#include <pcl/gpu/kinfu_large_scale/raycaster.h>
+#include <pcl/gpu/kinfu_large_scale/marching_cubes.h>
+#include <pcl/gpu/containers/initialization.h>
+
+#include <pcl/common/time.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/visualization/image_viewer.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/vtk_io.h>
+
+#include "openni_capture.h"
+#include "color_handler.h"
+#include "evaluation.h"
+
+#include <pcl/common/angles.h>
+
+//#include "tsdf_volume.h"
+//#include "tsdf_volume.hpp"
+
+#ifdef HAVE_OPENCV
+ #include <opencv2/highgui/highgui.hpp>
+ #include <opencv2/imgproc/imgproc.hpp>
+//#include "video_recorder.h"
+#endif
+typedef pcl::ScopeTime ScopeTimeT;
+
+#include <Eigen/Dense>
+#include <cmath>
+#include <iostream>
+#include <boost/shared_ptr.hpp>
+#ifdef _WIN32
+# define WIN32_LEAN_AND_MEAN
+# include <windows.h>
+#endif
+
+
+//SIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTART
+#include "pcl/common/common.h"
+#include "pcl/common/transforms.h"
+#include <pcl/console/print.h>
+#include <pcl/console/time.h>
+// define the following in order to eliminate the deprecated headers warning
+#define VTK_EXCLUDE_STRSTREAM_HEADERS
+#include <pcl/io/vtk_lib_io.h>
+//
+#include <pcl/simulation/camera.h>
+#include "pcl/simulation/model.h"
+#include "pcl/simulation/scene.h"
+#include "pcl/simulation/range_likelihood.h"
+// Writing PNG files:
+#include <opencv/cv.h>
+#include <opencv/highgui.h>
+//SIMENDSIMENDSIMENDSIMENDSIMENDSIMENDSIMEND
+
+using namespace std;
+using namespace pcl;
+using namespace pcl::gpu;
+using namespace Eigen;
+namespace pc = pcl::console;
+
+//SIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTART
+using namespace pcl::console;
+using namespace pcl::io;
+using namespace pcl::simulation;
+using namespace std;
+uint16_t t_gamma[2048];
+Scene::Ptr scene_;
+Camera::Ptr camera_;
+RangeLikelihood::Ptr range_likelihood_;
+//SIMENDSIMENDSIMENDSIMENDSIMENDSIMENDSIMEND
+
+
+namespace pcl
+{
+ namespace gpu
+ {
+ void paint3DView (const KinfuTracker::View& rgb24, KinfuTracker::View& view, float colors_weight = 0.5f);
+ void mergePointNormal (const DeviceArray<PointXYZ>& cloud, const DeviceArray<Normal>& normals, DeviceArray<PointNormal>& output);
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct SampledScopeTime : public StopWatch
+{
+ enum { EACH = 33 };
+ SampledScopeTime(int& time_ms, int i) : time_ms_(time_ms), i_(i) {}
+ ~SampledScopeTime()
+ {
+ time_ms_ += stopWatch_.getTime ();
+ if (i_ % EACH == 0 && i_)
+ {
+ cout << "Average frame time = " << time_ms_ / EACH << "ms ( " << 1000.f * EACH / time_ms_ << "fps )" << endl;
+ time_ms_ = 0;
+ }
+ }
+private:
+ StopWatch stopWatch_;
+ int& time_ms_;
+ int i_;
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+setViewerPose (visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
+{
+ Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f (0, 0, 0);
+ Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f (0, 0, 1) + pos_vector;
+ Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f (0, -1, 0);
+ viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2],
+ look_at_vector[0], look_at_vector[1], look_at_vector[2],
+ up_vector[0], up_vector[1], up_vector[2]);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+Eigen::Affine3f
+getViewerPose (visualization::PCLVisualizer& viewer)
+{
+ Eigen::Affine3f pose = viewer.getViewerPose();
+ Eigen::Matrix3f rotation = pose.linear();
+
+ Matrix3f axis_reorder;
+ axis_reorder << 0, 0, 1,
+ -1, 0, 0,
+ 0, -1, 0;
+
+ rotation = rotation * axis_reorder;
+ pose.linear() = rotation;
+ return pose;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//SIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTARTSIMSTART
+void
+write_depth_image(const float* depth_buffer)
+{
+ int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
+ uint8_t* depth_img = new uint8_t[npixels * 3];
+
+ float min_depth = depth_buffer[0];
+ float max_depth = depth_buffer[0];
+ for (int i=1; i<npixels; i++)
+ {
+ if (depth_buffer[i] < min_depth) min_depth = depth_buffer[i];
+ if (depth_buffer[i] > max_depth) max_depth = depth_buffer[i];
+ }
+
+ for (int y = 0; y < 480; ++y)
+ {
+ for (int x = 0; x < 640; ++x)
+ {
+ int i= y*640 + x ;
+ int i_in= (480-1 -y) *640 + x ; // flip up down
+
+
+ float zn = 0.7;
+ float zf = 20.0;
+ float d = depth_buffer[i_in];
+ float z = -zf*zn/((zf-zn)*(d - zf/(zf-zn)));
+ float b = 0.075;
+ float f = 580.0;
+ uint16_t kd = static_cast<uint16_t>(1090 - b*f/z*8);
+ if (kd < 0) kd = 0;
+ else if (kd>2047) kd = 2047;
+
+ int pval = t_gamma[kd];
+ int lb = pval & 0xff;
+ switch (pval>>8) {
+ case 0:
+ depth_img[3*i+2] = 255;
+ depth_img[3*i+1] = 255-lb;
+ depth_img[3*i+0] = 255-lb;
+ break;
+ case 1:
+ depth_img[3*i+2] = 255;
+ depth_img[3*i+1] = lb;
+ depth_img[3*i+0] = 0;
+ break;
+ case 2:
+ depth_img[3*i+2] = 255-lb;
+ depth_img[3*i+1] = 255;
+ depth_img[3*i+0] = 0;
+ break;
+ case 3:
+ depth_img[3*i+2] = 0;
+ depth_img[3*i+1] = 255;
+ depth_img[3*i+0] = lb;
+ break;
+ case 4:
+ depth_img[3*i+2] = 0;
+ depth_img[3*i+1] = 255-lb;
+ depth_img[3*i+0] = 255;
+ break;
+ case 5:
+ depth_img[3*i+2] = 0;
+ depth_img[3*i+1] = 0;
+ depth_img[3*i+0] = 255-lb;
+ break;
+ default:
+ depth_img[3*i+2] = 0;
+ depth_img[3*i+1] = 0;
+ depth_img[3*i+0] = 0;
+ break;
+ }
+ }
+ }
+
+ // Write to file:
+ IplImage *cv_ipl = cvCreateImage( cvSize(640 ,480), 8, 3);
+ cv::Mat cv_mat(cv_ipl);
+ cv_mat.data = depth_img;
+
+ std::stringstream ss;
+ ss <<"depth_image.png" ;
+ cv::imwrite(ss.str() , cv_mat);
+
+ delete [] depth_img;
+}
+
+
+void
+write_rgb_image(const uint8_t* rgb_buffer)
+{
+ int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
+ uint8_t* rgb_img = new uint8_t[npixels * 3];
+
+ for (int y = 0; y < 480; ++y)
+ {
+ for (int x = 0; x < 640; ++x)
+ {
+ int px= y*640 + x ;
+ int px_in= (480-1 -y) *640 + x ; // flip up down
+ rgb_img [3* (px) +0] = rgb_buffer[3*px_in+0];
+ rgb_img [3* (px) +1] = rgb_buffer[3*px_in+1];
+ rgb_img [3* (px) +2] = rgb_buffer[3*px_in+2];
+ }
+ }
+
+ // Write to file:
+ IplImage *cv_ipl = cvCreateImage( cvSize(640 ,480), 8, 3);
+ cv::Mat cv_mat(cv_ipl);
+ cv_mat.data = rgb_img ;
+// cv::imwrite("rgb_image.png", cv_mat);
+
+ std::stringstream ss;
+ ss <<"rgb_image.png" ;
+ cv::imwrite(ss.str() , cv_mat);
+
+ delete [] rgb_img;
+}
+
+
+void
+depthBufferToMM(const float* depth_buffer,unsigned short* depth_img)
+{
+ int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
+ // unsigned short * depth_img = new unsigned short[npixels ];
+ for (int y = 0; y < 480; ++y)
+ {
+ for (int x = 0; x < 640; ++x)
+ {
+ int i= y*640 + x ;
+ int i_in= (480-1 -y) *640 + x ; // flip up down
+ float zn = 0.7;
+ float zf = 20.0;
+ float d = depth_buffer[i_in];
+ unsigned short z_new = (unsigned short) floor( 1000*( -zf*zn/((zf-zn)*(d - zf/(zf-zn)))));
+
+ if (z_new < 0) z_new = 0;
+// else if (z_new>65535) z_new = 65535;
+ else if (z_new>5000) z_new = 0;
+
+ // if ( z_new < 18000){
+// cout << z_new << " " << d << " " << x << "\n";
+// }
+ depth_img[i] = z_new;
+ }
+ }
+}
+
+
+void
+write_depth_image_uint(unsigned short* depth_img)
+{
+ // Write to file:
+ IplImage *cv_ipl = cvCreateImage( cvSize(640 ,480), IPL_DEPTH_16U, 1);
+ cv::Mat cv_mat(cv_ipl);
+ cv_mat.data =(uchar *) depth_img;
+ cv::imwrite("depth_image_uint.png", cv_mat);
+}
+
+
+// display_tic_toc: a helper function which accepts a set of
+// timestamps and displays the elapsed time between them as
+// a fraction and time used [for profiling]
+void
+display_tic_toc (vector<double> &tic_toc,const string &fun_name)
+{
+ size_t tic_toc_size = tic_toc.size ();
+
+ double percent_tic_toc_last = 0;
+ double dtime = tic_toc[tic_toc_size-1] - tic_toc[0];
+ cout << "fraction_" << fun_name << ",";
+ for (size_t i = 0; i < tic_toc_size; i++)
+ {
+ double percent_tic_toc = (tic_toc[i] - tic_toc[0])/(tic_toc[tic_toc_size-1] - tic_toc[0]);
+ cout << percent_tic_toc - percent_tic_toc_last << ", ";
+ percent_tic_toc_last = percent_tic_toc;
+ }
+ cout << "\ntime_" << fun_name << ",";
+ double time_tic_toc_last = 0;
+ for (size_t i = 0; i < tic_toc_size; i++)
+ {
+ double percent_tic_toc = (tic_toc[i] - tic_toc[0])/(tic_toc[tic_toc_size-1] - tic_toc[0]);
+ cout << percent_tic_toc*dtime - time_tic_toc_last << ", ";
+ time_tic_toc_last = percent_tic_toc*dtime;
+ }
+ cout << "\ntotal_time_" << fun_name << " " << dtime << "\n";
+}
+
+void
+capture (Eigen::Isometry3d pose_in,unsigned short* depth_buffer_mm,const uint8_t* color_buffer)//, string point_cloud_fname)
+{
+ // No reference image - but this is kept for compatability with range_test_v2:
+ float* reference = new float[range_likelihood_->getRowHeight() * range_likelihood_->getColWidth()];
+ //const float* depth_buffer = range_likelihood_->getDepthBuffer();
+ // Copy one image from our last as a reference.
+ /*
+ for (int i=0, n=0; i<range_likelihood_->getRowHeight(); ++i)
+ {
+ for (int j=0; j<range_likelihood_->getColWidth(); ++j)
+ {
+ reference[n++] = 0;//depth_buffer[i*range_likelihood_->getWidth() + j];
+ }
+ }
+ */
+
+ std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
+ std::vector<float> scores;
+ int n = 1;
+ poses.push_back (pose_in);
+ // HACK: mfallon modified computeLikelihoods to only call render() (which is currently private)
+ // need to make render public and use it.
+ // For simulation it is used alone from the reset of range_likelihood_
+ range_likelihood_->computeLikelihoods (reference, poses, scores);
+
+ color_buffer =range_likelihood_->getColorBuffer();
+ const float* db_ptr= range_likelihood_->getDepthBuffer ();
+ range_likelihood_->addNoise ();
+ depthBufferToMM (db_ptr,depth_buffer_mm);
+
+ // Writing these images is a smaller computation draw relative to KinFu:
+ write_depth_image (db_ptr);
+ //write_depth_image_uint(depth_buffer_mm);
+ write_rgb_image (color_buffer);
+
+/*
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out (new pcl::PointCloud<pcl::PointXYZRGB>);
+ bool write_cloud=true;
+ if (write_cloud)
+ {
+ // Read Color Buffer from the GPU before creating PointCloud:
+ // By default the buffers are not read back from the GPU
+ range_likelihood_->getColorBuffer ();
+ range_likelihood_->getDepthBuffer ();
+
+ // Add noise directly to the CPU depth buffer
+ range_likelihood_->addNoise ();
+
+ // Optional argument to save point cloud in global frame:
+ // Save camera relative:
+ //range_likelihood_->getPointCloud(pc_out);
+ // Save in global frame - applying the camera frame:
+ //range_likelihood_->getPointCloud(pc_out,true,camera_->pose());
+ // Save in local frame
+ range_likelihood_->getPointCloud (pc_out,false,camera_->pose ());
+ // TODO: what to do when there are more than one simulated view?
+ std::cout << pc_out->points.size() << " points written to file\n";
+
+ pcl::PCDWriter writer;
+ //writer.write (point_cloud_fname, *pc_out, false); /// ASCII
+ writer.writeBinary (point_cloud_fname, *pc_out);
+ //cout << "finished writing file\n";
+ }
+ */
+
+ delete [] reference;
+}
+
+// Read in a 3D model
+void
+load_PolygonMesh_model (std::string polygon_file)
+{
+ pcl::PolygonMesh mesh; // (new pcl::PolygonMesh);
+ //pcl::io::loadPolygonFile("/home/mfallon/data/models/dalet/Darlek_modified_works.obj",mesh);
+ if (!pcl::io::loadPolygonFile (polygon_file, mesh)){
+ std::cout << "No ply file found, exiting" << std::endl;
+ exit(-1);
+ }
+ pcl::PolygonMesh::Ptr cloud (new pcl::PolygonMesh (mesh));
+
+ TriangleMeshModel::Ptr model = TriangleMeshModel::Ptr (new TriangleMeshModel (cloud));
+ scene_->add (model);
+
+ std::cout << "Just read " << polygon_file << std::endl;
+ std::cout << mesh.polygons.size () << " polygons and "
+ << mesh.cloud.data.size () << " triangles\n";
+}
+
+// A 'halo' camera - a circular ring of poses all pointing at a center point
+// @param: focus_center: the center points
+// @param: halo_r: radius of the ring
+// @param: halo_dz: elevation of the camera above/below focus_center's z value
+// @param: n_poses: number of generated poses
+void
+generate_halo(
+ std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > &poses,
+ Eigen::Vector3d focus_center,double halo_r,double halo_dz,int n_poses)
+{
+ for (double t=0;t < (2*M_PI);t =t + (2*M_PI)/ ((double) n_poses) ){
+ double x = halo_r*cos(t);
+ double y = halo_r*sin(t);
+ double z = halo_dz;
+ double pitch =atan2( halo_dz,halo_r);
+ double yaw = atan2(-y,-x);
+
+ Eigen::Isometry3d pose;
+ pose.setIdentity();
+ Eigen::Matrix3d m;
+ m = AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
+ * AngleAxisd(pitch, Eigen::Vector3d::UnitY())
+ * AngleAxisd(0, Eigen::Vector3d::UnitZ());
+
+ pose *=m;
+ Vector3d v(x,y,z);
+ v += focus_center;
+ pose.translation() = v;
+ poses.push_back(pose);
+ }
+ return ;
+}
+//SIMENDSIMENDSIMENDSIMENDSIMENDSIMENDSIMENDSIMENDSIMENDSIMENDSIMEND
+
+
+template<typename CloudT> void
+writeCloudFile (int format, const CloudT& cloud);
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+writePolygonMeshFile (int format, const pcl::PolygonMesh& mesh);
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+template<typename MergedT, typename PointT>
+typename PointCloud<MergedT>::Ptr merge(const PointCloud<PointT>& points, const PointCloud<RGB>& colors)
+{
+ typename PointCloud<MergedT>::Ptr merged_ptr(new PointCloud<MergedT>());
+
+ pcl::copyPointCloud (points, *merged_ptr);
+ //pcl::copyPointCloud (colors, *merged_ptr); why error?
+ //pcl::concatenateFields (points, colors, *merged_ptr); why error?
+
+ for (size_t i = 0; i < colors.size (); ++i)
+ merged_ptr->points[i].rgba = colors.points[i].rgba;
+
+ return merged_ptr;
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+boost::shared_ptr<pcl::PolygonMesh> convertToMesh(const DeviceArray<PointXYZ>& triangles)
+{
+ if (triangles.empty())
+ return boost::shared_ptr<pcl::PolygonMesh>();
+
+ pcl::PointCloud<pcl::PointXYZ> cloud;
+ cloud.width = (int)triangles.size();
+ cloud.height = 1;
+ triangles.download(cloud.points);
+
+ boost::shared_ptr<pcl::PolygonMesh> mesh_ptr( new pcl::PolygonMesh() );
+ pcl::toPCLPointCloud2(cloud, mesh_ptr->cloud);
+
+ mesh_ptr->polygons.resize (triangles.size() / 3);
+ for (size_t i = 0; i < mesh_ptr->polygons.size (); ++i)
+ {
+ pcl::Vertices v;
+ v.vertices.push_back(i*3+0);
+ v.vertices.push_back(i*3+2);
+ v.vertices.push_back(i*3+1);
+ mesh_ptr->polygons[i] = v;
+ }
+ return mesh_ptr;
+
+ cout << mesh_ptr->polygons.size () << " plys\n";
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct CurrentFrameCloudView
+{
+ CurrentFrameCloudView() : cloud_device_ (480, 640), cloud_viewer_ ("Frame Cloud Viewer")
+ {
+ cloud_ptr_ = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ>);
+
+ cloud_viewer_.setBackgroundColor (0, 0, 0.15);
+ cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_POINT_SIZE, 1);
+ cloud_viewer_.addCoordinateSystem (1.0);
+ cloud_viewer_.initCameraParameters ();
+ cloud_viewer_.setPosition (0, 500);
+ cloud_viewer_.setSize (640, 480);
+ cloud_viewer_.setCameraClipDistances (0.01, 10.01);
+ }
+
+ void
+ show (const KinfuTracker& kinfu)
+ {
+ kinfu.getLastFrameCloud (cloud_device_);
+
+ int c;
+ cloud_device_.download (cloud_ptr_->points, c);
+ cloud_ptr_->width = cloud_device_.cols ();
+ cloud_ptr_->height = cloud_device_.rows ();
+ cloud_ptr_->is_dense = false;
+
+ cloud_viewer_.removeAllPointClouds ();
+ cloud_viewer_.addPointCloud<PointXYZ>(cloud_ptr_);
+ cloud_viewer_.spinOnce ();
+ }
+
+ void
+ setViewerPose (const Eigen::Affine3f& viewer_pose) {
+ ::setViewerPose (cloud_viewer_, viewer_pose);
+ }
+
+ PointCloud<PointXYZ>::Ptr cloud_ptr_;
+ DeviceArray2D<PointXYZ> cloud_device_;
+ visualization::PCLVisualizer cloud_viewer_;
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct ImageView
+{
+ ImageView() : paint_image_ (false), accumulate_views_ (false)
+ {
+ viewerScene_.setWindowTitle ("View3D from ray tracing");
+ viewerScene_.setPosition (0, 0);
+ viewerDepth_.setWindowTitle ("Kinect Depth stream");
+ viewerDepth_.setPosition (640, 0);
+ //viewerColor_.setWindowTitle ("Kinect RGB stream");
+ }
+
+ void
+ showScene (KinfuTracker& kinfu, const PtrStepSz<const KinfuTracker::PixelRGB>& rgb24, bool registration, Eigen::Affine3f* pose_ptr = 0)
+ {
+ if (pose_ptr)
+ {
+ raycaster_ptr_->run(kinfu.volume(), *pose_ptr);
+ raycaster_ptr_->generateSceneView(view_device_);
+ }
+ else
+ kinfu.getImage (view_device_);
+
+ if (paint_image_ && registration && !pose_ptr)
+ {
+ colors_device_.upload (rgb24.data, rgb24.step, rgb24.rows, rgb24.cols);
+ paint3DView (colors_device_, view_device_);
+ }
+
+ int cols;
+ view_device_.download (view_host_, cols);
+ viewerScene_.showRGBImage (reinterpret_cast<unsigned char*> (&view_host_[0]), view_device_.cols (), view_device_.rows ());
+
+ //viewerColor_.showRGBImage ((unsigned char*)&rgb24.data, rgb24.cols, rgb24.rows);
+
+#ifdef HAVE_OPENCV
+ if (accumulate_views_)
+ {
+ views_.push_back (cv::Mat ());
+ cv::cvtColor (cv::Mat (480, 640, CV_8UC3, (void*)&view_host_[0]), views_.back (), CV_RGB2GRAY);
+ //cv::copy(cv::Mat(480, 640, CV_8UC3, (void*)&view_host_[0]), views_.back());
+ }
+#endif
+ }
+
+ void
+ showDepth (const PtrStepSz<const unsigned short>& depth)
+ {
+ viewerDepth_.showShortImage (depth.data, depth.cols, depth.rows, 0, 5000, true);
+ }
+
+ void
+ showGeneratedDepth (KinfuTracker& kinfu, const Eigen::Affine3f& pose)
+ {
+ raycaster_ptr_->run(kinfu.volume(), pose);
+ raycaster_ptr_->generateDepthImage(generated_depth_);
+
+ int c;
+ vector<unsigned short> data;
+ generated_depth_.download(data, c);
+
+ viewerDepth_.showShortImage (&data[0], generated_depth_.cols(), generated_depth_.rows(), 0, 5000, true);
+ }
+
+ void
+ toggleImagePaint()
+ {
+ paint_image_ = !paint_image_;
+ cout << "Paint image: " << (paint_image_ ? "On (requires registration mode)" : "Off") << endl;
+ }
+
+ bool paint_image_;
+ bool accumulate_views_;
+
+ visualization::ImageViewer viewerScene_;
+ visualization::ImageViewer viewerDepth_;
+ //visualization::ImageViewer viewerColor_;
+
+ KinfuTracker::View view_device_;
+ KinfuTracker::View colors_device_;
+ vector<KinfuTracker::PixelRGB> view_host_;
+
+ RayCaster::Ptr raycaster_ptr_;
+
+ KinfuTracker::DepthMap generated_depth_;
+
+#ifdef HAVE_OPENCV
+ vector<cv::Mat> views_;
+#endif
+};
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct SceneCloudView
+{
+ enum { GPU_Connected6 = 0, CPU_Connected6 = 1, CPU_Connected26 = 2 };
+
+ SceneCloudView() : extraction_mode_ (GPU_Connected6), compute_normals_ (false), valid_combined_ (false), cube_added_(false), cloud_viewer_ ("Scene Cloud Viewer")
+ {
+ cloud_ptr_ = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ>);
+ normals_ptr_ = PointCloud<Normal>::Ptr (new PointCloud<Normal>);
+ combined_ptr_ = PointCloud<PointNormal>::Ptr (new PointCloud<PointNormal>);
+ point_colors_ptr_ = PointCloud<RGB>::Ptr (new PointCloud<RGB>);
+
+ cloud_viewer_.setBackgroundColor (0, 0, 0);
+ cloud_viewer_.addCoordinateSystem (1.0);
+ cloud_viewer_.initCameraParameters ();
+ cloud_viewer_.setPosition (0, 500);
+ cloud_viewer_.setSize (640, 480);
+ cloud_viewer_.setCameraClipDistances (0.01, 10.01);
+
+ cloud_viewer_.addText ("H: print help", 2, 15, 20, 34, 135, 246);
+ }
+
+ void
+ show (KinfuTracker& kinfu, bool integrate_colors)
+ {
+ viewer_pose_ = kinfu.getCameraPose();
+
+ ScopeTimeT time ("PointCloud Extraction");
+ cout << "\nGetting cloud... " << flush;
+
+ valid_combined_ = false;
+
+ if (extraction_mode_ != GPU_Connected6) // So use CPU
+ {
+ kinfu.volume().fetchCloudHost (*cloud_ptr_, extraction_mode_ == CPU_Connected26);
+ }
+ else
+ {
+ DeviceArray<PointXYZ> extracted = kinfu.volume().fetchCloud (cloud_buffer_device_);
+
+ if (compute_normals_)
+ {
+ kinfu.volume().fetchNormals (extracted, normals_device_);
+ pcl::gpu::mergePointNormal (extracted, normals_device_, combined_device_);
+ combined_device_.download (combined_ptr_->points);
+ combined_ptr_->width = (int)combined_ptr_->points.size ();
+ combined_ptr_->height = 1;
+
+ valid_combined_ = true;
+ }
+ else
+ {
+ extracted.download (cloud_ptr_->points);
+ cloud_ptr_->width = (int)cloud_ptr_->points.size ();
+ cloud_ptr_->height = 1;
+ }
+
+ if (integrate_colors)
+ {
+ kinfu.colorVolume().fetchColors(extracted, point_colors_device_);
+ point_colors_device_.download(point_colors_ptr_->points);
+ point_colors_ptr_->width = (int)point_colors_ptr_->points.size ();
+ point_colors_ptr_->height = 1;
+ }
+ else
+ point_colors_ptr_->points.clear();
+ }
+ size_t points_size = valid_combined_ ? combined_ptr_->points.size () : cloud_ptr_->points.size ();
+ cout << "Done. Cloud size: " << points_size / 1000 << "K" << endl;
+
+ cloud_viewer_.removeAllPointClouds ();
+ if (valid_combined_)
+ {
+ visualization::PointCloudColorHandlerRGBHack<PointNormal> rgb(combined_ptr_, point_colors_ptr_);
+ cloud_viewer_.addPointCloud<PointNormal> (combined_ptr_, rgb, "Cloud");
+ cloud_viewer_.addPointCloudNormals<PointNormal>(combined_ptr_, 50);
+ }
+ else
+ {
+ visualization::PointCloudColorHandlerRGBHack<PointXYZ> rgb(cloud_ptr_, point_colors_ptr_);
+ cloud_viewer_.addPointCloud<PointXYZ> (cloud_ptr_, rgb);
+ }
+ }
+
+ void
+ toggleCube(const Eigen::Vector3f& size)
+ {
+ if (cube_added_)
+ cloud_viewer_.removeShape("cube");
+ else
+ cloud_viewer_.addCube(size*0.5, Eigen::Quaternionf::Identity(), size(0), size(1), size(2));
+
+ cube_added_ = !cube_added_;
+ }
+
+ void
+ toggleExtractionMode ()
+ {
+ extraction_mode_ = (extraction_mode_ + 1) % 3;
+
+ switch (extraction_mode_)
+ {
+ case 0: cout << "Cloud exctraction mode: GPU, Connected-6" << endl; break;
+ case 1: cout << "Cloud exctraction mode: CPU, Connected-6 (requires a lot of memory)" << endl; break;
+ case 2: cout << "Cloud exctraction mode: CPU, Connected-26 (requires a lot of memory)" << endl; break;
+ }
+ ;
+ }
+
+ void
+ toggleNormals ()
+ {
+ compute_normals_ = !compute_normals_;
+ cout << "Compute normals: " << (compute_normals_ ? "On" : "Off") << endl;
+ }
+
+ void
+ clearClouds (bool print_message = false)
+ {
+ cloud_viewer_.removeAllPointClouds ();
+ cloud_ptr_->points.clear ();
+ normals_ptr_->points.clear ();
+ if (print_message)
+ cout << "Clouds/Meshes were cleared" << endl;
+ }
+
+ void
+ showMesh(KinfuTracker& kinfu, bool /*integrate_colors*/)
+ {
+ ScopeTimeT time ("Mesh Extraction");
+ cout << "\nGetting mesh... " << flush;
+
+ if (!marching_cubes_)
+ marching_cubes_ = MarchingCubes::Ptr( new MarchingCubes() );
+
+ DeviceArray<PointXYZ> triangles_device = marching_cubes_->run(kinfu.volume(), triangles_buffer_device_);
+ mesh_ptr_ = convertToMesh(triangles_device);
+
+ cloud_viewer_.removeAllPointClouds ();
+ if (mesh_ptr_){
+ cloud_viewer_.addPolygonMesh(*mesh_ptr_);
+ cout << "mesh ptr exist\n";
+ }else{
+ cout << "mesh ptr no exist\n";
+ }
+
+ cout << "Done. Triangles number: " << triangles_device.size() / MarchingCubes::POINTS_PER_TRIANGLE / 1000 << "K" << endl;
+ }
+
+ int extraction_mode_;
+ bool compute_normals_;
+ bool valid_combined_;
+ bool cube_added_;
+
+ Eigen::Affine3f viewer_pose_;
+
+ visualization::PCLVisualizer cloud_viewer_;
+
+ PointCloud<PointXYZ>::Ptr cloud_ptr_;
+ PointCloud<Normal>::Ptr normals_ptr_;
+
+ DeviceArray<PointXYZ> cloud_buffer_device_;
+ DeviceArray<Normal> normals_device_;
+
+ PointCloud<PointNormal>::Ptr combined_ptr_;
+ DeviceArray<PointNormal> combined_device_;
+
+ DeviceArray<RGB> point_colors_device_;
+ PointCloud<RGB>::Ptr point_colors_ptr_;
+
+ MarchingCubes::Ptr marching_cubes_;
+ DeviceArray<PointXYZ> triangles_buffer_device_;
+
+ boost::shared_ptr<pcl::PolygonMesh> mesh_ptr_;
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct KinFuApp
+{
+ enum { PCD_BIN = 1, PCD_ASCII = 2, PLY = 3, MESH_PLY = 7, MESH_VTK = 8 };
+
+ KinFuApp(CaptureOpenNI& source, float vsz) : exit_ (false), scan_ (false), scan_mesh_(false), scan_volume_ (false), independent_camera_ (false),
+ registration_ (false), integrate_colors_ (false), capture_ (source)
+ {
+ //Init Kinfu Tracker
+ Eigen::Vector3f volume_size = Vector3f::Constant (vsz/*meters*/);
+
+ float f = capture_.depth_focal_length_VGA;
+ kinfu_.setDepthIntrinsics (f, f);
+ kinfu_.volume().setSize (volume_size);
+
+ Eigen::Matrix3f R = Eigen::Matrix3f::Identity (); // * AngleAxisf( pcl::deg2rad(-30.f), Vector3f::UnitX());
+ Eigen::Vector3f t = volume_size * 0.5f - Vector3f (0, 0, volume_size (2) / 2 * 1.2f);
+
+ Eigen::Affine3f pose = Eigen::Translation3f (t) * Eigen::AngleAxisf (R);
+
+ kinfu_.setInitalCameraPose (pose);
+ kinfu_.volume().setTsdfTruncDist (0.030f/*meters*/);
+ kinfu_.setIcpCorespFilteringParams (0.1f/*meters*/, sin ( pcl::deg2rad(20.f) ));
+ //kinfu_.setDepthTruncationForICP(5.f/*meters*/);
+ kinfu_.setCameraMovementThreshold(0.001f);
+
+ //Init KinfuApp
+ tsdf_cloud_ptr_ = pcl::PointCloud<pcl::PointXYZI>::Ptr (new pcl::PointCloud<pcl::PointXYZI>);
+ image_view_.raycaster_ptr_ = RayCaster::Ptr( new RayCaster(kinfu_.rows (), kinfu_.cols (), f, f) );
+
+ scene_cloud_view_.cloud_viewer_.registerKeyboardCallback (keyboard_callback, (void*)this);
+ image_view_.viewerScene_.registerKeyboardCallback (keyboard_callback, (void*)this);
+ image_view_.viewerDepth_.registerKeyboardCallback (keyboard_callback, (void*)this);
+
+ float diag = sqrt ((float)kinfu_.cols () * kinfu_.cols () + kinfu_.rows () * kinfu_.rows ());
+ scene_cloud_view_.cloud_viewer_.setCameraFieldOfView (2 * atan (diag / (2 * f)) * 1.5);
+
+ scene_cloud_view_.toggleCube(volume_size);
+ }
+
+ ~KinFuApp()
+ {
+ if (evaluation_ptr_)
+ evaluation_ptr_->saveAllPoses(kinfu_);
+ }
+
+ void
+ initCurrentFrameView ()
+ {
+ current_frame_cloud_view_ = boost::shared_ptr<CurrentFrameCloudView>(new CurrentFrameCloudView ());
+ current_frame_cloud_view_->cloud_viewer_.registerKeyboardCallback (keyboard_callback, (void*)this);
+ current_frame_cloud_view_->setViewerPose (kinfu_.getCameraPose ());
+ }
+
+ void
+ tryRegistrationInit ()
+ {
+ registration_ = capture_.setRegistration (true);
+ cout << "Registration mode: " << (registration_ ? "On" : "Off (not supported by source)") << endl;
+ }
+
+ void
+ toggleColorIntegration(bool force = false)
+ {
+ if (registration_ || force)
+ {
+ const int max_color_integration_weight = 2;
+ kinfu_.initColorIntegration(max_color_integration_weight);
+ integrate_colors_ = true;
+ }
+ cout << "Color integration: " << (integrate_colors_ ? "On" : "Off (not supported by source)") << endl;
+ }
+
+ void
+ toggleIndependentCamera()
+ {
+ independent_camera_ = !independent_camera_;
+ cout << "Camera mode: " << (independent_camera_ ? "Independent" : "Bound to Kinect pose") << endl;
+ }
+
+ void
+ toggleEvaluationMode(const string& eval_folder, const string& match_file = string())
+ {
+ evaluation_ptr_ = Evaluation::Ptr( new Evaluation(eval_folder) );
+ if (!match_file.empty())
+ evaluation_ptr_->setMatchFile(match_file);
+
+ kinfu_.setDepthIntrinsics (evaluation_ptr_->fx, evaluation_ptr_->fy, evaluation_ptr_->cx, evaluation_ptr_->cy);
+ image_view_.raycaster_ptr_ = RayCaster::Ptr( new RayCaster(kinfu_.rows (), kinfu_.cols (),
+ evaluation_ptr_->fx, evaluation_ptr_->fy, evaluation_ptr_->cx, evaluation_ptr_->cy) );
+ }
+
+ void
+ execute (int argc, char** argv, std::string plyfile)
+ {
+ PtrStepSz<const unsigned short> depth;
+ PtrStepSz<const KinfuTracker::PixelRGB> rgb24;
+ int time_ms = 0;
+ bool has_image = false;
+
+ // Create simulation environment:
+ int width = 640;
+ int height = 480;
+ for (int i=0; i<2048; i++)
+ {
+ float v = i/2048.0;
+ v = powf(v, 3)* 6;
+ t_gamma[i] = v*6*256;
+ }
+
+ glutInit (&argc, argv);
+ glutInitDisplayMode (GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);// was GLUT_RGBA
+ glutInitWindowPosition (10, 10);
+ glutInitWindowSize (10, 10);
+ //glutInitWindowSize (window_width_, window_height_);
+ glutCreateWindow ("OpenGL range likelihood");
+
+ GLenum err = glewInit ();
+ if (GLEW_OK != err)
+ {
+ std::cerr << "Error: " << glewGetErrorString (err) << std::endl;
+ exit (-1);
+ }
+
+ std::cout << "Status: Using GLEW " << glewGetString (GLEW_VERSION) << std::endl;
+
+ if (glewIsSupported ("GL_VERSION_2_0"))
+ std::cout << "OpenGL 2.0 supported" << std::endl;
+ else
+ {
+ std::cerr << "Error: OpenGL 2.0 not supported" << std::endl;
+ exit(1);
+ }
+ std::cout << "GL_MAX_VIEWPORTS: " << GL_MAX_VIEWPORTS << std::endl;
+
+ camera_ = Camera::Ptr (new Camera ());
+ scene_ = Scene::Ptr (new Scene ());
+ range_likelihood_ = RangeLikelihood::Ptr (new RangeLikelihood (1, 1, height, width, scene_));
+
+ // Actually corresponds to default parameters:
+ range_likelihood_->setCameraIntrinsicsParameters (640,480, 576.09757860,
+ 576.09757860, 321.06398107, 242.97676897);
+ range_likelihood_->setComputeOnCPU (false);
+ range_likelihood_->setSumOnCPU (true);
+ range_likelihood_->setUseColor (true);
+
+ camera_->set(0.471703, 1.59862, 3.10937, 0, 0.418879, -12.2129);
+ camera_->set_pitch(0.418879); // not sure why this is here:
+
+ cout << "About to read: "<< plyfile << endl;
+ load_PolygonMesh_model (plyfile);
+
+ // Generate a series of poses:
+ std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
+ Eigen::Vector3d focus_center(0,0,1.3);
+ // double halo_r = 4.0;
+ double halo_r = 1.5;
+ double halo_dz = 1.5; // was 2;
+ // 20 is too quick when adding noise:
+ // 50 is ok though
+ int n_poses=50;
+ int n_pose_stop = 10;
+ // above means make a circle of 50 poses, stop after the 10th i.e. 1/5 of a halo ring:
+ generate_halo(poses,focus_center,halo_r,halo_dz,n_poses);
+
+ unsigned short * disparity_buf_ = new unsigned short[width*height ];
+ const KinfuTracker::PixelRGB* color_buf_;
+ const uint8_t* color_buf_uint;
+
+ // loop though and create the mesh:
+ for (int i = 0; !exit_; ++i)
+ {
+ vector<double> tic_toc;
+ tic_toc.push_back(getTime());
+ double tic_main = getTime();
+
+ Eigen::Vector3d t(poses[i].translation());
+ Eigen::Quaterniond r(poses[i].rotation());
+ std::stringstream ss;
+ ss << t[0]<<", "<<t[1]<<", "<<t[2]<<" | "
+ <<r.w()<<", "<<r.x()<<", "<<r.y()<<", "<<r.z() ;
+ std::cout << i << ": " << ss.str() << " pose_simulatedposition\n";
+
+ capture (poses[i],disparity_buf_, color_buf_uint);//,ss.str());
+ color_buf_ = (const KinfuTracker::PixelRGB*) color_buf_uint;
+ PtrStepSz<const unsigned short> depth_sim = PtrStepSz<const unsigned short>(height, width, disparity_buf_, 2*width);
+ //cout << depth_sim.rows << " by " << depth_sim.cols << " | s: " << depth_sim.step << "\n";
+ // RGB-KinFu currently disabled for now - problems with color in KinFu apparently
+ // but this constructor might not be right either: not sure about step size
+ integrate_colors_=false;
+ PtrStepSz<const KinfuTracker::PixelRGB> rgb24_sim = PtrStepSz<const KinfuTracker::PixelRGB>(height, width, color_buf_, width);
+ tic_toc.push_back (getTime ());
+
+ if (1==0){ // live capture - probably doesnt work anymore, left in here for comparison:
+ bool has_frame = evaluation_ptr_ ? evaluation_ptr_->grab(i, depth) : capture_.grab (depth, rgb24);
+ if (!has_frame)
+ {
+ cout << "Can't grab" << endl;
+ break;
+ }
+
+ depth_device_.upload (depth.data, depth.step, depth.rows, depth.cols);
+ if (integrate_colors_)
+ image_view_.colors_device_.upload (rgb24.data, rgb24.step, rgb24.rows, rgb24.cols);
+
+ {
+ SampledScopeTime fps(time_ms, i);
+
+ //run kinfu algorithm
+ if (integrate_colors_)
+ has_image = kinfu_ (depth_device_, image_view_.colors_device_);
+ else
+ has_image = kinfu_ (depth_device_);
+ }
+ }else{ //simulate:
+
+ cout << " color: " << integrate_colors_ << "\n"; // integrate_colors_ seems to be zero
+ depth_device_.upload (depth_sim.data, depth_sim.step, depth_sim.rows, depth_sim.cols);
+ if (integrate_colors_){
+ image_view_.colors_device_.upload (rgb24_sim.data, rgb24_sim.step, rgb24_sim.rows, rgb24_sim.cols);
+ }
+
+ tic_toc.push_back (getTime ());
+
+ {
+ SampledScopeTime fps(time_ms, i);
+ //run kinfu algorithm
+ if (integrate_colors_)
+ has_image = kinfu_ (depth_device_, image_view_.colors_device_);
+ else
+ has_image = kinfu_ (depth_device_);
+ }
+
+ }
+
+ tic_toc.push_back (getTime ());
+
+ Eigen::Affine3f k_aff = kinfu_.getCameraPose();
+ Eigen::Matrix3f k_m;
+ k_m =k_aff.rotation();
+ Eigen::Quaternionf k_r;
+ k_r = Eigen::Quaternionf(k_m);
+ std::stringstream ss_k;
+ ss_k << k_aff(0,3) <<", "<< k_aff(1,3)<<", "<< k_aff(2,3)<<" | "
+ <<k_r.w()<<", "<<k_r.x()<<", "<<k_r.y()<<", "<<k_r.z() ;
+ std::cout << i << ": " << ss_k.str() << " pose_kinect\n";
+
+ // Everything below this is Visualization or I/O:
+ if (i >n_pose_stop){
+ int pause;
+ cout << "Enter a key to write Mesh file\n";
+ cin >> pause;
+
+ scene_cloud_view_.showMesh(kinfu_, integrate_colors_);
+ writeMesh(KinFuApp::MESH_VTK);
+ // writeMesh(KinFuApp::MESH_PLY);
+
+ if (scan_)
+ {
+ scan_ = false;
+ scene_cloud_view_.show (kinfu_, integrate_colors_);
+
+ if (scan_volume_)
+ {
+ // download tsdf volume
+ {
+ ScopeTimeT time ("tsdf volume download");
+ cout << "Downloading TSDF volume from device ... " << flush;
+ // kinfu_.volume().downloadTsdfAndWeighs (tsdf_volume_.volumeWriteable (), tsdf_volume_.weightsWriteable ());
+ kinfu_.volume ().downloadTsdfAndWeighsLocal ();
+ // tsdf_volume_.setHeader (Eigen::Vector3i (pcl::device::VOLUME_X, pcl::device::VOLUME_Y, pcl::device::VOLUME_Z), kinfu_.volume().getSize ());
+ kinfu_.volume ().setHeader (Eigen::Vector3i (pcl::device::VOLUME_X, pcl::device::VOLUME_Y, pcl::device::VOLUME_Z), kinfu_.volume().getSize ());
+ // cout << "done [" << tsdf_volume_.size () << " voxels]" << endl << endl;
+ cout << "done [" << kinfu_.volume ().size () << " voxels]" << endl << endl;
+ }
+ {
+ ScopeTimeT time ("converting");
+ cout << "Converting volume to TSDF cloud ... " << flush;
+ // tsdf_volume_.convertToTsdfCloud (tsdf_cloud_ptr_);
+ kinfu_.volume ().convertToTsdfCloud (tsdf_cloud_ptr_);
+ cout << "done [" << tsdf_cloud_ptr_->size () << " points]" << endl << endl;
+ }
+ }
+ else
+ cout << "[!] tsdf volume download is disabled" << endl << endl;
+ }
+
+ if (scan_mesh_)
+ {
+ scan_mesh_ = false;
+ scene_cloud_view_.showMesh(kinfu_, integrate_colors_);
+ }
+
+ if (has_image)
+ {
+ Eigen::Affine3f viewer_pose = getViewerPose(scene_cloud_view_.cloud_viewer_);
+// image_view_.showScene (kinfu_, rgb24, registration_, independent_camera_ ? &viewer_pose : 0);
+ image_view_.showScene (kinfu_, rgb24_sim, registration_, independent_camera_ ? &viewer_pose : 0);
+ }
+
+ if (current_frame_cloud_view_)
+ current_frame_cloud_view_->show (kinfu_);
+
+ image_view_.showDepth (depth_sim);
+ //image_view_.showDepth (depth);
+ // image_view_.showGeneratedDepth(kinfu_, kinfu_.getCameraPose());
+
+ if (!independent_camera_)
+ setViewerPose (scene_cloud_view_.cloud_viewer_, kinfu_.getCameraPose());
+
+ scene_cloud_view_.cloud_viewer_.spinOnce (3);
+
+ // As of April 2012, entering a key will end this program...
+ cout << "Paused after view\n";
+ cin >> pause;
+ }
+ double elapsed = (getTime() -tic_main);
+ cout << elapsed << " sec elapsed [" << (1/elapsed) << "]\n";
+ tic_toc.push_back (getTime ());
+ display_tic_toc (tic_toc, "kinfu_app_sim");
+ }
+ }
+
+ void
+ writeCloud (int format) const
+ {
+ const SceneCloudView& view = scene_cloud_view_;
+
+ if (!view.cloud_ptr_->points.empty ())
+ {
+ if(view.point_colors_ptr_->points.empty()) // no colors
+ {
+ if (view.valid_combined_)
+ writeCloudFile (format, view.combined_ptr_);
+ else
+ writeCloudFile (format, view.cloud_ptr_);
+ }
+ else
+ {
+ if (view.valid_combined_)
+ writeCloudFile (format, merge<PointXYZRGBNormal>(*view.combined_ptr_, *view.point_colors_ptr_));
+ else
+ writeCloudFile (format, merge<PointXYZRGB>(*view.cloud_ptr_, *view.point_colors_ptr_));
+ }
+ }
+ }
+
+ void
+ writeMesh(int format) const
+ {
+ if (scene_cloud_view_.mesh_ptr_) {
+ writePolygonMeshFile(format, *scene_cloud_view_.mesh_ptr_);
+ }
+ }
+
+ void
+ printHelp ()
+ {
+ cout << endl;
+ cout << "KinFu app hotkeys" << endl;
+ cout << "=================" << endl;
+ cout << " H : print this help" << endl;
+ cout << " Esc : exit" << endl;
+ cout << " T : take cloud" << endl;
+ cout << " A : take mesh" << endl;
+ cout << " M : toggle cloud exctraction mode" << endl;
+ cout << " N : toggle normals exctraction" << endl;
+ cout << " I : toggle independent camera mode" << endl;
+ cout << " B : toggle volume bounds" << endl;
+ cout << " * : toggle scene view painting ( requires registration mode )" << endl;
+ cout << " C : clear clouds" << endl;
+ cout << " 1,2,3 : save cloud to PCD(binary), PCD(ASCII), PLY(ASCII)" << endl;
+ cout << " 7,8 : save mesh to PLY, VTK" << endl;
+ cout << " X, V : TSDF volume utility" << endl;
+ cout << endl;
+ }
+
+ bool exit_;
+ bool scan_;
+ bool scan_mesh_;
+ bool scan_volume_;
+
+ bool independent_camera_;
+
+ bool registration_;
+ bool integrate_colors_;
+
+ CaptureOpenNI& capture_;
+ KinfuTracker kinfu_;
+
+ SceneCloudView scene_cloud_view_;
+ ImageView image_view_;
+ boost::shared_ptr<CurrentFrameCloudView> current_frame_cloud_view_;
+
+ KinfuTracker::DepthMap depth_device_;
+
+ // pcl::TSDFVolume<float, short> tsdf_volume_;
+ pcl::PointCloud<pcl::PointXYZI>::Ptr tsdf_cloud_ptr_;
+
+ Evaluation::Ptr evaluation_ptr_;
+
+ static void
+ keyboard_callback (const visualization::KeyboardEvent &e, void *cookie)
+ {
+ KinFuApp* app = reinterpret_cast<KinFuApp*> (cookie);
+
+ int key = e.getKeyCode ();
+
+ if (e.keyUp ())
+ switch (key)
+ {
+ case 27: app->exit_ = true; break;
+ case (int)'t': case (int)'T': app->scan_ = true; break;
+ case (int)'a': case (int)'A': app->scan_mesh_ = true; break;
+ case (int)'h': case (int)'H': app->printHelp (); break;
+ case (int)'m': case (int)'M': app->scene_cloud_view_.toggleExtractionMode (); break;
+ case (int)'n': case (int)'N': app->scene_cloud_view_.toggleNormals (); break;
+ case (int)'c': case (int)'C': app->scene_cloud_view_.clearClouds (true); break;
+ case (int)'i': case (int)'I': app->toggleIndependentCamera (); break;
+ case (int)'b': case (int)'B': app->scene_cloud_view_.toggleCube(app->kinfu_.volume().getSize()); break;
+ case (int)'7': case (int)'8': app->writeMesh (key - (int)'0'); break;
+ case (int)'1': case (int)'2': case (int)'3': app->writeCloud (key - (int)'0'); break;
+ case '*': app->image_view_.toggleImagePaint (); break;
+
+ case (int)'x': case (int)'X':
+ app->scan_volume_ = !app->scan_volume_;
+ cout << endl << "Volume scan: " << (app->scan_volume_ ? "enabled" : "disabled") << endl << endl;
+ break;
+ case (int)'v': case (int)'V':
+ cout << "Saving TSDF volume to tsdf_volume.dat ... " << flush;
+ // app->tsdf_volume_.save ("tsdf_volume.dat", true);
+ app->kinfu_.volume ().save ("tsdf_volume.dat", true);
+ //cout << "done [" << app->tsdf_volume_.size () << " voxels]" << endl;
+ cout << "done [" << app->app->kinfu_.volume ().size () << " voxels]" << endl;
+ cout << "Saving TSDF volume cloud to tsdf_cloud.pcd ... " << flush;
+ pcl::io::savePCDFile<pcl::PointXYZI> ("tsdf_cloud.pcd", *app->tsdf_cloud_ptr_, true);
+ cout << "done [" << app->tsdf_cloud_ptr_->size () << " points]" << endl;
+ break;
+
+ default:
+ break;
+ }
+ }
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+template<typename CloudPtr> void
+writeCloudFile (int format, const CloudPtr& cloud_prt)
+{
+ if (format == KinFuApp::PCD_BIN)
+ {
+ cout << "Saving point cloud to 'cloud_bin.pcd' (binary)... " << flush;
+ pcl::io::savePCDFile ("cloud_bin.pcd", *cloud_prt, true);
+ }
+ else
+ if (format == KinFuApp::PCD_ASCII)
+ {
+ cout << "Saving point cloud to 'cloud.pcd' (ASCII)... " << flush;
+ pcl::io::savePCDFile ("cloud.pcd", *cloud_prt, false);
+ }
+ else /* if (format == KinFuApp::PLY) */
+ {
+ cout << "Saving point cloud to 'cloud.ply' (ASCII)... " << flush;
+ pcl::io::savePLYFileASCII ("cloud.ply", *cloud_prt);
+
+ }
+ cout << "Done" << endl;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void
+writePolygonMeshFile (int format, const pcl::PolygonMesh& mesh)
+{
+ cout << "writePolygonMeshFile mf" << endl;
+
+ if (format == KinFuApp::MESH_PLY)
+ {
+ cout << "Saving mesh to to 'mesh.ply'... " << flush;
+ pcl::io::savePLYFile("mesh.ply", mesh);
+ }
+ else /* if (format == KinFuApp::MESH_VTK) */
+ {
+ cout << "Saving mesh to to 'mesh.vtk'... " << flush;
+ pcl::io::saveVTKFile("mesh.vtk", mesh);
+ }
+ cout << "Done" << endl;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+int
+print_cli_help ()
+{
+ cout << "\nKinfu app concole parameters help:" << endl;
+ cout << " --help, -h : print this message" << endl;
+ cout << " --registration, -r : try to enable registration ( requires source to support this )" << endl;
+ cout << " --current-cloud, -cc : show current frame cloud" << endl;
+ cout << " --save-views, -sv : accumulate scene view and save in the end ( Requires OpenCV. Will cause 'bad_alloc' after some time )" << endl;
+ cout << " --registration, -r : enable registration mode" << endl;
+ cout << " --integrate-colors, -ic : enable color integration mode ( allows to get cloud with colors )" << endl;
+ cout << " -volume_suze <size_in_meters> : define integration volume size" << endl;
+ cout << " -dev <deivce>, -oni <oni_file> : select depth source. Default will be selected if not specified" << endl;
+ cout << "";
+ cout << " For RGBD benchmark (Requires OpenCV):" << endl;
+ cout << " -eval <eval_folder> [-match_file <associations_file_in_the_folder>]" << endl;
+ cout << " For Simuation (Requires pcl::simulation):" << endl;
+ cout << " -plyfile : path to ply file for simulation testing " << endl;
+
+ return 0;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+int
+main (int argc, char* argv[])
+{
+ if (pc::find_switch (argc, argv, "--help") || pc::find_switch (argc, argv, "-h"))
+ return print_cli_help ();
+
+ int device = 0;
+ pc::parse_argument (argc, argv, "-gpu", device);
+ pcl::gpu::setDevice (device);
+ pcl::gpu::printShortCudaDeviceInfo (device);
+
+ if(checkIfPreFermiGPU(device))
+ return cout << endl << "Kinfu is not supported for pre-Fermi GPU architectures, and not built for them by default. Exiting..." << endl, 1;
+
+ CaptureOpenNI capture;
+
+ int openni_device = 0;
+ std::string oni_file, eval_folder, match_file;
+ if (pc::parse_argument (argc, argv, "-dev", openni_device) > 0)
+ {
+ capture.open (openni_device);
+ }
+ else
+ if (pc::parse_argument (argc, argv, "-oni", oni_file) > 0)
+ {
+ capture.open (oni_file);
+ }
+ else
+ if (pc::parse_argument (argc, argv, "-eval", eval_folder) > 0)
+ {
+ //init data source latter
+ pc::parse_argument (argc, argv, "-match_file", match_file);
+ }
+ else
+ {
+ //capture.open (openni_device);
+// capture.open ("/home/mfallon/gcc-4.4/kinfu/skeletonrec.oni");
+// capture.open ("/home/mfallon/gcc-4.4/kinfu/MultipleHands.oni");
+ //capture.open("d:/onis/20111013-224932.oni");
+ //capture.open("d:/onis/reg20111229-180846.oni");
+ //capture.open("d:/onis/white1.oni");
+ //capture.open("/media/Main/onis/20111013-224932.oni");
+ //capture.open("20111013-225218.oni");
+ //capture.open("d:/onis/20111013-224551.oni");
+ //capture.open("d:/onis/20111013-224719.oni");
+ }
+
+ //SIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIM
+ // read model for simulation mode:
+ std::string plyfile;
+ pc::parse_argument (argc, argv, "-plyfile", plyfile);
+ //SIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIMSIM
+
+ float volume_size = 3.f;
+ pc::parse_argument (argc, argv, "-volume_size", volume_size);
+
+ KinFuApp app (capture, volume_size);
+
+ if (pc::parse_argument (argc, argv, "-eval", eval_folder) > 0)
+ app.toggleEvaluationMode(eval_folder, match_file);
+
+ if (pc::find_switch (argc, argv, "--current-cloud") || pc::find_switch (argc, argv, "-cc"))
+ app.initCurrentFrameView ();
+
+ if (pc::find_switch (argc, argv, "--save-views") || pc::find_switch (argc, argv, "-sv"))
+ app.image_view_.accumulate_views_ = true; //will cause bad alloc after some time
+
+ if (pc::find_switch (argc, argv, "--registration") || pc::find_switch (argc, argv, "-r"))
+ app.tryRegistrationInit();
+
+ bool force = pc::find_switch (argc, argv, "-icf");
+ if (force || pc::find_switch (argc, argv, "--integrate-colors") || pc::find_switch (argc, argv, "-ic"))
+ app.toggleColorIntegration(force);
+
+
+ // executing
+ try { app.execute (argc, argv,plyfile); }
+ catch (const std::bad_alloc& /*e*/) { cout << "Bad alloc" << endl; }
+ catch (const std::exception& /*e*/) { cout << "Exception" << endl; }
+
+#ifdef HAVE_OPENCV
+ for (size_t t = 0; t < app.image_view_.views_.size (); ++t)
+ {
+ if (t == 0)
+ {
+ cout << "Saving depth map of first view." << endl;
+ cv::imwrite ("./depthmap_1stview.png", app.image_view_.views_[0]);
+ cout << "Saving sequence of (" << app.image_view_.views_.size () << ") views." << endl;
+ }
+ char buf[4096];
+ sprintf (buf, "./%06d.png", (int)t);
+ cv::imwrite (buf, app.image_view_.views_[t]);
+ printf ("writing: %s\n", buf);
+ }
+#endif
+ return 0;
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#pragma once
+
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/containers/kernel_containers.h>
+
+#include <boost/shared_ptr.hpp>
+#include <string>
+
+#include <pcl/gpu/kinfu_large_scale/kinfu.h>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace kinfuLS
+ {
+ class CaptureOpenNI
+ {
+ public:
+ typedef pcl::gpu::kinfuLS::PixelRGB RGB;
+
+ enum { PROP_OPENNI_REGISTRATION_ON = 104 };
+
+
+ CaptureOpenNI();
+ CaptureOpenNI(int device);
+ CaptureOpenNI(const std::string& oni_filename);
+
+ void open(int device);
+ void open(const std::string& oni_filename);
+ void release();
+
+ ~CaptureOpenNI();
+
+ bool grab (PtrStepSz<const unsigned short>& depth, PtrStepSz<const RGB>& rgb24);
+
+ //parameters taken from camera/oni
+ float depth_focal_length_VGA;
+ float baseline; // mm
+ int shadow_value;
+ int no_sample_value;
+ double pixelSize; //mm
+
+ unsigned short max_depth; //mm
+
+ bool setRegistration (bool value = false);
+ private:
+ struct Impl;
+ boost::shared_ptr<Impl> impl_;
+ void getParams ();
+
+ };
+ }
+ }
+};
--- /dev/null
+ /*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+#include <pcl/gpu/kinfu_large_scale/device.h>
+#include <pcl/gpu/kinfu_large_scale/standalone_marching_cubes.h>
+#include <pcl/gpu/kinfu_large_scale/impl/standalone_marching_cubes.hpp>
+#include <pcl/gpu/kinfu_large_scale/world_model.h>
+#include <pcl/gpu/kinfu_large_scale/impl/world_model.hpp>
+
+#include <pcl/console/parse.h>
+
+int
+print_help ()
+{
+ std::cout << "\nUsage:" << std::endl;
+ std::cout << " pcl_kinfu_largeScale_mesh_output <tsdf_world.pcd> [options]" << std::endl << std::endl ;
+
+ std::cout << "\nAvailable options:" << std::endl;
+ std::cout << " --help, -h : print this message" << std::endl;
+ std::cout << " --volume_size <in_meters> : define integration volume size. MUST match the size used when scanning." << std::endl << std::endl;
+
+ return 0;
+}
+
+
+int
+main (int argc, char** argv)
+{
+ if (pcl::console::find_switch (argc, argv, "--help") || pcl::console::find_switch (argc, argv, "-h"))
+ return print_help ();
+
+ //Reading input cloud
+ pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZI>);
+
+ if (argc < 2) {
+ PCL_ERROR ("No pcd file to read... Exiting...\n");
+ print_help ();
+ return (-1);
+ }
+
+ if (pcl::io::loadPCDFile<pcl::PointXYZI> (argv[1], *cloud) == -1) //* load the file
+ {
+ PCL_ERROR ("Couldn't read file %s \n", argv[1]);
+ print_help ();
+ return (-1);
+ }
+
+ try {
+
+ // Creating world model object
+ pcl::kinfuLS::WorldModel<pcl::PointXYZI> wm;
+
+ //Adding current cloud to the world model
+ wm.addSlice (cloud);
+
+ std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> clouds;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > transforms;
+
+ //Get world as a vector of cubes
+ wm.getWorldAsCubes (pcl::device::kinfuLS::VOLUME_X, clouds, transforms, 0.025); // 2.5% overlapp (12 cells with a 512-wide cube)
+
+ //Creating the standalone marching cubes instance
+ float volume_size = pcl::device::kinfuLS::VOLUME_SIZE;
+ pcl::console::parse_argument (argc, argv, "--volume_size", volume_size);
+ pcl::console::parse_argument (argc, argv, "-vs", volume_size);
+
+ PCL_WARN ("Processing world with volume size set to %.2f meters\n", volume_size);
+
+ pcl::gpu::kinfuLS::StandaloneMarchingCubes<pcl::PointXYZI> m_cubes (pcl::device::kinfuLS::VOLUME_X, pcl::device::kinfuLS::VOLUME_Y, pcl::device::kinfuLS::VOLUME_Z, volume_size);
+
+ //~ //Creating the output
+ //~ boost::shared_ptr<pcl::PolygonMesh> mesh_ptr_;
+ //~ std::vector< boost::shared_ptr<pcl::PolygonMesh> > meshes;
+
+ m_cubes.getMeshesFromTSDFVector (clouds, transforms);
+
+ PCL_INFO ("Done!\n");
+ return (0);
+
+ }
+ catch (const pcl::PCLException& /*e*/) { PCL_ERROR ("PCLException... Exiting...\n"); }
+ catch (const std::bad_alloc& /*e*/) { PCL_ERROR ("Bad alloc... Exiting...\n"); }
+ catch (const std::exception& /*e*/) { PCL_ERROR ("Exception... Exiting...\n"); }
+ return (-1);
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2012 Sudarshan Srinivasan <sudarshan85@gmail.com>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Raphael Favier, Technical University Eindhoven, (r.mysurname < aT > tue.nl)
+ */
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/io/openni_grabber.h>
+#include <boost/thread/condition.hpp>
+#include <boost/circular_buffer.hpp>
+#include <csignal>
+#include <pcl/io/pcd_io.h>
+#include <pcl/common/time.h> //fps calculations
+
+#include <pcl/gpu/containers/kernel_containers.h>
+#include <pcl/io/png_io.h>
+#include <ctime>
+#include <pcl/console/print.h>
+
+#define FPS_CALC(_WHAT_) \
+do \
+{ \
+ static unsigned count = 0;\
+ static double last = pcl::getTime ();\
+ double now = pcl::getTime (); \
+ ++count; \
+ if (now - last >= 1.0) \
+ { \
+ std::cerr << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+}while(false)
+
+bool is_done = false;
+boost::mutex io_mutex;
+
+const int BUFFER_SIZE = 1000;
+static int counter = 1;
+//////////////////////////////////////////////////////////////////////////////////////////
+class MapsBuffer
+{
+ public:
+
+ struct PixelRGB
+ {
+ unsigned char r, g, b;
+ };
+
+ struct MapsRgb
+ {
+ pcl::gpu::PtrStepSz<const PixelRGB> rgb_;
+ pcl::gpu::PtrStepSz<const unsigned short> depth_;
+ double time_stamp_;
+ };
+
+ MapsBuffer () {}
+
+ bool
+ pushBack (boost::shared_ptr<const MapsRgb>); // thread-save wrapper for push_back() method of ciruclar_buffer
+
+ boost::shared_ptr<const MapsRgb>
+ getFront (bool); // thread-save wrapper for front() method of ciruclar_buffer
+
+ inline bool
+ isFull ()
+ {
+ boost::mutex::scoped_lock buff_lock (bmutex_);
+ return (buffer_.full ());
+ }
+
+ inline bool
+ isEmpty ()
+ {
+ boost::mutex::scoped_lock buff_lock (bmutex_);
+ return (buffer_.empty ());
+ }
+
+ inline int
+ getSize ()
+ {
+ boost::mutex::scoped_lock buff_lock (bmutex_);
+ return (int (buffer_.size ()));
+ }
+
+ inline int
+ getCapacity ()
+ {
+ return (int (buffer_.capacity ()));
+ }
+
+ inline void
+ setCapacity (int buff_size)
+ {
+ boost::mutex::scoped_lock buff_lock (bmutex_);
+ buffer_.set_capacity (buff_size);
+ }
+
+ private:
+ MapsBuffer (const MapsBuffer&); // Disabled copy constructor
+ MapsBuffer& operator =(const MapsBuffer&); // Disabled assignment operator
+
+ boost::mutex bmutex_;
+ boost::condition_variable buff_empty_;
+ boost::circular_buffer<boost::shared_ptr<const MapsRgb> > buffer_;
+
+};
+
+
+//////////////////////////////////////////////////////////////////////////////////////////
+bool
+MapsBuffer::pushBack(boost::shared_ptr<const MapsRgb> maps_rgb )
+{
+ bool retVal = false;
+ {
+ boost::mutex::scoped_lock buff_lock (bmutex_);
+ if (!buffer_.full ())
+ retVal = true;
+ buffer_.push_back (maps_rgb);
+ }
+ buff_empty_.notify_one ();
+ return (retVal);
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////
+boost::shared_ptr< const MapsBuffer::MapsRgb >
+MapsBuffer::getFront(bool print)
+{
+ boost::shared_ptr< const MapsBuffer::MapsRgb > depth_rgb;
+ {
+ boost::mutex::scoped_lock buff_lock (bmutex_);
+ while (buffer_.empty ())
+ {
+ if (is_done)
+ break;
+ {
+ boost::mutex::scoped_lock io_lock (io_mutex);
+ //std::cout << "No data in buffer_ yet or buffer is empty." << std::endl;
+ }
+ buff_empty_.wait (buff_lock);
+ }
+ depth_rgb = buffer_.front ();
+ buffer_.pop_front ();
+ }
+
+ if(print)
+ PCL_INFO("%d maps left in the buffer...\n", buffer_.size ());
+
+ return (depth_rgb);
+}
+
+
+MapsBuffer buff;
+std::vector<unsigned short> source_depth_data_;
+std::vector<MapsBuffer::PixelRGB> source_image_data_;
+
+
+//////////////////////////////////////////////////////////////////////////////////////////
+void
+writeToDisk (const boost::shared_ptr<const MapsBuffer::MapsRgb>& map_rbg)
+{
+ //save rgb
+ std::stringstream ss;
+ ss.precision(std::numeric_limits<double>::digits10 + 2);
+ ss.width(20);
+ ss << map_rbg->time_stamp_;
+
+ std::string prefix = "./frame-";
+ std::string ext = " -rgb.png";
+ std::string fname = prefix + ss.str () + ext;
+ pcl::io::saveRgbPNGFile (fname, (unsigned char*)map_rbg->rgb_.data, 640,480);
+
+ // save depth map
+ ext = " -depth.png";
+ fname = prefix + ss.str () + ext;
+ pcl::io::saveShortPNGFile (fname, (short unsigned int*)map_rbg->depth_.data, 640,480,1);
+
+ counter++;
+ FPS_CALC ("maps write");
+}
+
+void
+grabberMapsCallBack(const boost::shared_ptr<openni_wrapper::Image>& image_wrapper, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_wrapper, float)
+{
+ MapsBuffer::MapsRgb rgb_depth;
+ rgb_depth.time_stamp_ = pcl::getTime();
+
+ // fill in depth values
+ rgb_depth.depth_.cols = depth_wrapper->getWidth();
+ rgb_depth.depth_.rows = depth_wrapper->getHeight();
+ rgb_depth.depth_.step = rgb_depth.depth_.cols * rgb_depth.depth_.elemSize();
+ source_depth_data_.resize(rgb_depth.depth_.cols * rgb_depth.depth_.rows);
+ depth_wrapper->fillDepthImageRaw(rgb_depth.depth_.cols, rgb_depth.depth_.rows, &source_depth_data_[0]);
+ rgb_depth.depth_.data = &source_depth_data_[0];
+
+ // fill in rgb values
+ rgb_depth.rgb_.cols = image_wrapper->getWidth();
+ rgb_depth.rgb_.rows = image_wrapper->getHeight();
+ rgb_depth.rgb_.step = rgb_depth.rgb_.cols * rgb_depth.rgb_.elemSize();
+ source_image_data_.resize(rgb_depth.rgb_.cols * rgb_depth.rgb_.rows);
+ image_wrapper->fillRGB(rgb_depth.rgb_.cols, rgb_depth.rgb_.rows, (unsigned char*)&source_image_data_[0]);
+ rgb_depth.rgb_.data = &source_image_data_[0];
+
+ // make it a shared pointer
+ boost::shared_ptr<MapsBuffer::MapsRgb> ptr (new MapsBuffer::MapsRgb (rgb_depth));
+
+ // push to buffer
+ if (!buff.pushBack (ptr))
+ {
+ {
+ boost::mutex::scoped_lock io_lock(io_mutex);
+ PCL_WARN ("Warning! Buffer was full, overwriting data\n");
+ }
+ }
+ FPS_CALC ("kinect callback");
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Procuder thread function
+void
+grabAndSend ()
+{
+ pcl::Grabber* interface = new pcl::OpenNIGrabber ();
+ //boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& )> f = boost::bind(&grabberCallBack, _1);
+
+ boost::function<void (const boost::shared_ptr<openni_wrapper::Image>&, const boost::shared_ptr<openni_wrapper::DepthImage>&, float constant)> f = boost::bind (&grabberMapsCallBack, _1, _2, _3);
+
+
+ interface->registerCallback (f);
+ interface->start ();
+
+ while (true)
+ {
+ if (is_done)
+ break;
+ boost::this_thread::sleep (boost::posix_time::seconds (1));
+ }
+ interface->stop ();
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Consumer thread function
+void
+receiveAndProcess ()
+{
+ while (true)
+ {
+ if (is_done)
+ break;
+ writeToDisk (buff.getFront (false));
+ }
+
+ {
+ boost::mutex::scoped_lock io_lock (io_mutex);
+ PCL_INFO ("Writing remaing %d maps in the buffer to disk...\n", buff.getSize ());
+ }
+ while (!buff.isEmpty ())
+ {
+ writeToDisk (buff.getFront (true));
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+void
+ctrlC (int)
+{
+ boost::mutex::scoped_lock io_lock (io_mutex);
+ std::cout << std::endl;
+ PCL_WARN ("Ctrl-C detected, exit condition set to true\n");
+ is_done = true;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+int
+main (int argc, char** argv)
+{
+ int buff_size = BUFFER_SIZE;
+ if (argc == 2)
+ {
+ buff_size = atoi (argv[1]);
+ std::cout << "Setting buffer size to " << buff_size << " frames " << std::endl;
+ }
+ else
+ {
+ std::cout << "Using default buffer size of " << buff_size << " frames " << std::endl;
+ }
+ buff.setCapacity (buff_size);
+ std::cout << "Starting the producer and consumer threads..." << std::endl;
+ std::cout << "Press Ctrl-C to end" << std::endl;
+ boost::thread producer (grabAndSend);
+ boost::this_thread::sleep (boost::posix_time::seconds (2));
+ boost::thread consumer (receiveAndProcess);
+ boost::thread consumer2 (receiveAndProcess);
+ boost::thread consumer3 (receiveAndProcess);
+ signal (SIGINT, ctrlC);
+ producer.join ();
+ {
+ boost::mutex::scoped_lock io_lock (io_mutex);
+ PCL_WARN ("Producer done\n");
+ }
+ consumer.join ();
+ consumer2.join();
+ consumer3.join();
+
+ PCL_WARN ("Consumers done\n");
+ return (0);
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Bernhard Zeisl, (myname.mysurname@inf.ethz.ch)
+ */
+
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/common/file_io.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/visualization/point_cloud_handlers.h>
+
+#include <pcl/gpu/kinfu_large_scale/kinfu.h>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/containers/initialization.h>
+#include "openni_capture.h"
+
+#include "tsdf_volume.h"
+#include "tsdf_volume.hpp"
+
+using namespace std;
+namespace pc = pcl::console;
+
+typedef pcl::PointXYZ PointT;
+typedef float VoxelT;
+typedef short WeightT;
+
+string cloud_file = "cloud.pcd";
+string volume_file = "tsdf_volume.dat";
+
+double min_trunc_dist = 30.0f;
+
+bool quit = false, save = false;
+bool extract_cloud_volume = false;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+/// DEVICE_VOLUME
+
+/** \brief Class for storing and handling the TSDF Volume on the GPU */
+class DeviceVolume
+{
+public:
+
+ typedef boost::shared_ptr<DeviceVolume> Ptr;
+ typedef boost::shared_ptr<const DeviceVolume> ConstPtr;
+
+ /** \brief Constructor
+ * param[in] volume_size size of the volume in mm
+ * param[in] volume_res volume grid resolution (typically device::VOLUME_X x device::VOLUME_Y x device::VOLUME_Z)
+ */
+ DeviceVolume (const Eigen::Vector3f &volume_size, const Eigen::Vector3i &volume_res)
+ : volume_size_ (volume_size)
+ {
+ // initialize GPU
+ device_volume_.create (volume_res[1] * volume_res[2], volume_res[0]); // (device::VOLUME_Y * device::VOLUME_Z, device::VOLUME_X)
+ pcl::device::initVolume (device_volume_);
+
+ // truncation distance
+ Eigen::Vector3f voxel_size = volume_size.array() / volume_res.array().cast<float>();
+ trunc_dist_ = max ((float)min_trunc_dist, 2.1f * max (voxel_size[0], max (voxel_size[1], voxel_size[2])));
+ };
+
+ /** \brief Creates the TSDF volume on the GPU
+ * param[in] depth depth readings from the sensor
+ * param[in] intr camaera intrinsics
+ */
+ void
+ createFromDepth (const pcl::device::PtrStepSz<const unsigned short> &depth, const pcl::device::Intr &intr);
+
+ /** \brief Downloads the volume from the GPU
+ * param[out] volume volume structure where the data is written to (size needs to be appropriately set beforehand (is checked))
+ */
+ bool
+ getVolume (pcl::TSDFVolume<VoxelT, WeightT>::Ptr &volume);
+
+ /** \brief Generates and returns a point cloud form the implicit surface in the TSDF volume
+ * param[out] cloud point cloud containing the surface
+ */
+ bool
+ getCloud (pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);
+
+private:
+
+ template<class D, class Matx> D&
+ device_cast (Matx& matx)
+ {
+ return (*reinterpret_cast<D*>(matx.data ()));
+ };
+
+ pcl::gpu::DeviceArray2D<int> device_volume_;
+
+ Eigen::Vector3f volume_size_;
+
+ float trunc_dist_;
+
+};
+
+
+void
+DeviceVolume::createFromDepth (const pcl::device::PtrStepSz<const unsigned short> &depth, const pcl::device::Intr &intr)
+{
+ using namespace pcl;
+
+ typedef Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Matrix3frm;
+
+ const int rows = 480;
+ const int cols = 640;
+
+ // scale depth values
+ gpu::DeviceArray2D<float> device_depth_scaled;
+ device_depth_scaled.create (rows, cols);
+
+ // upload depth map on GPU
+ pcl::gpu::KinfuTracker::DepthMap device_depth;
+ device_depth.upload (depth.data, depth.step, depth.rows, depth.cols);
+
+ // initial camera rotation and translation
+ Matrix3frm init_Rcam = Eigen::Matrix3f::Identity ();
+ Eigen::Vector3f init_tcam = volume_size_ * 0.5f - Eigen::Vector3f (0, 0, volume_size_(2)/2 * 1.2f);
+
+ Matrix3frm init_Rcam_inv = init_Rcam.inverse ();
+ device::Mat33& device_Rcam_inv = device_cast<device::Mat33> (init_Rcam_inv);
+ float3& device_tcam = device_cast<float3> (init_tcam);
+
+ // integrate depth values into volume
+ float3 device_volume_size = device_cast<float3> (volume_size_);
+ device::integrateTsdfVolume (device_depth, intr, device_volume_size, device_Rcam_inv, device_tcam, trunc_dist_,
+ device_volume_, device_depth_scaled);
+}
+
+
+bool
+DeviceVolume::getVolume (pcl::TSDFVolume<VoxelT, WeightT>::Ptr &volume)
+{
+ int volume_size = device_volume_.rows() * device_volume_.cols();
+
+ if ((size_t)volume_size != volume->size())
+ {
+ pc::print_error ("Device volume size (%d) and tsdf volume size (%d) don't match. ABORTING!\n", volume_size, volume->size());
+ return false;
+ }
+
+ vector<VoxelT>& volume_vec = volume->volumeWriteable();
+ vector<WeightT>& weights_vec = volume->weightsWriteable();
+
+ device_volume_.download (&volume_vec[0], device_volume_.cols() * sizeof(int));
+
+ #pragma omp parallel for
+ for(int i = 0; i < (int) volume->size(); ++i)
+ {
+ short2 *elem = (short2*)&volume_vec[i];
+ volume_vec[i] = (float)(elem->x)/pcl::device::DIVISOR;
+ weights_vec[i] = (short)(elem->y);
+ }
+
+ return true;
+}
+
+
+bool
+DeviceVolume::getCloud (pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud)
+{
+ const int DEFAULT_VOLUME_CLOUD_BUFFER_SIZE = 10 * 1000 * 1000;
+
+ // point buffer on the device
+ pcl::gpu::DeviceArray<pcl::PointXYZ> device_cloud_buffer (DEFAULT_VOLUME_CLOUD_BUFFER_SIZE);
+
+ // do the extraction
+ float3 device_volume_size = device_cast<float3> (volume_size_);
+ /*size_t size =*/ pcl::device::extractCloud (device_volume_, device_volume_size, device_cloud_buffer);
+
+ // write into point cloud structure
+ device_cloud_buffer.download (cloud->points);
+ cloud->width = (int)cloud->points.size ();
+ cloud->height = 1;
+
+ return true;
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+/// HELPER FUNCTIONS
+
+/** \brief Converts depth and RGB sensor readings into a point cloud
+ * param[in] depth depth data from sensor
+ * param[in] rgb24 color data from sensor
+ * param[in] intr camera intrinsics
+ * param[out] cloud the generated point cloud
+ * \note: position in mm is converted to m
+ * \note: RGB reading not working!
+ */
+//TODO implement correct color reading (how does rgb24 look like?)
+bool
+convertDepthRGBToCloud (const pcl::device::PtrStepSz<const unsigned short> &depth, const pcl::device::PtrStepSz<const pcl::gpu::KinfuTracker::PixelRGB> &rgb24, const pcl::device::Intr &intr,
+ pcl::PointCloud<PointT>::Ptr &cloud)
+{
+ // resize point cloud if it doesn't fit
+ if (depth.rows != (int)cloud->height || depth.cols != (int)cloud->width)
+ cloud = pcl::PointCloud<PointT>::Ptr (new pcl::PointCloud<PointT> (depth.cols, depth.rows));
+
+ // std::cout << "step = " << rgb24.step << std::endl;
+ // std::cout << "elem size = " << rgb24.elem_size << std::endl;
+
+ // iterate over all depth and rgb values
+ for (int y = 0; y < depth.rows; ++y)
+ {
+ // get pointers to the values in one row
+ const unsigned short *depth_row_ptr = depth.ptr(y);
+ // const pcl::gpu::KinfuTracker::RGB *rgb24_row_ptr = rgb24.ptr(y);
+ // const char* rgb24_row_ptr = (const char*) rgb24.ptr(y);
+
+ // iterate over row and store values
+ for (int x = 0; x < depth.cols; ++x)
+ {
+ float u = (x - intr.cx) / intr.fx;
+ float v = (y - intr.cy) / intr.fy;
+
+ PointT &point = cloud->at (x, y);
+
+ point.z = depth_row_ptr[x] / 1000.0f;
+ point.x = u * point.z;
+ point.y = v * point.z;
+
+/* uint8_t r = *(rgb24_row_ptr + 0);
+ uint8_t g = *(rgb24_row_ptr + 1);
+ uint8_t b = *(rgb24_row_ptr + 2);
+ uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
+ point.rgb = *reinterpret_cast<float*>(&rgb);
+
+ point.r = *((const char*)rgb24.data + y*rgb24.step + x*rgb24.elem_size);
+ point.g = *((const char*)rgb24.data + y*rgb24.step + x*rgb24.elem_size + 1);
+ point.b = *((const char*)rgb24.data + y*rgb24.step + x*rgb24.elem_size + 2);
+*/
+ }
+ }
+
+ cloud->is_dense = false;
+
+ return true;
+}
+
+/** \brief Captures data from a sensor and generates a point cloud from it
+ * param[in] capture capturing device object
+ * param[out] depth the depth reading
+ * param[out] rgb24 the color reading
+ * param[out] intr camera intrinsics for this reading
+ * param[out] cloud point cloud generated from the readings
+ */
+bool
+captureCloud (pcl::gpu::CaptureOpenNI &capture,
+ pcl::device::PtrStepSz<const unsigned short> &depth, pcl::device::PtrStepSz<const pcl::gpu::KinfuTracker::PixelRGB> &rgb24,
+ pcl::device::Intr &intr, pcl::PointCloud<PointT>::Ptr &cloud)
+{
+ // capture frame
+ if (!capture.grab (depth, rgb24))
+ {
+ pc::print_error ("Can't capture via sensor.\n");
+ return false;
+ }
+
+ // get intrinsics from capture
+ float f = capture.depth_focal_length_VGA;
+ intr = pcl::device::Intr (f, f, depth.cols/2, depth.rows/2);
+
+ // generate point cloud
+ cloud = pcl::PointCloud<PointT>::Ptr (new pcl::PointCloud<PointT> (depth.cols, depth.rows));
+ if (!convertDepthRGBToCloud (depth, rgb24, intr, cloud))
+ {
+ pc::print_error ("Conversion depth --> cloud was not successful!\n");
+ return false;
+ }
+
+ return true;
+}
+
+/** \brief callback function for the PCLvisualizer */
+void
+keyboard_callback (const pcl::visualization::KeyboardEvent &event, void *cookie)
+{
+ if (event.keyDown())
+ {
+ switch (event.getKeyCode())
+ {
+ case 27:
+ case (int)'q': case (int)'Q':
+ case (int)'e': case (int)'E':
+ cout << "Exiting program" << endl;
+ quit = true;
+ break;
+ case (int)'s': case (int)'S':
+ cout << "Saving volume and cloud" << endl;
+ save = true;
+ break;
+ default:
+ break;
+ }
+ }
+}
+
+/** \brief prints usage information for the executable */
+void
+printUsage (char* argv[])
+{
+ pc::print_error ("usage: %s <options>\n\n", argv[0]);
+
+ pc::print_info (" where options are:\n");
+ pc::print_info (" -cf = cloud filename (default: ");
+ pc::print_value ("%s)", cloud_file.c_str()); pc::print_info (")\n");
+ pc::print_info (" -vf = volume filename (default: ");
+ pc::print_value ("%s", volume_file.c_str()); pc::print_info (")\n");
+ pc::print_info (" -ec = extract cloud from generated volume (default: ");
+ pc::print_value ("0 (false)", volume_file.c_str()); pc::print_info (")\n");
+ pc::print_info (" -td = minimal truncation distance (default: ");
+ pc::print_value ("%f", min_trunc_dist); pc::print_info (")\n");
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+/// MAIN
+
+/** \brief main loop for the program */
+int
+main (int argc, char* argv[])
+{
+ pc::print_info ("Records a 2.5 point cloud (organized = depth map) as TSDF volume. For more information, use: %s -h\n", argv[0]);
+
+ /***
+ * PARSE COMMAND LINE
+ */
+
+ // check for help
+ if (pc::find_argument (argc, argv, "-h") > 0)
+ {
+ pc::print_warn ("Showing help: \n\n");
+ printUsage (argv);
+ return (EXIT_SUCCESS);
+ }
+
+ // parse input cloud file
+ pc::parse_argument (argc, argv, "-cf", cloud_file);
+
+ // pase output volume file
+ pc::parse_argument (argc, argv, "-vf", volume_file);
+
+ // parse options to extract and save cloud from volume
+ pc::parse_argument (argc, argv, "-ec", extract_cloud_volume);
+
+ // parse minimual truncation distance
+ pc::parse_argument (argc, argv, "-td", min_trunc_dist);
+
+ /***
+ * SET UP AND VISUALIZATION
+ */
+
+ pc::print_info (" -------------------- START OF ALGORITHM --------------------\n");
+
+ pcl::gpu::setDevice (0);
+ pcl::gpu::printShortCudaDeviceInfo (0);
+
+ pcl::gpu::CaptureOpenNI capture (0); // first OpenNI device;
+ pcl::device::PtrStepSz<const unsigned short> depth;
+ pcl::device::PtrStepSz<const pcl::gpu::KinfuTracker::PixelRGB> rgb24;
+
+ pcl::PointCloud<PointT>::Ptr cloud; // (new pcl::PointCloud<PointT>);
+ pcl::device::Intr intr;
+
+ // capture first frame
+ if (!captureCloud (capture, depth, rgb24, intr, cloud))
+ return EXIT_FAILURE;
+
+ // start visualizer
+ pcl::visualization::PCLVisualizer visualizer;
+ // pcl::visualization::PointCloudColorHandlerRGBField<PointT> color_handler (cloud);
+ // pcl::visualization::PointCloudColorHandlerCustom<PointT> color_handler (cloud, 0.5, 0.5, 0.5);
+ visualizer.addPointCloud<PointT> (cloud); //, color_handler, "cloud");
+ visualizer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1);
+ visualizer.addCoordinateSystem (1);
+ visualizer.initCameraParameters();
+ visualizer.registerKeyboardCallback (keyboard_callback);
+ visualizer.spinOnce();
+
+ /***
+ * CAPTURING DATA AND GENERATING CLOUD
+ */
+
+ pc::print_highlight ("Capturing data ... \n");
+
+ while (!quit && !save)
+ {
+ // capture data and convert to point cloud
+ if (!captureCloud (capture, depth, rgb24, intr, cloud))
+ return EXIT_FAILURE;
+
+ // update visualization
+ visualizer.updatePointCloud<PointT> (cloud); //, color_handler, "cloud");
+ visualizer.spinOnce();
+ }
+
+ if (quit)
+ return (EXIT_SUCCESS);
+
+
+ /***
+ * GENERATE VOLUME
+ */
+
+ // create volume object
+ pcl::TSDFVolume<VoxelT, WeightT>::Ptr volume (new pcl::TSDFVolume<VoxelT, WeightT>);
+ Eigen::Vector3i resolution (pcl::device::VOLUME_X, pcl::device::VOLUME_Y, pcl::device::VOLUME_Z);
+ Eigen::Vector3f volume_size = Eigen::Vector3f::Constant (3000);
+ volume->resize (resolution, volume_size);
+
+ DeviceVolume::Ptr device_volume (new DeviceVolume (volume->volumeSize(), volume->gridResolution()));
+
+
+ // integrate depth in device volume
+ pc::print_highlight ("Converting depth map to volume ... "); cout << flush;
+ device_volume->createFromDepth (depth, intr);
+
+ // get volume from device
+ if (!device_volume->getVolume (volume))
+ {
+ pc::print_error ("Coudln't get volume from device!\n");
+ return (EXIT_FAILURE);
+ }
+ pc::print_info ("done [%d voxels]\n", volume->size());
+
+
+ // generating TSDF cloud
+ pc::print_highlight ("Generating tsdf volume cloud ... "); cout << flush;
+ pcl::PointCloud<pcl::PointXYZI>::Ptr tsdf_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ volume->convertToTsdfCloud (tsdf_cloud);
+ pc::print_info ("done [%d points]\n", tsdf_cloud->size());
+
+
+ // get cloud from volume
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_volume (new pcl::PointCloud<pcl::PointXYZ>);
+ if (extract_cloud_volume)
+ {
+ pc::print_highlight ("Generating cloud from volume ... "); cout << flush;
+ if (!device_volume->getCloud (cloud_volume))
+ {
+ pc::print_error ("Cloudn't get cloud from device volume!\n");
+ return (EXIT_FAILURE);
+ }
+ pc::print_info ("done [%d points]\n", cloud_volume->size());
+ }
+
+
+ /***
+ * STORE RESULTS
+ */
+
+ pc::print_highlight ("Storing results:\n");
+
+ // point cloud
+ pc::print_info ("Saving captured cloud to "); pc::print_value ("%s", cloud_file.c_str()); pc::print_info (" ... ");
+ if (pcl::io::savePCDFile (cloud_file, *cloud, true) < 0)
+ {
+ cout << endl;
+ pc::print_error ("Cloudn't save the point cloud to file %s.\n", cloud_file.c_str());
+ }
+ else
+ pc::print_info ("done [%d points].\n", cloud->size());
+
+ // volume
+ if (!volume->save (volume_file, true))
+ pc::print_error ("Cloudn't save the volume to file %s.\n", volume_file.c_str());
+
+ // TSDF point cloud
+ string tsdf_cloud_file (pcl::getFilenameWithoutExtension(volume_file) + "_cloud.pcd");
+ pc::print_info ("Saving volume cloud to "); pc::print_value ("%s", tsdf_cloud_file.c_str()); pc::print_info (" ... ");
+ if (pcl::io::savePCDFile (tsdf_cloud_file, *tsdf_cloud, true) < 0)
+ {
+ cout << endl;
+ pc::print_error ("Cloudn't save the volume point cloud to file %s.\n", tsdf_cloud_file.c_str());
+ }
+ else
+ pc::print_info ("done [%d points].\n", tsdf_cloud->size());
+
+ // point cloud from volume
+ if (extract_cloud_volume)
+ {
+ string cloud_volume_file (pcl::getFilenameWithoutExtension(cloud_file) + "_from_volume.pcd");
+ pc::print_info ("Saving cloud from volume to "); pc::print_value ("%s", cloud_volume_file.c_str()); pc::print_info (" ... ");
+ if (pcl::io::savePCDFile (cloud_volume_file, *cloud_volume, true) < 0)
+ {
+ cout << endl;
+ pc::print_error ("Cloudn't save the point cloud to file %s.\n", cloud_volume_file.c_str());
+ }
+ else
+ pc::print_info ("done [%d points].\n", cloud_volume->size());
+ }
+
+ pc::print_info (" -------------------- END OF ALGORITHM --------------------\n");
+
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Raphael Favier, Technical University Eindhoven, (r.mysurname <aT> tue.nl)
+ */
+
+
+#include <boost/filesystem.hpp>
+#include <boost/thread/thread.hpp>
+
+#include <fstream>
+#include <iostream>
+#include <sstream>
+
+#include <pcl/common/transforms.h>
+
+#include <pcl/kdtree/kdtree_flann.h>
+
+#include <pcl/features/normal_3d.h>
+
+#include <pcl/visualization/pcl_visualizer.h>
+
+#include <pcl/surface/texture_mapping.h>
+
+#include <pcl/io/vtk_lib_io.h>
+
+using namespace pcl;
+
+/** \brief Save a textureMesh object to obj file */
+int
+saveOBJFile (const std::string &file_name,
+ const pcl::TextureMesh &tex_mesh, unsigned precision)
+{
+ if (tex_mesh.cloud.data.empty ())
+ {
+ PCL_ERROR ("[pcl::io::saveOBJFile] Input point cloud has no data!\n");
+ return (-1);
+ }
+
+ // Open file
+ std::ofstream fs;
+ fs.precision (precision);
+ fs.open (file_name.c_str ());
+
+ // Define material file
+ std::string mtl_file_name = file_name.substr (0, file_name.find_last_of (".")) + ".mtl";
+ // Strip path for "mtllib" command
+ std::string mtl_file_name_nopath = mtl_file_name;
+ mtl_file_name_nopath.erase (0, mtl_file_name.find_last_of ('/') + 1);
+
+ /* Write 3D information */
+ // number of points
+ int nr_points = tex_mesh.cloud.width * tex_mesh.cloud.height;
+ int point_size = tex_mesh.cloud.data.size () / nr_points;
+
+ // mesh size
+ int nr_meshes = tex_mesh.tex_polygons.size ();
+ // number of faces for header
+ int nr_faces = 0;
+ for (int m = 0; m < nr_meshes; ++m)
+ nr_faces += tex_mesh.tex_polygons[m].size ();
+
+ // Write the header information
+ fs << "####" << std::endl;
+ fs << "# OBJ dataFile simple version. File name: " << file_name << std::endl;
+ fs << "# Vertices: " << nr_points << std::endl;
+ fs << "# Faces: " <<nr_faces << std::endl;
+ fs << "# Material information:" << std::endl;
+ fs << "mtllib " << mtl_file_name_nopath << std::endl;
+ fs << "####" << std::endl;
+
+ // Write vertex coordinates
+ fs << "# Vertices" << std::endl;
+ for (int i = 0; i < nr_points; ++i)
+ {
+ int xyz = 0;
+ // "v" just be written one
+ bool v_written = false;
+ for (size_t d = 0; d < tex_mesh.cloud.fields.size (); ++d)
+ {
+ int count = tex_mesh.cloud.fields[d].count;
+ if (count == 0)
+ count = 1; // we simply cannot tolerate 0 counts (coming from older converter code)
+ int c = 0;
+ // adding vertex
+ if ((tex_mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
+ tex_mesh.cloud.fields[d].name == "x" ||
+ tex_mesh.cloud.fields[d].name == "y" ||
+ tex_mesh.cloud.fields[d].name == "z"))
+ {
+ if (!v_written)
+ {
+ // write vertices beginning with v
+ fs << "v ";
+ v_written = true;
+ }
+ float value;
+ memcpy (&value, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[d].offset + c * sizeof (float)], sizeof (float));
+ fs << value;
+ if (++xyz == 3)
+ break;
+ fs << " ";
+ }
+ }
+ if (xyz != 3)
+ {
+ PCL_ERROR ("[pcl::io::saveOBJFile] Input point cloud has no XYZ data!\n");
+ return (-2);
+ }
+ fs << std::endl;
+ }
+ fs << "# "<< nr_points <<" vertices" << std::endl;
+
+ // Write vertex normals
+ for (int i = 0; i < nr_points; ++i)
+ {
+ int xyz = 0;
+ // "vn" just be written one
+ bool v_written = false;
+ for (size_t d = 0; d < tex_mesh.cloud.fields.size (); ++d)
+ {
+ int count = tex_mesh.cloud.fields[d].count;
+ if (count == 0)
+ count = 1; // we simply cannot tolerate 0 counts (coming from older converter code)
+ int c = 0;
+ // adding vertex
+ if ((tex_mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
+ tex_mesh.cloud.fields[d].name == "normal_x" ||
+ tex_mesh.cloud.fields[d].name == "normal_y" ||
+ tex_mesh.cloud.fields[d].name == "normal_z"))
+ {
+ if (!v_written)
+ {
+ // write vertices beginning with vn
+ fs << "vn ";
+ v_written = true;
+ }
+ float value;
+ memcpy (&value, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[d].offset + c * sizeof (float)], sizeof (float));
+ fs << value;
+ if (++xyz == 3)
+ break;
+ fs << " ";
+ }
+ }
+ if (xyz != 3)
+ {
+ PCL_ERROR ("[pcl::io::saveOBJFile] Input point cloud has no normals!\n");
+ return (-2);
+ }
+ fs << std::endl;
+ }
+ // Write vertex texture with "vt" (adding latter)
+
+ for (int m = 0; m < nr_meshes; ++m)
+ {
+ if(tex_mesh.tex_coordinates.size() == 0)
+ continue;
+
+ PCL_INFO ("%d vertex textures in submesh %d\n", tex_mesh.tex_coordinates[m].size (), m);
+ fs << "# " << tex_mesh.tex_coordinates[m].size() << " vertex textures in submesh " << m << std::endl;
+ for (size_t i = 0; i < tex_mesh.tex_coordinates[m].size (); ++i)
+ {
+ fs << "vt ";
+ fs << tex_mesh.tex_coordinates[m][i][0] << " " << tex_mesh.tex_coordinates[m][i][1] << std::endl;
+ }
+ }
+
+ int f_idx = 0;
+
+ // int idx_vt =0;
+ PCL_INFO ("Writting faces...\n");
+ for (int m = 0; m < nr_meshes; ++m)
+ {
+ if (m > 0)
+ f_idx += tex_mesh.tex_polygons[m-1].size ();
+
+ if(tex_mesh.tex_materials.size() !=0)
+ {
+ fs << "# The material will be used for mesh " << m << std::endl;
+ //TODO pbl here with multi texture and unseen faces
+ fs << "usemtl " << tex_mesh.tex_materials[m].tex_name << std::endl;
+ fs << "# Faces" << std::endl;
+ }
+ for (size_t i = 0; i < tex_mesh.tex_polygons[m].size(); ++i)
+ {
+ // Write faces with "f"
+ fs << "f";
+ size_t j = 0;
+ // There's one UV per vertex per face, i.e., the same vertex can have
+ // different UV depending on the face.
+ for (j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j)
+ {
+ unsigned int idx = tex_mesh.tex_polygons[m][i].vertices[j] + 1;
+ fs << " " << idx
+ << "/" << 3*(i+f_idx) +j+1
+ << "/" << idx; // vertex index in obj file format starting with 1
+ }
+ fs << std::endl;
+ }
+ PCL_INFO ("%d faces in mesh %d \n", tex_mesh.tex_polygons[m].size () , m);
+ fs << "# "<< tex_mesh.tex_polygons[m].size() << " faces in mesh " << m << std::endl;
+ }
+ fs << "# End of File";
+
+ // Close obj file
+ PCL_INFO ("Closing obj file\n");
+ fs.close ();
+
+ /* Write material defination for OBJ file*/
+ // Open file
+ PCL_INFO ("Writing material files\n");
+ //dont do it if no material to write
+ if(tex_mesh.tex_materials.size() ==0)
+ return (0);
+
+ std::ofstream m_fs;
+ m_fs.precision (precision);
+ m_fs.open (mtl_file_name.c_str ());
+
+ // default
+ m_fs << "#" << std::endl;
+ m_fs << "# Wavefront material file" << std::endl;
+ m_fs << "#" << std::endl;
+ for(int m = 0; m < nr_meshes; ++m)
+ {
+ m_fs << "newmtl " << tex_mesh.tex_materials[m].tex_name << std::endl;
+ m_fs << "Ka "<< tex_mesh.tex_materials[m].tex_Ka.r << " " << tex_mesh.tex_materials[m].tex_Ka.g << " " << tex_mesh.tex_materials[m].tex_Ka.b << std::endl; // defines the ambient color of the material to be (r,g,b).
+ m_fs << "Kd "<< tex_mesh.tex_materials[m].tex_Kd.r << " " << tex_mesh.tex_materials[m].tex_Kd.g << " " << tex_mesh.tex_materials[m].tex_Kd.b << std::endl; // defines the diffuse color of the material to be (r,g,b).
+ m_fs << "Ks "<< tex_mesh.tex_materials[m].tex_Ks.r << " " << tex_mesh.tex_materials[m].tex_Ks.g << " " << tex_mesh.tex_materials[m].tex_Ks.b << std::endl; // defines the specular color of the material to be (r,g,b). This color shows up in highlights.
+ m_fs << "d " << tex_mesh.tex_materials[m].tex_d << std::endl; // defines the transparency of the material to be alpha.
+ m_fs << "Ns "<< tex_mesh.tex_materials[m].tex_Ns << std::endl; // defines the shininess of the material to be s.
+ m_fs << "illum "<< tex_mesh.tex_materials[m].tex_illum << std::endl; // denotes the illumination model used by the material.
+ // illum = 1 indicates a flat material with no specular highlights, so the value of Ks is not used.
+ // illum = 2 denotes the presence of specular highlights, and so a specification for Ks is required.
+ m_fs << "map_Kd " << tex_mesh.tex_materials[m].tex_file << std::endl;
+ m_fs << "###" << std::endl;
+ }
+ m_fs.close ();
+ return (0);
+}
+
+/** \brief Display a 3D representation showing the a cloud and a list of camera with their 6DOf poses */
+void showCameras (pcl::texture_mapping::CameraVector cams, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud)
+{
+
+ // visualization object
+ pcl::visualization::PCLVisualizer visu ("cameras");
+
+ // add a visual for each camera at the correct pose
+ for(int i = 0 ; i < cams.size () ; ++i)
+ {
+ // read current camera
+ pcl::TextureMapping<pcl::PointXYZ>::Camera cam = cams[i];
+ double focal = cam.focal_length;
+ double height = cam.height;
+ double width = cam.width;
+
+ // create a 5-point visual for each camera
+ pcl::PointXYZ p1, p2, p3, p4, p5;
+ p1.x=0; p1.y=0; p1.z=0;
+ double angleX = RAD2DEG (2.0 * atan (width / (2.0*focal)));
+ double angleY = RAD2DEG (2.0 * atan (height / (2.0*focal)));
+ double dist = 0.75;
+ double minX, minY, maxX, maxY;
+ maxX = dist*tan (atan (width / (2.0*focal)));
+ minX = -maxX;
+ maxY = dist*tan (atan (height / (2.0*focal)));
+ minY = -maxY;
+ p2.x=minX; p2.y=minY; p2.z=dist;
+ p3.x=maxX; p3.y=minY; p3.z=dist;
+ p4.x=maxX; p4.y=maxY; p4.z=dist;
+ p5.x=minX; p5.y=maxY; p5.z=dist;
+ p1=pcl::transformPoint (p1, cam.pose);
+ p2=pcl::transformPoint (p2, cam.pose);
+ p3=pcl::transformPoint (p3, cam.pose);
+ p4=pcl::transformPoint (p4, cam.pose);
+ p5=pcl::transformPoint (p5, cam.pose);
+ std::stringstream ss;
+ ss << "Cam #" << i+1;
+ visu.addText3D(ss.str (), p1, 0.1, 1.0, 1.0, 1.0, ss.str ());
+
+ ss.str ("");
+ ss << "camera_" << i << "line1";
+ visu.addLine (p1, p2,ss.str ());
+ ss.str ("");
+ ss << "camera_" << i << "line2";
+ visu.addLine (p1, p3,ss.str ());
+ ss.str ("");
+ ss << "camera_" << i << "line3";
+ visu.addLine (p1, p4,ss.str ());
+ ss.str ("");
+ ss << "camera_" << i << "line4";
+ visu.addLine (p1, p5,ss.str ());
+ ss.str ("");
+ ss << "camera_" << i << "line5";
+ visu.addLine (p2, p5,ss.str ());
+ ss.str ("");
+ ss << "camera_" << i << "line6";
+ visu.addLine (p5, p4,ss.str ());
+ ss.str ("");
+ ss << "camera_" << i << "line7";
+ visu.addLine (p4, p3,ss.str ());
+ ss.str ("");
+ ss << "camera_" << i << "line8";
+ visu.addLine (p3, p2,ss.str ());
+ }
+
+ // add a coordinate system
+ visu.addCoordinateSystem (1.0, "global");
+
+ // add the mesh's cloud (colored on Z axis)
+ pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler (cloud, "z");
+ visu.addPointCloud (cloud, color_handler, "cloud");
+
+ // reset camera
+ visu.resetCamera ();
+
+ // wait for user input
+ visu.spin ();
+}
+
+/** \brief Helper function that jump to a specific line of a text file */
+std::ifstream& GotoLine(std::ifstream& file, unsigned int num)
+{
+ file.seekg (std::ios::beg);
+ for(int i=0; i < num - 1; ++i)
+ {
+ file.ignore (std::numeric_limits<std::streamsize>::max (),'\n');
+ }
+ return (file);
+}
+
+/** \brief Helper function that reads a camera file outputed by Kinfu */
+bool readCamPoseFile(std::string filename, pcl::TextureMapping<pcl::PointXYZ>::Camera &cam)
+{
+ ifstream myReadFile;
+ myReadFile.open(filename.c_str (), ios::in);
+ if(!myReadFile.is_open ())
+ {
+ PCL_ERROR ("Error opening file %d\n", filename.c_str ());
+ return false;
+ }
+ myReadFile.seekg(ios::beg);
+
+ char current_line[1024];
+ double val;
+
+ // go to line 2 to read translations
+ GotoLine(myReadFile, 2);
+ myReadFile >> val; cam.pose (0,3)=val; //TX
+ myReadFile >> val; cam.pose (1,3)=val; //TY
+ myReadFile >> val; cam.pose (2,3)=val; //TZ
+
+ // go to line 7 to read rotations
+ GotoLine(myReadFile, 7);
+
+ myReadFile >> val; cam.pose (0,0)=val;
+ myReadFile >> val; cam.pose (0,1)=val;
+ myReadFile >> val; cam.pose (0,2)=val;
+
+ myReadFile >> val; cam.pose (1,0)=val;
+ myReadFile >> val; cam.pose (1,1)=val;
+ myReadFile >> val; cam.pose (1,2)=val;
+
+ myReadFile >> val; cam.pose (2,0)=val;
+ myReadFile >> val; cam.pose (2,1)=val;
+ myReadFile >> val; cam.pose (2,2)=val;
+
+ cam.pose (3,0) = 0.0;
+ cam.pose (3,1) = 0.0;
+ cam.pose (3,2) = 0.0;
+ cam.pose (3,3) = 1.0; //Scale
+
+ // go to line 12 to read camera focal length and size
+ GotoLine (myReadFile, 12);
+ myReadFile >> val; cam.focal_length=val;
+ myReadFile >> val; cam.height=val;
+ myReadFile >> val; cam.width=val;
+
+ // close file
+ myReadFile.close ();
+
+ return true;
+
+}
+
+int
+main (int argc, char** argv)
+{
+
+ // read mesh from plyfile
+ PCL_INFO ("\nLoading mesh from file %s...\n", argv[1]);
+ pcl::PolygonMesh triangles;
+ pcl::io::loadPolygonFilePLY(argv[1], triangles);
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::fromPCLPointCloud2(triangles.cloud, *cloud);
+
+ // Create the texturemesh object that will contain our UV-mapped mesh
+ TextureMesh mesh;
+ mesh.cloud = triangles.cloud;
+ std::vector< pcl::Vertices> polygon_1;
+
+ // push faces into the texturemesh object
+ polygon_1.resize (triangles.polygons.size ());
+ for(size_t i =0; i < triangles.polygons.size (); ++i)
+ {
+ polygon_1[i] = triangles.polygons[i];
+ }
+ mesh.tex_polygons.push_back(polygon_1);
+ PCL_INFO ("\tInput mesh contains %d faces and %d vertices\n", mesh.tex_polygons[0].size (), cloud->points.size ());
+ PCL_INFO ("...Done.\n");
+
+ // Load textures and cameras poses and intrinsics
+ PCL_INFO ("\nLoading textures and camera poses...\n");
+ pcl::texture_mapping::CameraVector my_cams;
+
+ const boost::filesystem::path base_dir (".");
+ std::string extension (".txt");
+ int cpt_cam = 0;
+ for (boost::filesystem::directory_iterator it (base_dir); it != boost::filesystem::directory_iterator (); ++it)
+ {
+ if(boost::filesystem::is_regular_file (it->status ()) && boost::filesystem::extension (it->path ()) == extension)
+ {
+ pcl::TextureMapping<pcl::PointXYZ>::Camera cam;
+ readCamPoseFile(it->path ().string (), cam);
+ cam.texture_file = boost::filesystem::basename (it->path ()) + ".png";
+ my_cams.push_back (cam);
+ cpt_cam++ ;
+ }
+ }
+ PCL_INFO ("\tLoaded %d textures.\n", my_cams.size ());
+ PCL_INFO ("...Done.\n");
+
+ // Display cameras to user
+ PCL_INFO ("\nDisplaying cameras. Press \'q\' to continue texture mapping\n");
+ showCameras(my_cams, cloud);
+
+
+ // Create materials for each texture (and one extra for occluded faces)
+ mesh.tex_materials.resize (my_cams.size () + 1);
+ for(int i = 0 ; i <= my_cams.size() ; ++i)
+ {
+ pcl::TexMaterial mesh_material;
+ mesh_material.tex_Ka.r = 0.2f;
+ mesh_material.tex_Ka.g = 0.2f;
+ mesh_material.tex_Ka.b = 0.2f;
+
+ mesh_material.tex_Kd.r = 0.8f;
+ mesh_material.tex_Kd.g = 0.8f;
+ mesh_material.tex_Kd.b = 0.8f;
+
+ mesh_material.tex_Ks.r = 1.0f;
+ mesh_material.tex_Ks.g = 1.0f;
+ mesh_material.tex_Ks.b = 1.0f;
+
+ mesh_material.tex_d = 1.0f;
+ mesh_material.tex_Ns = 75.0f;
+ mesh_material.tex_illum = 2;
+
+ std::stringstream tex_name;
+ tex_name << "material_" << i;
+ tex_name >> mesh_material.tex_name;
+
+ if(i < my_cams.size ())
+ mesh_material.tex_file = my_cams[i].texture_file;
+ else
+ mesh_material.tex_file = "occluded.jpg";
+
+ mesh.tex_materials[i] = mesh_material;
+ }
+
+
+ // Sort faces
+ PCL_INFO ("\nSorting faces by cameras...\n");
+ pcl::TextureMapping<pcl::PointXYZ> tm; // TextureMapping object that will perform the sort
+ tm.textureMeshwithMultipleCameras(mesh, my_cams);
+
+
+ PCL_INFO ("Sorting faces by cameras done.\n");
+ for(int i = 0 ; i <= my_cams.size() ; ++i)
+ {
+ PCL_INFO ("\tSub mesh %d contains %d faces and %d UV coordinates.\n", i, mesh.tex_polygons[i].size (), mesh.tex_coordinates[i].size ());
+ }
+
+
+ // compute normals for the mesh
+ PCL_INFO ("\nEstimating normals...\n");
+ pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
+ pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+ pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
+ tree->setInputCloud (cloud);
+ n.setInputCloud (cloud);
+ n.setSearchMethod (tree);
+ n.setKSearch (20);
+ n.compute (*normals);
+
+ // Concatenate XYZ and normal fields
+ pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
+ pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
+ PCL_INFO ("...Done.\n");
+
+ pcl::toPCLPointCloud2 (*cloud_with_normals, mesh.cloud);
+
+ PCL_INFO ("\nSaving mesh to textured_mesh.obj\n");
+
+ saveOBJFile ("textured_mesh.obj", mesh, 5);
+
+ return (0);
+}
--- /dev/null
+set(SUBSYS_NAME gpu_octree)
+set(SUBSYS_PATH gpu/octree)
+set(SUBSYS_DESC "Octree GPU")
+set(SUBSYS_DEPS common gpu_containers gpu_utils)
+
+set(build TRUE)
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
+mark_as_advanced("BUILD_${SUBSYS_NAME}")
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS})
+PCL_SET_SUBSYS_INCLUDE_DIR("${SUBSYS_NAME}" "${SUBSYS_PATH}")
+
+if(build)
+ FILE(GLOB incs include/pcl/gpu/octree/*.hpp)
+ FILE(GLOB srcs src/*.cpp src/*.hpp src/*.cu)
+ source_group("Source Files" FILES ${srcs})
+
+ FILE(GLOB utils src/utils/*.hpp)
+ source_group("Source Files\\utils" FILES ${utils})
+
+ FILE(GLOB cuda src/cuda/*.hpp src/cuda/*.cu)
+ source_group("Source Files\\cuda" FILES ${cuda})
+
+ LIST(APPEND srcs ${utils} ${cuda})
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include" "${CMAKE_CURRENT_SOURCE_DIR}/src" "${CMAKE_CURRENT_SOURCE_DIR}/src/utils")
+ PCL_CUDA_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs})
+ target_link_libraries("${LIB_NAME}" pcl_gpu_containers)
+
+ set(EXT_DEPS "")
+ #set(EXT_DEPS CUDA)
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "")
+
+ # Install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}" ${incs})
+
+ add_subdirectory(test)
+endif()
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#ifndef _PCL_GPU_OCTREE_DEVICE_FORMAT_HPP_
+#define _PCL_GPU_OCTREE_DEVICE_FORMAT_HPP_
+
+#include <pcl/gpu/containers/device_array.h>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ struct NeighborIndices
+ {
+ DeviceArray<int> data;
+ DeviceArray<int> sizes;
+ int max_elems;
+
+ NeighborIndices() {}
+ NeighborIndices(int query_number, int max_elements) : max_elems(0)
+ {
+ create(query_number, max_elements);
+ }
+
+ void create(int query_number, int max_elements)
+ {
+ max_elems = max_elements;
+ data.create (query_number * max_elems);
+
+ if (max_elems != 1)
+ sizes.create(query_number);
+ }
+
+ void upload(const std::vector<int>& data, const std::vector<int>& sizes, int max_elements)
+ {
+ this->data.upload(data);
+ this->sizes.upload(sizes);
+ max_elems = max_elements;
+ }
+
+ bool validate(size_t cloud_size) const
+ {
+ return (sizes.size() == cloud_size) && (cloud_size * max_elems == data.size());
+ }
+
+ operator PtrStep<int>() const
+ {
+ return PtrStep<int>((int*)data.ptr(), max_elems * sizeof(int));
+ }
+
+ size_t neighboors_size() const { return data.size()/max_elems; }
+ };
+ }
+}
+
+#endif /* _PCL_GPU_OCTREE_DEVICE_FORMAT_HPP_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#ifndef _PCL_GPU_OCTREE_
+#define _PCL_GPU_OCTREE_
+
+#include <vector>
+
+#include <pcl/point_types.h>
+#include <pcl/pcl_macros.h>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/octree/device_format.hpp>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ /**
+ * \brief Octree implementation on GPU. It suppors parallel building and paralel batch search as well .
+ * \author Anaoly Baksheev, Itseez, myname.mysurname@mycompany.com
+ */
+
+ class PCL_EXPORTS Octree
+ {
+ public:
+
+ /** \brief Default constructor.*/
+ Octree();
+
+ /** \brief Denstructor.*/
+ virtual ~Octree();
+
+ /** \brief Types */
+ typedef boost::shared_ptr<Octree> Ptr;
+
+ /** \brief Point typwe supported */
+ typedef pcl::PointXYZ PointType;
+
+ /** \brief Point cloud supported */
+ typedef DeviceArray<PointType> PointCloud;
+
+ /** \brief Point Batch query cloud type */
+ typedef DeviceArray<PointType> Queries;
+
+ /** \brief Point Radiuses for batch query */
+ typedef DeviceArray<float> Radiuses;
+
+ /** \brief Point Indices for batch query */
+ typedef DeviceArray<int> Indices;
+
+ /** \brief Point Sqrt distances array type */
+ typedef DeviceArray<float> ResultSqrDists;
+
+ const PointCloud* cloud_;
+
+ /** \brief Sets cloud for which octree is built */
+ void setCloud(const PointCloud& cloud_arg);
+
+ /** \brief Performs parallel octree building */
+ void build();
+
+ /** \brief Returns true if tree has been built */
+ bool isBuilt();
+
+ /** \brief Downloads Octree from GPU to search using CPU fucntion. It use usefull for signle (not-batch) search */
+ void internalDownload();
+
+ /** \brief Performs search of all points wihtin given radius on CPU. It call \a internalDownload if nessesary
+ * \param[in] center center of sphere
+ * \param[in] radius radious of sphere
+ * \param[out] out indeces of points within give sphere
+ * \param[in] max_nn maximum numver of results returned
+ */
+ void radiusSearchHost(const PointType& center, float radius, std::vector<int>& out, int max_nn = INT_MAX);
+
+ /** \brief Performs approximate neares neighbor search on CPU. It call \a internalDownload if nessesary
+ * \param[in] query 3D point for which neighbour is be fetched
+ * \param[out] out_index neighbour index
+ * \param[out] sqr_dist square distance to the neighbour returned
+ */
+ void approxNearestSearchHost(const PointType& query, int& out_index, float& sqr_dist);
+
+ /** \brief Performs batch radius search on GPU
+ * \param[in] centers array of centers
+ * \param[in] radius radius for all queries
+ * \param[in] max_results max number of returned points for each querey
+ * \param[out] result results packed to signle array
+ */
+ void radiusSearch(const Queries& centers, float radius, int max_results, NeighborIndices& result) const;
+
+ /** \brief Performs batch radius search on GPU
+ * \param[in] centers array of centers
+ * \param[in] radiuses array of radiuses
+ * \param[in] max_results max number of returned points for each querey
+ * \param[out] result results packed to signle array
+ */
+ void radiusSearch(const Queries& centers, const Radiuses& radiuses, int max_results, NeighborIndices& result) const;
+
+ /** \brief Performs batch radius search on GPU
+ * \param[in] centers array of centers
+ * \param[in] indices indices for centers array (only for these points search is performed)
+ * \param[in] radius radius for all queries
+ * \param[in] max_results max number of returned points for each querey
+ * \param[out] result results packed to signle array
+ */
+ void radiusSearch(const Queries& centers, const Indices& indices, float radius, int max_results, NeighborIndices& result) const;
+
+ /** \brief Batch approximate nearest search on GPU
+ * \param[in] queries array of centers
+ * \param[out] result array of results ( one index for each query )
+ */
+ void approxNearestSearch(const Queries& queries, NeighborIndices& result) const;
+
+ /** \brief Batch exact k-nearest search on GPU for k == 1 only!
+ * \param[in] queries array of centers
+ * \param[in] k nubmer of neighbors (only k == 1 is supported)
+ * \param[out] results array of results
+ */
+ void nearestKSearchBatch(const Queries& queries, int k, NeighborIndices& results) const;
+
+ /** \brief Desroys octree and release all resources */
+ void clear();
+ private:
+ void *impl;
+ bool built_;
+ };
+
+ /** \brief Performs brute force radius search on GPU
+ * \param[in] cloud cloud where to search
+ * \param[in] query query point
+ * \param[in] radius radius
+ * \param[out] result indeces of points within give sphere
+ * \param[in] buffer buffer for intermediate results. Keep reference to it between calls to eliminate internal allocations
+ */
+ PCL_EXPORTS void bruteForceRadiusSearchGPU(const Octree::PointCloud& cloud, const Octree::PointType& query, float radius, DeviceArray<int>& result, DeviceArray<int>& buffer);
+ }
+}
+
+#endif /* _PCL_GPU_OCTREE_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include "internal.hpp"
+#include "pcl/gpu/utils/device/limits.hpp"
+#include "pcl/gpu/utils/device/warp.hpp"
+
+#include "utils/copygen.hpp"
+#include "utils/boxutils.hpp"
+#include "utils/scan_block.hpp"
+
+
+namespace pcl { namespace device { namespace appnearest_search
+{
+ typedef OctreeImpl::PointType PointType;
+
+ struct Batch
+ {
+ const PointType* queries;
+
+ const int *indices;
+ const float* points;
+ int points_step; // elem step
+
+ OctreeGlobalWithBox octree;
+
+ int queries_num;
+ mutable int* output;
+ };
+
+ struct KernelPolicy
+ {
+ enum
+ {
+ CTA_SIZE = 512,
+
+ LOG_WARP_SIZE = 5,
+ WARP_SIZE = 1 << LOG_WARP_SIZE,
+ WARPS_COUNT = CTA_SIZE/WARP_SIZE,
+ };
+ };
+
+ struct Warp_appNearestSearch
+ {
+ public:
+ const Batch& batch;
+
+ int query_index;
+ float3 query;
+ int result_idx;
+
+ __device__ __forceinline__ Warp_appNearestSearch(const Batch& batch_arg, int query_index_arg)
+ : batch(batch_arg), query_index(query_index_arg){}
+
+ __device__ __forceinline__ void launch(bool active)
+ {
+ int node_idx = -1;
+ if (active)
+ {
+ PointType q = batch.queries[query_index];
+ query = make_float3(q.x, q.y, q.z);
+
+ node_idx = findNode();
+ }
+
+ processNode(node_idx);
+
+ if (active)
+ batch.output[query_index] = batch.indices[result_idx];
+ }
+
+ private:
+
+ __device__ __forceinline__ int findNode()
+ {
+ float3 minp = batch.octree.minp;
+ float3 maxp = batch.octree.maxp;
+
+ if(query.x < minp.x || query.y < minp.y || query.z < minp.z)
+ return 0;
+
+ if(query.x > maxp.x || query.y > maxp.y || query.z > maxp.z)
+ return 0;
+
+ int node_idx = 0;
+ int code = CalcMorton(minp, maxp)(query);
+ int level = 0;
+
+ for(;;)
+ {
+ int mask_pos = 1 << Morton::extractLevelCode(code, level);
+
+ int node = batch.octree.nodes[node_idx];
+ int mask = node & 0xFF;
+
+ if(__popc(mask) == 0) // leaf
+ return node_idx;
+
+ if ( (mask & mask_pos) == 0) // no child
+ return node_idx;
+
+ node_idx = (node >> 8) + __popc(mask & (mask_pos - 1));
+ ++level;
+ }
+ };
+
+ __device__ __forceinline__ void processNode(int node_idx)
+ {
+ __shared__ volatile int per_warp_buffer[KernelPolicy::WARPS_COUNT];
+
+ int mask = __ballot(node_idx != -1);
+
+ while(mask)
+ {
+ unsigned int laneId = Warp::laneId();
+ unsigned int warpId = threadIdx.x/warpSize;
+
+ int active_lane = __ffs(mask) - 1; //[0..31]
+ mask &= ~(1 << active_lane);
+
+ volatile int* warp_buffer = &per_warp_buffer[warpId];
+
+ //broadcast beg
+ if (active_lane == laneId)
+ *warp_buffer = batch.octree.begs[node_idx];
+ int beg = *warp_buffer;
+
+ //broadcast end
+ if (active_lane == laneId)
+ *warp_buffer = batch.octree.ends[node_idx];
+ int end = *warp_buffer;
+
+ float3 active_query;
+ volatile float* warp_buffer_float = (float*)&per_warp_buffer[warpId];
+
+ //broadcast warp_query
+ if (active_lane == laneId)
+ *warp_buffer_float = query.x;
+ active_query.x = *warp_buffer_float;
+
+ if (active_lane == laneId)
+ *warp_buffer_float = query.y;
+ active_query.y = *warp_buffer_float;
+
+ if (active_lane == laneId)
+ *warp_buffer_float = query.z;
+ active_query.z = *warp_buffer_float;
+
+ int offset = NearestWarpKernel<KernelPolicy::CTA_SIZE>(batch.points + beg, batch.points_step, end - beg, active_query);
+
+ if (active_lane == laneId)
+ result_idx = beg + offset;
+ }
+ }
+
+ template<int CTA_SIZE>
+ __device__ __forceinline__ int NearestWarpKernel(const float* points, int points_step, int length, const float3& active_query)
+ {
+ __shared__ volatile float dist2[CTA_SIZE];
+ __shared__ volatile int index[CTA_SIZE];
+
+ int tid = threadIdx.x;
+ dist2[tid] = pcl::device::numeric_limits<float>::max();
+
+ //serial step
+ for (int idx = Warp::laneId(); idx < length; idx += Warp::STRIDE)
+ {
+ float dx = points[idx ] - active_query.x;
+ float dy = points[idx + points_step ] - active_query.y;
+ float dz = points[idx + points_step * 2] - active_query.z;
+
+ float d2 = dx * dx + dy * dy + dz * dz;
+
+ if (dist2[tid] > d2)
+ {
+ dist2[tid] = d2;
+ index[tid] = idx;
+ }
+ }
+ //parallel step
+ unsigned int lane = Warp::laneId();
+
+ float mind2 = dist2[tid];
+
+ if (lane < 16)
+ {
+ float next = dist2[tid + 16];
+ if (mind2 > next)
+ {
+ dist2[tid] = mind2 = next;
+ index[tid] = index[tid + 16];
+ }
+ }
+
+ if (lane < 8)
+ {
+ float next = dist2[tid + 8];
+ if (mind2 > next)
+ {
+ dist2[tid] = mind2 = next;
+ index[tid] = index[tid + 8];
+ }
+ }
+
+ if (lane < 4)
+ {
+ float next = dist2[tid + 4];
+ if (mind2 > next)
+ {
+ dist2[tid] = mind2 = next;
+ index[tid] = index[tid + 4];
+ }
+ }
+
+ if (lane < 2)
+ {
+ float next = dist2[tid + 2];
+ if (mind2 > next)
+ {
+ dist2[tid] = mind2 = next;
+ index[tid] = index[tid + 2];
+ }
+ }
+
+ if (lane < 1)
+ {
+ float next = dist2[tid + 1];
+ if (mind2 > next)
+ {
+ dist2[tid] = mind2 = next;
+ index[tid] = index[tid + 1];
+ }
+ }
+
+ return index[tid - lane];
+ }
+ };
+
+ __global__ void KernelAN(const Batch batch)
+ {
+ int query_index = blockIdx.x * blockDim.x + threadIdx.x;
+
+ bool active = query_index < batch.queries_num;
+
+ if (__all(active == false))
+ return;
+
+ Warp_appNearestSearch search(batch, query_index);
+ search.launch(active);
+ }
+
+} } }
+
+
+void pcl::device::OctreeImpl::approxNearestSearch(const Queries& queries, NeighborIndices& results) const
+{
+ typedef pcl::device::appnearest_search::Batch BatchType;
+
+ BatchType batch;
+ batch.indices = indices;
+ batch.octree = octreeGlobal;
+
+ batch.queries_num = (int)queries.size();
+ batch.output = results.data;
+
+ batch.points = points_sorted;
+ batch.points_step = (int)points_sorted.elem_step();
+ batch.queries = queries;
+
+ int block = pcl::device::appnearest_search::KernelPolicy::CTA_SIZE;
+ int grid = (batch.queries_num + block - 1) / block;
+
+ cudaSafeCall( cudaFuncSetCacheConfig(pcl::device::appnearest_search::KernelAN, cudaFuncCachePreferL1) );
+
+ pcl::device::appnearest_search::KernelAN<<<grid, block>>>(batch);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+
+#include <thrust/copy.h>
+#include <thrust/device_ptr.h>
+#include <thrust/iterator/counting_iterator.h>
+
+#include "internal.hpp"
+
+#include "cuda.h"
+
+using namespace std;
+using namespace thrust;
+
+namespace pcl
+{
+ namespace device
+ {
+ struct InSphere
+ {
+ float x_, y_, z_, radius2_;
+ InSphere(float x, float y, float z, float radius) : x_(x), y_(y), z_(z), radius2_(radius * radius) {}
+
+ __device__ __host__ __forceinline__ bool operator()(const float3& point) const
+ {
+ float dx = point.x - x_;
+ float dy = point.y - y_;
+ float dz = point.z - z_;
+
+ return (dx * dx + dy * dy + dz * dz) < radius2_;
+ }
+
+ __device__ __host__ __forceinline__ bool operator()(const float4& point) const
+ {
+ return (*this)(make_float3(point.x, point.y, point.z));
+ }
+ };
+ }
+}
+
+#if defined(CUDA_VERSION) && CUDA_VERSION == 4000
+ //workaround of bug in Thrust
+ typedef thrust::counting_iterator<int, thrust::use_default, thrust::use_default, thrust::use_default> It;
+ template<> struct thrust::iterator_difference<It> { typedef int type; };
+#endif
+
+
+void pcl::device::bruteForceRadiusSearch(const OctreeImpl::PointCloud& cloud, const OctreeImpl::PointType& query, float radius, DeviceArray<int>& result, DeviceArray<int>& buffer)
+{
+ typedef OctreeImpl::PointType PointType;
+
+ if (buffer.size() < cloud.size())
+ buffer.create(cloud.size());
+
+ InSphere cond(query.x, query.y, query.z, radius);
+
+ device_ptr<const PointType> cloud_ptr((const PointType*)cloud.ptr());
+ device_ptr<int> res_ptr(buffer.ptr());
+
+ counting_iterator<int> first(0);
+ counting_iterator<int> last = first + cloud.size();
+
+ //main bottle neck is a kernel call overhead/allocs
+ //work time for 871k points ~0.8ms
+ int count = (int)(thrust::copy_if(first, last, cloud_ptr, res_ptr, cond) - res_ptr);
+ result = DeviceArray<int>(buffer.ptr(), count);
+}
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#include "internal.hpp"
+#include "pcl/gpu/utils/device/limits.hpp"
+#include "pcl/gpu/utils/device/warp.hpp"
+
+#include "utils/copygen.hpp"
+#include "utils/boxutils.hpp"
+#include "utils/bitonic_sort.hpp"
+#include "octree_iterator.hpp"
+
+namespace pcl { namespace device { namespace knn_search
+{
+ typedef OctreeImpl::PointType PointType;
+
+ struct Batch
+ {
+ const PointType* queries;
+
+ //int k == 1;
+
+ const int *indices;
+
+ // X1 X2 X3 X4 ..
+ // Y1 Y2 Y3 Y4 ..
+ // Z1 Z2 Z3 Z4 ..
+ const float* points;
+ int points_step; // elem step
+
+ OctreeGlobalWithBox octree;
+
+ int queries_num;
+ mutable int* output;
+ mutable int* sizes;
+ };
+
+ struct KernelPolicy
+ {
+ enum
+ {
+ CTA_SIZE = 512,
+ WARP_SIZE = 32,
+ WARPS_COUNT = CTA_SIZE/WARP_SIZE,
+ };
+ };
+
+ struct Warp_knnSearch
+ {
+ public:
+ typedef OctreeIteratorDeviceNS OctreeIterator;
+
+ const Batch& batch;
+
+ int query_index;
+ float3 query;
+
+ float min_distance;
+ int min_idx;
+
+ OctreeIterator iterator;
+
+ __device__ __forceinline__ Warp_knnSearch(const Batch& batch_arg, int query_index_arg)
+ : batch(batch_arg), query_index(query_index_arg), min_distance(numeric_limits<float>::max()), min_idx(0), iterator(batch.octree) { }
+
+ __device__ __forceinline__ void launch(bool active)
+ {
+ if (active)
+ {
+ PointType q = batch.queries[query_index];
+ query = make_float3(q.x, q.y, q.z);
+ }
+ else
+ query_index = -1;
+
+ while(__any(active))
+ {
+ int leaf = -1;
+
+ if (active)
+ leaf = examineNode(iterator);
+
+ processLeaf(leaf);
+
+ active = active && iterator.level >= 0;
+ }
+ if (query_index != -1)
+ {
+ batch.output[query_index] = batch.indices[min_idx];
+
+ if (batch.sizes)
+ batch.sizes[query_index] = 1;
+ }
+ }
+ private:
+
+ __device__ __forceinline__ int examineNode(OctreeIterator& iterator)
+ {
+ int node_idx = *iterator;
+ int code = batch.octree.codes[node_idx];
+
+ float3 node_minp = batch.octree.minp;
+ float3 node_maxp = batch.octree.maxp;
+ calcBoundingBox(iterator.level, code, node_minp, node_maxp);
+
+ //if true, take nothing, and go to next
+ if (checkIfNodeOutsideSphere(node_minp, node_maxp, query, min_distance))
+ {
+ ++iterator;
+ return -1;
+ }
+
+ //need to go to next level
+ int node = batch.octree.nodes[node_idx];
+ int children_mask = node & 0xFF;
+ bool isLeaf = children_mask == 0;
+
+ if (isLeaf)
+ {
+ ++iterator;
+ return node_idx;
+ }
+
+ //goto next level
+ int first = node >> 8;
+ int len = __popc(children_mask);
+ iterator.gotoNextLevel(first, len);
+ return -1;
+ };
+
+ __device__ __forceinline__ void processLeaf(int node_idx)
+ {
+ int mask = __ballot(node_idx != -1);
+
+ unsigned int laneId = Warp::laneId();
+ unsigned int warpId = Warp::id();
+
+ __shared__ volatile int per_warp_buffer[KernelPolicy::WARPS_COUNT];
+
+ while(mask)
+ {
+ int active_lane = __ffs(mask) - 1; //[0..31]
+ mask &= ~(1 << active_lane);
+
+ volatile int* warp_buffer = &per_warp_buffer[warpId];
+
+ //broadcast beg
+ if (active_lane == laneId)
+ *warp_buffer = batch.octree.begs[node_idx];
+ int beg = *warp_buffer;
+
+ //broadcast end
+ if (active_lane == laneId)
+ *warp_buffer = batch.octree.ends[node_idx];
+ int end = *warp_buffer;
+
+ float3 active_query;
+ volatile float* warp_buffer_float = (float*)&per_warp_buffer[warpId];
+
+ //broadcast warp_query
+ if (active_lane == laneId)
+ *warp_buffer_float = query.x;
+ active_query.x = *warp_buffer_float;
+
+ if (active_lane == laneId)
+ *warp_buffer_float = query.y;
+ active_query.y = *warp_buffer_float;
+
+ if (active_lane == laneId)
+ *warp_buffer_float = query.z;
+ active_query.z = *warp_buffer_float;
+
+ //broadcast query_index
+ if (active_lane == laneId)
+ *warp_buffer = query_index;
+ float active_query_index = *warp_buffer;
+
+ float dist;
+ int offset = NearestWarpKernel<KernelPolicy::CTA_SIZE>(batch.points + beg, batch.points_step, end - beg, active_query, dist);
+
+ if (active_lane == laneId)
+ if (min_distance > dist)
+ {
+ min_distance = dist;
+ min_idx = beg + offset;
+ }
+ }
+ }
+
+ template<int CTA_SIZE>
+ __device__ __forceinline__ int NearestWarpKernel(const float* points, int points_step, int length, const float3& active_query, float& dist)
+ {
+ __shared__ volatile float dist2[CTA_SIZE];
+ __shared__ volatile int index[CTA_SIZE];
+
+ int tid = threadIdx.x;
+ dist2[tid] = pcl::device::numeric_limits<float>::max();
+
+ //serial step
+ for (int idx = Warp::laneId(); idx < length; idx += Warp::STRIDE)
+ {
+ float dx = points[idx ] - active_query.x;
+ float dy = points[idx + points_step ] - active_query.y;
+ float dz = points[idx + points_step * 2] - active_query.z;
+
+ float d2 = dx * dx + dy * dy + dz * dz;
+
+ if (dist2[tid] > d2)
+ {
+ dist2[tid] = d2;
+ index[tid] = idx;
+ }
+ }
+ //parallel step
+ unsigned int lane = Warp::laneId();
+
+ float mind2 = dist2[tid];
+
+ if (lane < 16)
+ {
+ float next = dist2[tid + 16];
+ if (mind2 > next)
+ {
+ dist2[tid] = mind2 = next;
+ index[tid] = index[tid + 16];
+ }
+ }
+
+ if (lane < 8)
+ {
+ float next = dist2[tid + 8];
+ if (mind2 > next)
+ {
+ dist2[tid] = mind2 = next;
+ index[tid] = index[tid + 8];
+ }
+ }
+
+ if (lane < 4)
+ {
+ float next = dist2[tid + 4];
+ if (mind2 > next)
+ {
+ dist2[tid] = mind2 = next;
+ index[tid] = index[tid + 4];
+ }
+ }
+
+ if (lane < 2)
+ {
+ float next = dist2[tid + 2];
+ if (mind2 > next)
+ {
+ dist2[tid] = mind2 = next;
+ index[tid] = index[tid + 2];
+ }
+ }
+
+ if (lane < 1)
+ {
+ float next = dist2[tid + 1];
+ if (mind2 > next)
+ {
+ dist2[tid] = mind2 = next;
+ index[tid] = index[tid + 1];
+ }
+ }
+
+ dist = sqrt(dist2[tid - lane]);
+ return index[tid - lane];
+ }
+ };
+
+ __global__ void KernelKNN(const Batch batch)
+ {
+ int query_index = blockIdx.x * blockDim.x + threadIdx.x;
+
+ bool active = query_index < batch.queries_num;
+
+ if (__all(active == false))
+ return;
+
+ Warp_knnSearch search(batch, query_index);
+ search.launch(active);
+ }
+
+} } }
+
+
+void pcl::device::OctreeImpl::nearestKSearchBatch(const Queries& queries, int /*k*/, NeighborIndices& results) const
+{
+ typedef pcl::device::knn_search::Batch BatchType;
+
+ BatchType batch;
+ batch.octree = octreeGlobal;
+ batch.indices = indices;
+
+ batch.queries_num = (int)queries.size();
+ batch.queries = queries;
+
+ batch.output = results.data;
+ batch.sizes = results.sizes;
+
+ batch.points = points_sorted;
+ batch.points_step = points_sorted.step()/points_sorted.elem_size;
+
+ cudaSafeCall( cudaFuncSetCacheConfig(pcl::device::knn_search::KernelKNN, cudaFuncCachePreferL1) );
+
+ int block = pcl::device::knn_search::KernelPolicy::CTA_SIZE;
+ int grid = (batch.queries_num + block - 1) / block;
+
+ pcl::device::knn_search::KernelKNN<<<grid, block>>>(batch);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include <cfloat>
+#include "internal.hpp"
+
+#include "pcl/gpu/utils/timers_cuda.hpp"
+#include "pcl/gpu/utils/device/funcattrib.hpp"
+#include "pcl/gpu/utils/device/algorithm.hpp"
+#include "pcl/gpu/utils/device/static_check.hpp"
+#include "utils/scan_block.hpp"
+#include "utils/morton.hpp"
+
+#include <thrust/device_ptr.h>
+#include <thrust/sequence.h>
+#include <thrust/sort.h>
+#include <thrust/reduce.h>
+#include <thrust/device_ptr.h>
+
+using namespace pcl::gpu;
+using namespace thrust;
+
+namespace pcl
+{
+ namespace device
+ {
+ template<typename PointType>
+ struct SelectMinPoint
+ {
+ __host__ __device__ __forceinline__ PointType operator()(const PointType& e1, const PointType& e2) const
+ {
+ PointType result;
+ result.x = fmin(e1.x, e2.x);
+ result.y = fmin(e1.y, e2.y);
+ result.z = fmin(e1.z, e2.z);
+ return result;
+ }
+ };
+
+ template<typename PointType>
+ struct SelectMaxPoint
+ {
+ __host__ __device__ __forceinline__ PointType operator()(const PointType& e1, const PointType& e2) const
+ {
+ PointType result;
+ result.x = fmax(e1.x, e2.x);
+ result.y = fmax(e1.y, e2.y);
+ result.z = fmax(e1.z, e2.z);
+ return result;
+ }
+ };
+
+
+ template<typename PointType>
+ struct PointType_to_tuple
+ {
+ __device__ __forceinline__ thrust::tuple<float, float, float> operator()(const PointType& arg) const
+ {
+ thrust::tuple<float, float, float> res;
+ res.get<0>() = arg.x;
+ res.get<1>() = arg.y;
+ res.get<2>() = arg.z;
+ return res;
+ }
+ };
+ }
+}
+namespace pcl
+{
+ namespace device
+ {
+ const static int max_points_per_leaf = 96;
+
+ enum
+ {
+ GRID_SIZE = 1,
+ CTA_SIZE = 1024-32,
+ STRIDE = CTA_SIZE,
+
+ LEVEL_BITS_NUM = 3,
+ ARITY = 1 << LEVEL_BITS_NUM
+ };
+
+ __shared__ int nodes_num;
+ __shared__ int tasks_beg;
+ __shared__ int tasks_end;
+ __shared__ int total_new;
+ __shared__ volatile int offsets[CTA_SIZE];
+
+ struct SingleStepBuild
+ {
+ const int* codes;
+ int points_number;
+ mutable OctreeGlobal octree;
+
+ static __device__ __forceinline__ int divUp(int total, int grain) { return (total + grain - 1) / grain; };
+
+ __device__ __forceinline__ int FindCells(int task, int level, int cell_begs[], char cell_code[]) const
+ {
+ int cell_count = 0;
+
+ int beg = octree.begs[task];
+ int end = octree.ends[task];
+
+ if (end - beg < max_points_per_leaf)
+ {
+ //cell_count == 0;
+ }
+ else
+ {
+ int cur_code = Morton::extractLevelCode(codes[beg], level);
+
+ cell_begs[cell_count] = beg;
+ cell_code[cell_count] = cur_code;
+ ++cell_count;
+
+ int last_code = Morton::extractLevelCode(codes[end - 1], level);
+ if (last_code == cur_code)
+ {
+ cell_begs[cell_count] = end;
+ }
+ else
+ {
+ for(;;)
+ {
+ int search_code = cur_code + 1;
+ if (search_code == 8)
+ {
+ cell_begs[cell_count] = end;
+ break;
+ }
+
+ int morton_code = Morton::shiftLevelCode(search_code, level);
+ int pos = lower_bound(codes + beg, codes + end, morton_code, CompareByLevelCode(level)) - codes;
+
+ if (pos == end)
+ {
+ cell_begs[cell_count] = end;
+ break;
+ }
+ cur_code = Morton::extractLevelCode(codes[pos], level);
+
+ cell_begs[cell_count] = pos;
+ cell_code[cell_count] = cur_code;
+ ++cell_count;
+ beg = pos;
+ }
+ }
+ }
+ return cell_count;
+ }
+
+
+ __device__ __forceinline__ void operator()() const
+ {
+ //32 is a perfomance penalty step for search
+ Static<(max_points_per_leaf % 32) == 0>::check();
+
+ if (threadIdx.x == 0)
+ {
+ //init root
+ octree.codes[0] = 0;
+ octree.nodes[0] = 0;
+ octree. begs[0] = 0;
+ octree. ends[0] = points_number;
+ octree.parent[0] = -1;
+
+ //init shared
+ nodes_num = 1;
+ tasks_beg = 0;
+ tasks_end = 1;
+ total_new = 0;
+ }
+
+ int level = 0;
+
+ int cell_begs[ARITY + 1];
+ char cell_code[ARITY];
+
+ __syncthreads();
+
+ while (tasks_beg < tasks_end)
+ {
+ int task_count = tasks_end - tasks_beg;
+ int iters = divUp(task_count, CTA_SIZE);
+
+ int task = tasks_beg + threadIdx.x;
+
+ //__syncthreads(); // extra??
+
+ for(int it = 0; it < iters; ++it, task += STRIDE)
+ {
+ int cell_count = (task < tasks_end) ? FindCells(task, level, cell_begs, cell_code) : 0;
+
+ offsets[threadIdx.x] = cell_count;
+ __syncthreads();
+
+ scan_block<pcl::device::exclusive>(offsets);
+
+ //__syncthreads(); //because sync is inside the scan above
+
+ if (task < tasks_end)
+ {
+ if (cell_count > 0)
+ {
+ int parent_code_shifted = octree.codes[task] << LEVEL_BITS_NUM;
+ int offset = nodes_num + offsets[threadIdx.x];
+
+ int mask = 0;
+ for(int i = 0; i < cell_count; ++i)
+ {
+ octree.begs [offset + i] = cell_begs[i];
+ octree.ends [offset + i] = cell_begs[i + 1];
+ octree.codes[offset + i] = parent_code_shifted + cell_code[i];
+
+ octree.parent[offset + i] = task;
+ mask |= (1 << cell_code[i]);
+ }
+ octree.nodes[task] = (offset << 8) + mask;
+ }
+ else
+ octree.nodes[task] = 0;
+ }
+
+ __syncthreads();
+ if (threadIdx.x == CTA_SIZE - 1)
+ {
+ total_new += cell_count + offsets[threadIdx.x];
+ nodes_num += cell_count + offsets[threadIdx.x];
+ }
+ __syncthreads();
+
+ } /* for(int it = 0; it < iters; ++it, task += STRIDE) */
+
+ //__syncthreads(); //extra ??
+
+ if (threadIdx.x == CTA_SIZE - 1)
+ {
+ tasks_beg = tasks_end;
+ tasks_end += total_new;
+ total_new = 0;
+ }
+ ++level;
+ __syncthreads();
+ }
+
+ if (threadIdx.x == CTA_SIZE - 1)
+ *octree.nodes_num = nodes_num;
+ }
+ };
+
+ __global__ void __launch_bounds__(CTA_SIZE) singleStepKernel(const SingleStepBuild ssb) { ssb(); }
+ }
+}
+
+void pcl::device::OctreeImpl::build()
+{
+ using namespace pcl::device;
+ host_octree.downloaded = false;
+
+ int points_num = (int)points.size();
+
+ //allocatations
+ {
+ //ScopeTimer timer("new_allocs");
+ //+1 codes * points_num * sizeof(int)
+ //+1 indices * points_num * sizeof(int)
+ //+1 octreeGlobal.nodes * points_num * sizeof(int)
+
+ //+1 octreeGlobal.codes * points_num * sizeof(int)
+ //+1 octreeGlobal.begs * points_num * sizeof(int)
+ //+1 octreeGlobal.ends * points_num * sizeof(int)
+
+ //+1 octreeGlobal.parent * points_num * sizeof(int)
+
+ //+3 points_sorted * points_num * sizeof(float)
+ //==
+ // 10 rows
+
+ //left
+ //octreeGlobal.nodes_num * 1 * sizeof(int)
+ //==
+ // 3 * sizeof(int) => +1 row
+
+ const int transaction_size = 128 / sizeof(int);
+ int cols = max<int>(points_num, transaction_size * 4);
+ int rows = 10 + 1; // = 13
+
+ storage.create(rows, cols);
+
+ codes = DeviceArray<int>(storage.ptr(0), points_num);
+ indices = DeviceArray<int>(storage.ptr(1), points_num);
+
+ octreeGlobal.nodes = storage.ptr(2);
+ octreeGlobal.codes = storage.ptr(3);
+ octreeGlobal.begs = storage.ptr(4);
+ octreeGlobal.ends = storage.ptr(5);
+ octreeGlobal.parent = storage.ptr(6);
+
+ octreeGlobal.nodes_num = storage.ptr(7);
+
+ points_sorted = DeviceArray2D<float>(3, points_num, storage.ptr(8), storage.step());
+ }
+
+ {
+ //ScopeTimer timer("reduce-morton-sort-permutations");
+
+ device_ptr<PointType> beg(points.ptr());
+ device_ptr<PointType> end = beg + points.size();
+
+ {
+ PointType atmax, atmin;
+ atmax.x = atmax.y = atmax.z = FLT_MAX;
+ atmin.x = atmin.y = atmin.z = -FLT_MAX;
+ atmax.w = atmin.w = 0;
+
+ //ScopeTimer timer("reduce");
+ PointType minp = thrust::reduce(beg, end, atmax, SelectMinPoint<PointType>());
+ PointType maxp = thrust::reduce(beg, end, atmin, SelectMaxPoint<PointType>());
+
+ octreeGlobal.minp = make_float3(minp.x, minp.y, minp.z);
+ octreeGlobal.maxp = make_float3(maxp.x, maxp.y, maxp.z);
+ }
+
+ device_ptr<int> codes_beg(codes.ptr());
+ device_ptr<int> codes_end = codes_beg + codes.size();
+ {
+ //ScopeTimer timer("morton");
+ thrust::transform(beg, end, codes_beg, CalcMorton(octreeGlobal.minp, octreeGlobal.maxp));
+ }
+
+ device_ptr<int> indices_beg(indices.ptr());
+ device_ptr<int> indices_end = indices_beg + indices.size();
+ {
+ //ScopeTimer timer("sort");
+ thrust::sequence(indices_beg, indices_end);
+ thrust::sort_by_key(codes_beg, codes_end, indices_beg );
+ }
+ {
+ ////ScopeTimer timer("perm");
+ //thrust::copy(make_permutation_iterator(beg, indices_beg),
+ // make_permutation_iterator(end, indices_end), device_ptr<float3>(points_sorted.ptr()));
+
+
+ }
+
+ {
+ device_ptr<float> xs(points_sorted.ptr(0));
+ device_ptr<float> ys(points_sorted.ptr(1));
+ device_ptr<float> zs(points_sorted.ptr(2));
+ //ScopeTimer timer("perm2");
+ thrust::transform(make_permutation_iterator(beg, indices_beg),
+ make_permutation_iterator(end, indices_end),
+ make_zip_iterator(make_tuple(xs, ys, zs)), PointType_to_tuple<PointType>());
+
+
+ }
+ }
+
+ SingleStepBuild ssb;
+ ssb.octree = octreeGlobal;
+ ssb.codes = codes;
+ ssb.points_number = (int)codes.size();
+ //printFuncAttrib(singleStepKernel);
+
+ cudaSafeCall( cudaFuncSetCacheConfig(singleStepKernel, cudaFuncCachePreferL1) );
+
+ singleStepKernel<<<GRID_SIZE, CTA_SIZE>>>(ssb);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#include "pcl/gpu/utils/timers_cuda.hpp"
+#include "pcl/gpu/utils/safe_call.hpp"
+
+#include "internal.hpp"
+#include "utils/boxutils.hpp"
+
+#include<algorithm>
+#include<limits>
+
+using namespace pcl::gpu;
+using namespace pcl::device;
+using namespace std;
+
+namespace pcl
+{
+ namespace device
+ {
+ __global__ void get_cc_kernel(int *data)
+ {
+ data[threadIdx.x + blockDim.x * blockIdx.x] = threadIdx.x;
+ }
+ }
+}
+
+void pcl::device::OctreeImpl::get_gpu_arch_compiled_for(int& bin, int& ptx)
+{
+ cudaFuncAttributes attrs;
+ cudaSafeCall( cudaFuncGetAttributes(&attrs, get_cc_kernel) );
+ bin = attrs.binaryVersion;
+ ptx = attrs.ptxVersion;
+}
+
+void pcl::device::OctreeImpl::setCloud(const PointCloud& input_points)
+{
+ points = input_points;
+}
+
+void pcl::device::OctreeImpl::internalDownload()
+{
+ int number;
+ DeviceArray<int>(octreeGlobal.nodes_num, 1).download(&number);
+
+ DeviceArray<int>(octreeGlobal.begs, number).download(host_octree.begs);
+ DeviceArray<int>(octreeGlobal.ends, number).download(host_octree.ends);
+ DeviceArray<int>(octreeGlobal.nodes, number).download(host_octree.nodes);
+ DeviceArray<int>(octreeGlobal.codes, number).download(host_octree.codes);
+
+ points_sorted.download(host_octree.points_sorted, host_octree.points_sorted_step);
+ indices.download(host_octree.indices);
+
+ host_octree.downloaded = true;
+}
+
+namespace
+{
+ int getBitsNum(int interger)
+ {
+ int count = 0;
+ while(interger > 0)
+ {
+ if (interger & 1)
+ ++count;
+ interger>>=1;
+ }
+ return count;
+ }
+
+ struct OctreeIteratorHost
+ {
+ const static int MAX_LEVELS_PLUS_ROOT = 11;
+ int paths[MAX_LEVELS_PLUS_ROOT];
+ int level;
+
+ OctreeIteratorHost()
+ {
+ level = 0; // root level
+ paths[level] = (0 << 8) + 1;
+ }
+
+ void gotoNextLevel(int first, int len)
+ {
+ ++level;
+ paths[level] = (first << 8) + len;
+ }
+
+ int operator*() const
+ {
+ return paths[level] >> 8;
+ }
+
+ void operator++()
+ {
+ while(level >= 0)
+ {
+ int data = paths[level];
+
+ if ((data & 0xFF) > 1) // there are another siblings, can goto there
+ {
+ data += (1 << 8) - 1; // +1 to first and -1 from len
+ paths[level] = data;
+ break;
+ }
+ else
+ --level; //goto parent;
+ }
+ }
+ };
+
+}
+
+void pcl::device::OctreeImpl::radiusSearchHost(const PointType& query, float radius, vector<int>& out, int max_nn) const
+{
+ out.clear();
+
+ float3 center = make_float3(query.x, query.y, query.z);
+
+ OctreeIteratorHost iterator;
+
+ while(iterator.level >= 0)
+ {
+ int node_idx = *iterator;
+ int code = host_octree.codes[node_idx];
+
+ float3 node_minp = octreeGlobal.minp;
+ float3 node_maxp = octreeGlobal.maxp;
+ calcBoundingBox(iterator.level, code, node_minp, node_maxp);
+
+ //if true, take nothing, and go to next
+ if (checkIfNodeOutsideSphere(node_minp, node_maxp, center, radius))
+ {
+ ++iterator;
+ continue;
+ }
+
+ //if true, take all, and go to next
+ if (checkIfNodeInsideSphere(node_minp, node_maxp, center, radius))
+ {
+ int beg = host_octree.begs[node_idx];
+ int end = host_octree.ends[node_idx];
+
+ end = beg + min<int>((int)out.size() + end - beg, max_nn) - (int)out.size();
+
+ out.insert(out.end(), host_octree.indices.begin() + beg, host_octree.indices.begin() + end);
+ if (out.size() == (size_t)max_nn)
+ return;
+
+ ++iterator;
+ continue;
+ }
+
+ // test children
+ int children_mask = host_octree.nodes[node_idx] & 0xFF;
+
+ bool isLeaf = children_mask == 0;
+
+ if (isLeaf)
+ {
+ const int beg = host_octree.begs[node_idx];
+ const int end = host_octree.ends[node_idx];
+
+ for(int j = beg; j < end; ++j)
+ {
+ int index = host_octree.indices[j];
+ float point_x = host_octree.points_sorted[j ];
+ float point_y = host_octree.points_sorted[j + host_octree.points_sorted_step ];
+ float point_z = host_octree.points_sorted[j + host_octree.points_sorted_step * 2];
+
+ float dx = (point_x - center.x);
+ float dy = (point_y - center.y);
+ float dz = (point_z - center.z);
+
+ float dist2 = dx * dx + dy * dy + dz * dz;
+
+ if (dist2 < radius * radius)
+ out.push_back(index);
+
+ if (out.size() == (size_t)max_nn)
+ return;
+ }
+ ++iterator;
+ continue;
+ }
+
+ int first = host_octree.nodes[node_idx] >> 8;
+ iterator.gotoNextLevel(first, getBitsNum(children_mask));
+ }
+}
+
+void pcl::device::OctreeImpl::approxNearestSearchHost(const PointType& query, int& out_index, float& sqr_dist) const
+{
+ float3 minp = octreeGlobal.minp;
+ float3 maxp = octreeGlobal.maxp;
+
+ int node_idx = 0;
+
+ bool out_of_root = query.x < minp.x || query.y < minp.y || query.z < minp.z || query.x > maxp.x || query.y > maxp.y || query.z > maxp.z;
+
+ if(!out_of_root)
+ {
+ int code = CalcMorton(minp, maxp)(query);
+ int level = 0;
+
+ for(;;)
+ {
+ int mask_pos = 1 << Morton::extractLevelCode(code, level);
+
+ int node = host_octree.nodes[node_idx];
+ int mask = node & 0xFF;
+
+ if(getBitsNum(mask) == 0) // leaf
+ break;
+
+ if ( (mask & mask_pos) == 0) // no child
+ break;
+
+ node_idx = (node >> 8) + getBitsNum(mask & (mask_pos - 1));
+ ++level;
+ }
+ }
+
+ int beg = host_octree.begs[node_idx];
+ int end = host_octree.ends[node_idx];
+
+ sqr_dist = std::numeric_limits<float>::max();
+
+ for(int i = beg; i < end; ++i)
+ {
+ float point_x = host_octree.points_sorted[i ];
+ float point_y = host_octree.points_sorted[i + host_octree.points_sorted_step ];
+ float point_z = host_octree.points_sorted[i + host_octree.points_sorted_step * 2];
+
+ float dx = (point_x - query.x);
+ float dy = (point_y - query.y);
+ float dz = (point_z - query.z);
+
+ float d2 = dx * dx + dy * dy + dz * dz;
+
+ if (sqr_dist > d2)
+ {
+ sqr_dist = d2;
+ out_index = i;
+ }
+ }
+
+ out_index = host_octree.indices[out_index];
+}
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_GPU_OCTREE_ITERATOR
+#define PCL_GPU_OCTREE_ITERATOR
+
+namespace pcl
+{
+ namespace device
+ {
+
+ template<int CTA_SIZE, int STACK_DEPTH>
+ struct OctreeIteratorDevice
+ {
+ typedef int Storage[STACK_DEPTH][CTA_SIZE];
+
+ int level;
+ Storage& storage;
+
+ __device__ __forceinline__ OctreeIteratorDevice(Storage& storage_arg) : storage(storage_arg)
+ {
+ level = 0; // root level
+ storage[level][threadIdx.x] = (0 << 8) + 1;
+ }
+
+ __device__ __forceinline__ void gotoNextLevel(int first, int len)
+ {
+ ++level;
+ storage[level][threadIdx.x] = (first << 8) + len;
+ }
+
+ __device__ __forceinline__ int operator*() const
+ {
+ return storage[level][threadIdx.x] >> 8;
+ }
+
+ __device__ __forceinline__ void operator++()
+ {
+ while(level >= 0)
+ {
+ int data = storage[level][threadIdx.x];
+
+ if ((data & 0xFF) > 1) // there are another siblings, can goto there
+ {
+ data += (1 << 8) - 1; // +1 to first and -1 from len
+ storage[level][threadIdx.x] = data;
+ break;
+ }
+ else
+ --level; //goto parent;
+ }
+ }
+ };
+
+ struct OctreeIteratorDeviceNS
+ {
+ int level;
+ int node_idx;
+ int lenght;
+ const OctreeGlobalWithBox& octree;
+
+ __device__ __forceinline__ OctreeIteratorDeviceNS(const OctreeGlobalWithBox& octree_arg) : octree(octree_arg)
+ {
+ node_idx = 0;
+ level = 0;
+ lenght = 1;
+ }
+
+ __device__ __forceinline__ void gotoNextLevel(int first, int len)
+ {
+ node_idx = first;
+ lenght = len;
+ ++level;
+ }
+
+ __device__ __forceinline__ int operator*() const
+ {
+ return node_idx;
+ }
+
+ __device__ __forceinline__ void operator++()
+ {
+#if 1
+ while(level >= 0)
+ {
+ if (lenght > 1)
+ {
+ lenght--;
+ node_idx++;
+ break;
+ }
+
+ if (node_idx == 0)
+ {
+ level = -1;
+ return;
+ }
+
+ node_idx = octree.parent[node_idx];
+ --level;
+ if (node_idx == 0)
+ {
+ level = -1;
+ return;
+ }
+
+ int parent = octree.nodes[octree.parent[node_idx]];
+ int parent_first = parent >> 8;
+ int parent_len = __popc(parent & 0xFF);
+
+ int pos = node_idx - parent_first;
+
+ lenght = parent_len - pos;
+ }
+#endif
+ }
+
+ };
+ }
+}
+
+#endif /* PCL_GPU_OCTREE_ITERATOR */
\ No newline at end of file
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#include "internal.hpp"
+
+#include "pcl/gpu/utils/device/warp.hpp"
+#include "utils/copygen.hpp"
+#include "utils/boxutils.hpp"
+#include "utils/scan_block.hpp"
+
+#include "octree_iterator.hpp"
+
+namespace pcl
+{
+ namespace device
+ {
+ typedef OctreeImpl::PointType PointType;
+
+ template<typename RadiusStrategy, typename FetchStrategy>
+ struct Batch : public RadiusStrategy, public FetchStrategy
+ {
+ const int *indices;
+ PtrStep<float> points;
+ OctreeGlobalWithBox octree;
+
+ int max_results;
+ mutable int* output;
+ mutable int* output_sizes;
+ };
+
+ struct DirectQuery
+ {
+ PtrSz<PointType> queries;
+ __device__ __forceinline__ float3 fetch(int query_index) const
+ {
+ PointType q = queries.data[query_index];
+ return make_float3(q.x, q.y, q.z);
+ }
+ };
+
+
+ struct IndicesQuery : public DirectQuery
+ {
+ const int* queries_indices;
+ __device__ __forceinline__ float3 fetch(int query_index) const
+ {
+ PointType q = queries[queries_indices[query_index]];
+ return make_float3(q.x, q.y, q.z);
+ }
+ };
+
+ struct SharedRadius
+ {
+ float radius;
+ __device__ __forceinline__ float getRadius(int /*index*/) const { return radius; }
+ __device__ __forceinline__ float bradcastRadius2(float* /*ptr*/, bool /*active*/, float& /*radius_reg*/) const
+ {
+ return radius * radius;
+ }
+ };
+
+ struct IndividualRadius
+ {
+ const float* radiuses;
+ __device__ __forceinline__ float getRadius(int index) const { return radiuses[index]; }
+ __device__ __forceinline__ float bradcastRadius2(float* ptr, bool active, float& radius_reg) const
+ {
+ if (active)
+ *ptr = radius_reg * radius_reg;
+ return *ptr;
+ }
+ };
+
+ struct KernelPolicy
+ {
+ enum
+ {
+ CTA_SIZE = 512,
+
+ WARP_SIZE = 32,
+ WARPS_COUNT = CTA_SIZE/WARP_SIZE,
+
+ MAX_LEVELS_PLUS_ROOT = 11,
+
+ CHECK_FLAG = 1 << 31
+ };
+
+ struct SmemStorage
+ {
+ volatile int per_warp_buffer[WARPS_COUNT];
+ volatile int cta_buffer[CTA_SIZE];
+ };
+ };
+
+ __shared__ KernelPolicy::SmemStorage storage;
+
+
+ template<typename BatchType>
+ struct Warp_radiusSearch
+ {
+ public:
+ typedef OctreeIteratorDeviceNS OctreeIterator;
+
+ const BatchType& batch;
+ OctreeIterator iterator;
+
+ int found_count;
+ int query_index;
+ float3 query;
+ float radius;
+
+ __device__ __forceinline__ Warp_radiusSearch(const BatchType& batch_arg, int query_index_arg)
+ : batch(batch_arg), iterator(/**/batch.octree/*storage.paths*/), found_count(0), query_index(query_index_arg){}
+
+ __device__ __forceinline__ void launch(bool active)
+ {
+ if (active)
+ {
+ query = batch.fetch(query_index);
+ radius = batch.getRadius(query_index);
+ }
+ else
+ query_index = -1;
+
+ while(__any(active))
+ {
+ int leaf = -1;
+
+ if (active)
+ leaf = examineNode(iterator);
+
+ processLeaf(leaf);
+
+ active = active && iterator.level >= 0 && found_count < batch.max_results;
+ }
+
+ if (query_index != -1)
+ batch.output_sizes[query_index] = found_count;
+ }
+
+ private:
+
+ __device__ __forceinline__ int examineNode(OctreeIterator& iterator)
+ {
+ using namespace pcl::gpu;
+
+ int node_idx = *iterator;
+ int code = batch.octree.codes[node_idx];
+
+ float3 node_minp = batch.octree.minp;
+ float3 node_maxp = batch.octree.maxp;
+ calcBoundingBox(iterator.level, code, node_minp, node_maxp);
+
+ //if true, take nothing, and go to next
+ if (checkIfNodeOutsideSphere(node_minp, node_maxp, query, radius))
+ {
+ ++iterator;
+ return -1;
+ }
+
+ if (checkIfNodeInsideSphere(node_minp, node_maxp, query, radius))
+ {
+ ++iterator;
+ return node_idx; //return node to copy
+ }
+
+ //need to go to next level
+ int node = batch.octree.nodes[node_idx];
+ int children_mask = node & 0xFF;
+ bool isLeaf = children_mask == 0;
+
+ if (isLeaf)
+ {
+ ++iterator;
+ return (node_idx | KernelPolicy::CHECK_FLAG); // return node to check
+ }
+
+ //goto next level
+ int first = node >> 8;
+ int len = __popc(children_mask);
+ iterator.gotoNextLevel(first, len);
+ return -1;
+ };
+
+ __device__ __forceinline__ void processLeaf(int leaf)
+ {
+ int mask = __ballot(leaf != -1);
+
+ while(mask)
+ {
+ unsigned int laneId = Warp::laneId();
+ unsigned int warpId = Warp::id();
+
+ int active_lane = __ffs(mask) - 1; //[0..31]
+
+ mask &= ~(1 << active_lane);
+
+ //broadcast active_found_count
+ if (active_lane == laneId)
+ storage.per_warp_buffer[warpId] = found_count;
+ int active_found_count = storage.per_warp_buffer[warpId];
+
+ int node_idx = leaf & ~KernelPolicy::CHECK_FLAG;
+
+ //broadcast beg
+ if (active_lane == laneId)
+ storage.per_warp_buffer[warpId] = batch.octree.begs[node_idx];
+ int beg = storage.per_warp_buffer[warpId];
+
+ //broadcast end
+ if (active_lane == laneId)
+ storage.per_warp_buffer[warpId] = batch.octree.ends[node_idx];
+ int end = storage.per_warp_buffer[warpId];
+
+ //broadcast active_query_index
+ if (active_lane == laneId)
+ storage.per_warp_buffer[warpId] = query_index;
+ int active_query_index = storage.per_warp_buffer[warpId];
+
+ int length = end - beg;
+
+ int *out = batch.output + active_query_index * batch.max_results + active_found_count;
+ int length_left = batch.max_results - active_found_count;
+
+ int test = __any(active_lane == laneId && (leaf & KernelPolicy::CHECK_FLAG));
+
+ if (test)
+ {
+ float3 active_query;
+
+ //broadcast warp_query
+ if (active_lane == laneId)
+ storage.per_warp_buffer[warpId] = __float_as_int(query.x);
+ active_query.x = __int_as_float(storage.per_warp_buffer[warpId]);
+
+ if (active_lane == laneId)
+ storage.per_warp_buffer[warpId] = __float_as_int(query.y);
+ active_query.y = __int_as_float(storage.per_warp_buffer[warpId]);
+
+ if (active_lane == laneId)
+ storage.per_warp_buffer[warpId] = __float_as_int(query.z);
+ active_query.z = __int_as_float(storage.per_warp_buffer[warpId]);
+
+ float radius2 = batch.bradcastRadius2((float*)&storage.per_warp_buffer[warpId], (active_lane == laneId), radius);
+
+ length = TestWarpKernel(beg, active_query, radius2, length, out, length_left);
+ }
+ else
+ {
+ length = min(length, length_left);
+ Warp::copy(batch.indices + beg, batch.indices + beg + length, out);
+ }
+
+ if (active_lane == laneId)
+ found_count += length;
+ }
+ }
+
+ __device__ __forceinline__ int TestWarpKernel(int beg, const float3& active_query, float radius2, int length, int* out, int length_left)
+ {
+ unsigned int idx = Warp::laneId();
+ int last_threadIdx = threadIdx.x - idx + 31;
+
+ int total_new = 0;
+
+ for(;;)
+ {
+ int take = 0;
+
+ if (idx < length)
+ {
+ float dx = batch.points.ptr(0)[beg + idx] - active_query.x;
+ float dy = batch.points.ptr(1)[beg + idx] - active_query.y;
+ float dz = batch.points.ptr(2)[beg + idx] - active_query.z;
+
+ float d2 = dx * dx + dy * dy + dz * dz;
+
+ if (d2 < radius2)
+ take = 1;
+ }
+
+ storage.cta_buffer[threadIdx.x] = take;
+
+ int offset = scan_warp<exclusive>(storage.cta_buffer);
+
+ //ensure that we copy
+ bool out_of_bounds = (offset + total_new) >= length_left;
+
+ if (take && !out_of_bounds)
+ out[offset] = batch.indices[beg + idx];
+
+ int new_nodes = storage.cta_buffer[last_threadIdx];
+
+ idx += Warp::STRIDE;
+
+ total_new += new_nodes;
+ out += new_nodes;
+
+ if (__all(idx >= length) || __any(out_of_bounds) || total_new == length_left)
+ break;
+ }
+ return min(total_new, length_left);
+ }
+ };
+
+ template<typename BatchType>
+ __global__ void KernelRS(const BatchType batch)
+ {
+ int query_index = blockIdx.x * blockDim.x + threadIdx.x;
+
+ bool active = query_index < batch.queries.size;
+
+ if (__all(active == false))
+ return;
+
+ Warp_radiusSearch<BatchType> search(batch, query_index);
+ search.launch(active);
+ }
+ }
+}
+
+template<typename BatchType>
+void pcl::device::OctreeImpl::radiusSearchEx(BatchType& batch, const Queries& queries, NeighborIndices& results)
+{
+ batch.indices = indices;
+ batch.octree = octreeGlobal;
+
+ batch.max_results = results.max_elems;
+ batch.output = results.data;
+ batch.output_sizes = results.sizes;
+
+ batch.points = points_sorted;
+
+
+ cudaSafeCall( cudaFuncSetCacheConfig(KernelRS<BatchType>, cudaFuncCachePreferL1) );
+
+ int block = KernelPolicy::CTA_SIZE;
+ int grid = divUp((int)batch.queries.size, block);
+
+ KernelRS<<<grid, block>>>(batch);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
+
+
+void pcl::device::OctreeImpl::radiusSearch(const Queries& queries, float radius, NeighborIndices& results)
+{
+ typedef Batch<SharedRadius, DirectQuery> BatchType;
+
+ BatchType batch;
+ batch.radius = radius;
+ batch.queries = queries;
+ radiusSearchEx(batch, queries, results);
+}
+
+void pcl::device::OctreeImpl::radiusSearch(const Queries& queries, const Radiuses& radiuses, NeighborIndices& results)
+{
+ typedef Batch<IndividualRadius, DirectQuery> BatchType;
+
+ BatchType batch;
+ batch.radiuses = radiuses;
+ batch.queries = queries;
+ radiusSearchEx(batch, queries, results);
+}
+
+void pcl::device::OctreeImpl::radiusSearch(const Queries& queries, const Indices& indices, float radius, NeighborIndices& results)
+{
+ typedef Batch<SharedRadius, IndicesQuery> BatchType;
+
+ BatchType batch;
+ batch.radius = radius;
+
+ batch.queries = queries;
+ batch.queries_indices = indices;
+ batch.queries.size = indices.size();
+
+ radiusSearchEx(batch, queries, results);
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#ifndef PCL_GPU_OCTREE_INTERNAL_HPP_
+#define PCL_GPU_OCTREE_INTERNAL_HPP_
+
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/octree/device_format.hpp>
+#include <pcl/gpu/utils/safe_call.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+ struct OctreeGlobal
+ {
+ int *nodes;
+ int *codes;
+ int *begs;
+ int *ends;
+
+ int *nodes_num;
+
+ int *parent;
+
+ OctreeGlobal() : nodes(0), codes(0), begs(0), ends(0), nodes_num(0), parent(0) {}
+ };
+
+ struct OctreeGlobalWithBox : public OctreeGlobal
+ {
+ float3 minp, maxp;
+ };
+
+
+ class OctreeImpl
+ {
+ public:
+ typedef float4 PointType;
+ typedef DeviceArray<PointType> PointArray;
+
+ typedef PointArray PointCloud;
+ typedef PointArray Queries;
+
+ typedef DeviceArray<float> Radiuses;
+ typedef DeviceArray<int> BatchResult;
+ typedef DeviceArray<int> BatchResultSizes;
+ typedef DeviceArray<float> BatchResultSqrDists;
+ typedef DeviceArray<int> Indices;
+
+ typedef pcl::gpu::NeighborIndices NeighborIndices;
+
+ static void get_gpu_arch_compiled_for(int& bin, int& ptr);
+
+ OctreeImpl() {};
+ ~OctreeImpl() {};
+
+ void setCloud(const PointCloud& input_points);
+ void build();
+ void radiusSearchHost(const PointType& center, float radius, std::vector<int>& out, int max_nn) const;
+ void approxNearestSearchHost(const PointType& query, int& out_index, float& sqr_dist) const;
+
+ void radiusSearch(const Queries& queries, float radius, NeighborIndices& results);
+ void radiusSearch(const Queries& queries, const Radiuses& radiuses, NeighborIndices& results);
+
+ void radiusSearch(const Queries& queries, const Indices& indices, float radius, NeighborIndices& results);
+
+ void approxNearestSearch(const Queries& queries, NeighborIndices& results) const;
+
+ void nearestKSearchBatch(const Queries& queries, int k, NeighborIndices& results) const;
+
+ //just reference
+ PointCloud points;
+
+ // data
+ DeviceArray2D<float> points_sorted;
+ DeviceArray<int> codes;
+ DeviceArray<int> indices;
+
+ OctreeGlobalWithBox octreeGlobal;
+
+ //storage
+ DeviceArray2D<int> storage;
+
+ struct OctreeDataHost
+ {
+ std::vector<int> nodes;
+ std::vector<int> codes;
+
+ std::vector<int> begs;
+ std::vector<int> ends;
+
+
+ std::vector<int> indices;
+
+ std::vector<float> points_sorted;
+ int points_sorted_step;
+
+ int downloaded;
+
+ } host_octree;
+
+
+ void internalDownload();
+ private:
+ template<typename BatchType>
+ void radiusSearchEx(BatchType& batch, const Queries& queries, NeighborIndices& results);
+ };
+
+ void bruteForceRadiusSearch(const OctreeImpl::PointCloud& cloud, const OctreeImpl::PointType& query, float radius, DeviceArray<int>& result, DeviceArray<int>& buffer);
+
+ }
+}
+
+#endif /* PCL_GPU_OCTREE_INTERNAL_HPP_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include <pcl/gpu/octree/octree.hpp>
+#include <pcl/gpu/utils/timers_cuda.hpp>
+#include <pcl/gpu/utils/safe_call.hpp>
+
+#include "internal.hpp"
+#include "cuda_runtime.h"
+#include <pcl/gpu/utils/device/static_check.hpp>
+#include <pcl/exceptions.h>
+
+#include<assert.h>
+
+using namespace pcl::device;
+using namespace std;
+
+//////////////////////////////////////////////////////////////////////////////////////
+//////////////// Octree Host Interface implementation ////////////////////////////////
+
+pcl::gpu::Octree::Octree() : cloud_(0), impl(0)
+{
+ Static<sizeof(PointType) == sizeof(OctreeImpl::PointType)>::check();
+
+ int device;
+ cudaSafeCall( cudaGetDevice( &device ) );
+
+ cudaDeviceProp prop;
+ cudaSafeCall( cudaGetDeviceProperties( &prop, device) );
+
+ if (prop.major < 2)
+ pcl::gpu::error("This code requires devices with compute capability >= 2.0", __FILE__, __LINE__);
+
+ int bin, ptx;
+ OctreeImpl::get_gpu_arch_compiled_for(bin, ptx);
+
+ if (bin < 20 && ptx < 20)
+ pcl::gpu::error("This must be compiled for compute capability >= 2.0", __FILE__, __LINE__);
+
+ impl = new OctreeImpl();
+ built_ = false;
+}
+
+pcl::gpu::Octree::~Octree() { clear(); }
+
+void pcl::gpu::Octree::clear()
+{
+ if (impl)
+ delete static_cast<OctreeImpl*>(impl);
+}
+
+void pcl::gpu::Octree::setCloud(const PointCloud& cloud_arg)
+{
+ const OctreeImpl::PointCloud& cloud = (const OctreeImpl::PointCloud&)cloud_arg;
+ cloud_ = &cloud_arg;
+ static_cast<OctreeImpl*>(impl)->setCloud(cloud);
+}
+
+void pcl::gpu::Octree::build()
+{
+ static_cast<OctreeImpl*>(impl)->build();
+ built_ = true;
+}
+
+bool pcl::gpu::Octree::isBuilt()
+{
+ return built_;
+}
+
+void pcl::gpu::Octree::internalDownload()
+{
+ static_cast<OctreeImpl*>(impl)->internalDownload();
+}
+
+void pcl::gpu::Octree::radiusSearchHost(const PointType& center, float radius, std::vector<int>& out, int max_nn)
+{
+ if (!static_cast<OctreeImpl*>(impl)->host_octree.downloaded)
+ internalDownload();
+
+ OctreeImpl::PointType query;
+ query.x = center.x;
+ query.y = center.y;
+ query.z = center.z;
+
+ static_cast<OctreeImpl*>(impl)->radiusSearchHost(query, radius, out, max_nn);
+}
+
+void pcl::gpu::Octree::approxNearestSearchHost(const PointType& query, int& out_index, float& sqr_dist)
+{
+ if (!static_cast<OctreeImpl*>(impl)->host_octree.downloaded)
+ internalDownload();
+
+ OctreeImpl::PointType q;
+ q.x = query.x;
+ q.y = query.y;
+ q.z = query.z;
+
+ static_cast<OctreeImpl*>(impl)->approxNearestSearchHost(q, out_index, sqr_dist);
+
+}
+
+void pcl::gpu::Octree::radiusSearch(const Queries& queries, float radius, int max_results, NeighborIndices& results) const
+{
+ assert(queries.size() > 0);
+ results.create(static_cast<int> (queries.size()), max_results);
+ results.sizes.create(queries.size());
+
+ const OctreeImpl::Queries& q = (const OctreeImpl::Queries&)queries;
+ static_cast<OctreeImpl*>(impl)->radiusSearch(q, radius, results);
+}
+
+void pcl::gpu::Octree::radiusSearch(const Queries& queries, const Radiuses& radiuses, int max_results, NeighborIndices& results) const
+{
+ assert(queries.size() > 0);
+ assert(queries.size() == radiuses.size());
+ results.create(static_cast<int> (queries.size()), max_results);
+ results.sizes.create(queries.size());
+
+ const OctreeImpl::Queries& q = (const OctreeImpl::Queries&)queries;
+ static_cast<OctreeImpl*>(impl)->radiusSearch(q, radiuses, results);
+}
+
+void pcl::gpu::Octree::radiusSearch(const Queries& queries, const Indices& indices, float radius, int max_results, NeighborIndices& results) const
+{
+ assert(queries.size() > 0 && indices.size() > 0);
+ results.create(static_cast<int> (indices.size()), max_results);
+ results.sizes.create(indices.size());
+
+ const OctreeImpl::Queries& q = (const OctreeImpl::Queries&)queries;
+ static_cast<OctreeImpl*>(impl)->radiusSearch(q, indices, radius, results);
+}
+
+void pcl::gpu::Octree::approxNearestSearch(const Queries& queries, NeighborIndices& results) const
+{
+ assert(queries.size() > 0);
+ results.create(static_cast<int> (queries.size()), 1);
+
+ const OctreeImpl::Queries& q = (const OctreeImpl::Queries&)queries;
+ static_cast<OctreeImpl*>(impl)->approxNearestSearch(q, results);
+}
+
+void pcl::gpu::Octree::nearestKSearchBatch(const Queries& queries, int k, NeighborIndices& results) const
+{
+ if (k != 1)
+ throw pcl::PCLException("OctreeGPU::knnSearch is supported only for k == 1", __FILE__, "", __LINE__);
+
+ assert(queries.size() > 0);
+ results.create(static_cast<int> (queries.size()), k);
+
+ const OctreeImpl::Queries& q = (const OctreeImpl::Queries&)queries;
+
+ static_cast<OctreeImpl*>(impl)->nearestKSearchBatch(q, k, results);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////
+//////////////// Brute Force Radius Search Mediator //////////////////////////////////
+
+void pcl::gpu::bruteForceRadiusSearchGPU(const Octree::PointCloud& cloud, const PointXYZ& query, float radius, DeviceArray<int>& result, DeviceArray<int>& buffer)
+{
+ typedef OctreeImpl::PointType PointType;
+ typedef OctreeImpl::PointCloud PointCloud;
+
+ PointType query_local;
+ query_local.x = query.x;
+ query_local.y = query.y;
+ query_local.z = query.z;
+
+ Static<sizeof(PointType) == sizeof(OctreeImpl::PointType)>::check();
+
+ PointCloud cloud_local((PointType*)cloud.ptr(), cloud.size());
+ bruteForceRadiusSearch(cloud_local, query_local, radius, result, buffer);
+}
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_GPU_BITONIC_SORT_WARP_HPP
+#define PCL_GPU_BITONIC_SORT_WARP_HPP
+
+namespace pcl
+{
+ namespace device
+ {
+ template<typename T>
+ __device__ __forceinline__ void swap(T& a, T& b) { T t = a; a = b; b = t; }
+
+ template<typename V, typename K>
+ __device__ __forceinline__ void bitonicSortWarp(volatile K* keys, volatile V* vals, unsigned int dir = 1)
+ {
+ const unsigned int arrayLength = 64;
+ unsigned int lane = threadIdx.x & 31;
+
+ for(unsigned int size = 2; size < arrayLength; size <<= 1)
+ {
+ //Bitonic merge
+ unsigned int ddd = dir ^ ( (lane & (size / 2)) != 0 );
+
+ for(unsigned int stride = size / 2; stride > 0; stride >>= 1)
+ {
+ unsigned int pos = 2 * lane - (lane & (stride - 1));
+
+ if ( (keys[pos] > keys[pos + stride]) == ddd )
+ {
+ swap(keys[pos], keys[pos + stride]);
+ swap(vals[pos], vals[pos + stride]);
+ }
+ }
+ }
+
+ //ddd == dir for the last bitonic merge step
+ for(unsigned int stride = arrayLength / 2; stride > 0; stride >>= 1)
+ {
+ unsigned int pos = 2 * lane - (lane & (stride - 1));
+
+ if ( (keys[pos] > keys[pos + stride]) == dir )
+ {
+ swap(keys[pos], keys[pos + stride]);
+ swap(vals[pos], vals[pos + stride]);
+ }
+ }
+ }
+
+ }
+}
+
+#endif /* PCL_GPU_BITONIC_SORT_WARP_HPP */
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#ifndef _PCL_GPU_OCTREE_BOXUTILS_HPP_
+#define _PCL_GPU_OCTREE_BOXUTILS_HPP_
+
+#include "utils/morton.hpp"
+
+namespace pcl
+{
+ namespace device
+ {
+ __device__ __host__ __forceinline__
+ static bool checkIfNodeInsideSphere(const float3& minp, const float3& maxp, const float3& c, float r)
+ {
+ r *= r;
+
+ float d2_xmin = (minp.x - c.x) * (minp.x - c.x);
+ float d2_ymin = (minp.y - c.y) * (minp.y - c.y);
+ float d2_zmin = (minp.z - c.z) * (minp.z - c.z);
+
+ if (d2_xmin + d2_ymin + d2_zmin > r)
+ return false;
+
+ float d2_zmax = (maxp.z - c.z) * (maxp.z - c.z);
+
+ if (d2_xmin + d2_ymin + d2_zmax > r)
+ return false;
+
+ float d2_ymax = (maxp.y - c.y) * (maxp.y - c.y);
+
+ if (d2_xmin + d2_ymax + d2_zmin > r)
+ return false;
+
+ if (d2_xmin + d2_ymax + d2_zmax > r)
+ return false;
+
+ float d2_xmax = (maxp.x - c.x) * (maxp.x - c.x);
+
+ if (d2_xmax + d2_ymin + d2_zmin > r)
+ return false;
+
+ if (d2_xmax + d2_ymin + d2_zmax > r)
+ return false;
+
+ if (d2_xmax + d2_ymax + d2_zmin > r)
+ return false;
+
+ if (d2_xmax + d2_ymax + d2_zmax > r)
+ return false;
+
+ return true;
+ }
+
+ __device__ __host__ __forceinline__
+ static bool checkIfNodeOutsideSphere(const float3& minp, const float3& maxp, const float3& c, float r)
+ {
+ if (maxp.x < (c.x - r) || maxp.y < (c.y - r) || maxp.z < (c.z - r))
+ return true;
+
+ if ((c.x + r) < minp.x || (c.y + r) < minp.y || (c.z + r) < minp.z)
+ return true;
+
+ return false;
+ }
+
+ __device__ __host__ __forceinline__
+ static void calcBoundingBox(int level, int code, float3& res_minp, float3& res_maxp)
+ {
+ int cell_x, cell_y, cell_z;
+ Morton::decomposeCode(code, cell_x, cell_y, cell_z);
+
+ float cell_size_x = (res_maxp.x - res_minp.x) / (1 << level);
+ float cell_size_y = (res_maxp.y - res_minp.y) / (1 << level);
+ float cell_size_z = (res_maxp.z - res_minp.z) / (1 << level);
+
+ res_minp.x += cell_x * cell_size_x;
+ res_minp.y += cell_y * cell_size_y;
+ res_minp.z += cell_z * cell_size_z;
+
+ res_maxp.x = res_minp.x + cell_size_x;
+ res_maxp.y = res_minp.y + cell_size_y;
+ res_maxp.z = res_minp.z + cell_size_z;
+ }
+ }
+}
+
+#endif /* _PCL_GPU_OCTREE_BOXUTILS_HPP_ */
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_GPU_OCTREE_COPYGE_HPP
+#define PCL_GPU_OCTREE_COPYGE_HPP
+
+#include <pcl/gpu/utils/device/warp.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+
+ template <typename T>
+ __device__ void CopyKernel(const T* in, T *out, int length)
+ {
+ int STRIDE = gridDim.x * blockDim.x;
+ for (int idx = (blockIdx.x * blockDim.x) + threadIdx.x; idx < length; idx += STRIDE)
+ {
+ out[idx] = in[idx];
+ }
+ }
+
+ template <typename T>
+ __device__ void GenerateKernel(T* out, int beg, int end)
+ {
+ int length = end - beg;
+ int pos = beg;
+
+ int STRIDE = blockDim.x;
+ for (int idx = threadIdx.x; idx < length; idx += STRIDE, pos += STRIDE)
+ {
+ out[idx] = pos + threadIdx.x;
+ }
+ }
+
+ template <typename T>
+ __device__ void GenerateTasksKernel(T* out, int beg, int end, int level)
+ {
+ int length = end - beg;
+ int pos = beg;
+
+ int STRIDE = blockDim.x;
+ for (int idx = threadIdx.x; idx < length; idx += STRIDE, pos += STRIDE)
+ {
+ out[idx] = ((pos + threadIdx.x) << 8) + level;
+ }
+ }
+ }
+}
+
+#endif /* PCL_GPU_OCTREE_COPYGE_HPP */
\ No newline at end of file
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_GPU_EMULATION
+#define PCL_GPU_EMULATION
+
+#include "utils/warp_reduce.hpp"
+
+namespace pcl
+{
+ namespace device
+ {
+ struct Emulation
+ {
+ static __forceinline__ __device__ int Ballot(int predicate, volatile int* cta_buffer)
+ {
+#if __CUDA_ARCH__ >= 200
+ (void)cta_buffer;
+ return __ballot(predicate);
+#else
+ int tid = threadIdx.x;
+ cta_buffer[tid] = predicate ? (1 << (tid & 31)) : 0;
+ return warp_reduce(cta_buffer);
+#endif
+ }
+ };
+ }
+}
+
+#endif /* PCL_GPU_EMULATION */
\ No newline at end of file
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_GPU_OCTREE_MORTON_HPP
+#define PCL_GPU_OCTREE_MORTON_HPP
+
+
+namespace pcl
+{
+ namespace device
+ {
+ struct Morton
+ {
+ const static int levels = 10;
+ const static int bits_per_level = 3;
+ const static int nbits = levels * bits_per_level;
+
+ typedef int code_t;
+
+ __device__ __host__ __forceinline__
+ static int spreadBits(int x, int offset)
+ {
+ //......................9876543210
+ x = (x | (x << 10)) & 0x000f801f; //............98765..........43210
+ x = (x | (x << 4)) & 0x00e181c3; //........987....56......432....10
+ x = (x | (x << 2)) & 0x03248649; //......98..7..5..6....43..2..1..0
+ x = (x | (x << 2)) & 0x09249249; //....9..8..7..5..6..4..3..2..1..0
+
+ return x << offset;
+ }
+
+ __device__ __host__ __forceinline__
+ static int compactBits(int x, int offset)
+ {
+ x = ( x >> offset ) & 0x09249249; //....9..8..7..5..6..4..3..2..1..0
+ x = (x | (x >> 2)) & 0x03248649; //......98..7..5..6....43..2..1..0
+ x = (x | (x >> 2)) & 0x00e181c3; //........987....56......432....10
+ x = (x | (x >> 4)) & 0x000f801f; //............98765..........43210
+ x = (x | (x >> 10)) & 0x000003FF; //......................9876543210
+
+ return x;
+ }
+
+ __device__ __host__ __forceinline__
+ static code_t createCode(int cell_x, int cell_y, int cell_z)
+ {
+ return spreadBits(cell_x, 0) | spreadBits(cell_y, 1) | spreadBits(cell_z, 2);
+ }
+
+ __device__ __host__ __forceinline__
+ static void decomposeCode(code_t code, int& cell_x, int& cell_y, int& cell_z)
+ {
+ cell_x = compactBits(code, 0);
+ cell_y = compactBits(code, 1);
+ cell_z = compactBits(code, 2);
+ }
+
+ __host__ __device__ __forceinline__
+ static code_t extractLevelCode(code_t code, int level)
+ {
+ return (code >> (nbits - 3 * (level + 1) )) & 7;
+ }
+
+ __host__ __device__ __forceinline__
+ static code_t shiftLevelCode(code_t level_code, int level)
+ {
+ return level_code << (nbits - 3 * (level + 1));
+ }
+ };
+
+ struct CalcMorton
+ {
+ const static int depth_mult = 1 << Morton::levels;
+
+ float3 minp_;
+ float3 dims_;
+
+ __device__ __host__ __forceinline__ CalcMorton(float3 minp, float3 maxp) : minp_(minp)
+ {
+ dims_.x = maxp.x - minp.x;
+ dims_.y = maxp.y - minp.y;
+ dims_.z = maxp.z - minp.z;
+ }
+
+ __device__ __host__ __forceinline__ Morton::code_t operator()(const float3& p) const
+ {
+ int cellx = min((int)floor(depth_mult * (p.x - minp_.x)/dims_.x), depth_mult - 1);
+ int celly = min((int)floor(depth_mult * (p.y - minp_.y)/dims_.y), depth_mult - 1);
+ int cellz = min((int)floor(depth_mult * (p.z - minp_.z)/dims_.z), depth_mult - 1);
+
+ return Morton::createCode(cellx, celly, cellz);
+ }
+ __device__ __host__ __forceinline__ Morton::code_t operator()(const float4& p) const
+ {
+ return (*this)(make_float3(p.x, p.y, p.z));
+ }
+ };
+
+ struct CompareByLevelCode
+ {
+ int level;
+
+ __device__ __host__ __forceinline__
+ CompareByLevelCode(int level_arg) : level(level_arg) {}
+
+ __device__ __host__ __forceinline__
+ bool operator()(Morton::code_t code1, Morton::code_t code2) const
+ {
+ return Morton::extractLevelCode(code1, level) < Morton::extractLevelCode(code2, level);
+ }
+ };
+ }
+}
+
+#endif /* PCL_GPU_OCTREE_MORTON_HPP */
\ No newline at end of file
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_GPU_OCTREE_PRIORITY_ITERATOR
+#define PCL_GPU_OCTREE_PRIORITY_ITERATOR
+
+namespace pcl
+{
+ namespace device
+ {
+ struct OctreePriorityIteratorDevice
+ {
+ int level;
+ int node_idx;
+ int lenght;
+ const OctreeGlobalWithBox& octree;
+
+ __device__ __forceinline__ OctreePriorityIteratorDevice(const OctreeGlobalWithBox& octree_arg) : octree(octree_arg)
+ {
+ node_idx = 0;
+ level = 0;
+ lenght = 1;
+ }
+
+ __device__ __forceinline__ void gotoNextLevel(int first, int len)
+ {
+ node_idx = first;
+ lenght = len;
+ ++level;
+ }
+
+ __device__ __forceinline__ int operator*() const
+ {
+ return node_idx;
+ }
+
+ __device__ __forceinline__ void operator++()
+ {
+#if 1
+ while(level >= 0)
+ {
+ if (lenght > 1)
+ {
+ lenght--;
+ node_idx++;
+ break;
+ }
+
+ if (node_idx == 0)
+ {
+ level = -1;
+ return;
+ }
+
+ node_idx = octree.parent[node_idx];
+ --level;
+ if (node_idx == 0)
+ {
+ level = -1;
+ return;
+ }
+
+ int parent = octree.nodes[octree.parent[node_idx]];
+ int parent_first = parent >> 8;
+ int parent_len = __popc(parent & 0xFF);
+
+ int pos = node_idx - parent_first;
+
+ lenght = parent_len - pos;
+ }
+#endif
+ }
+
+ };
+ }
+}
+
+#endif /* PCL_GPU_OCTREE_PRIORITY_ITERATOR */
\ No newline at end of file
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_GPU_OCTREE_SCAN_BLOCK_HPP
+#define PCL_GPU_OCTREE_SCAN_BLOCK_HPP
+
+
+namespace pcl
+{
+ namespace device
+ {
+ enum ScanKind { exclusive, inclusive } ;
+
+ template <ScanKind Kind , class T>
+ __device__ __forceinline__ T scan_warp ( volatile T *ptr , const unsigned int idx = threadIdx.x )
+ {
+ const unsigned int lane = idx & 31; // index of thread in warp (0..31)
+
+ if ( lane >= 1) ptr [idx ] = ptr [idx - 1] + ptr [idx];
+ if ( lane >= 2) ptr [idx ] = ptr [idx - 2] + ptr [idx];
+ if ( lane >= 4) ptr [idx ] = ptr [idx - 4] + ptr [idx];
+ if ( lane >= 8) ptr [idx ] = ptr [idx - 8] + ptr [idx];
+ if ( lane >= 16) ptr [idx ] = ptr [idx - 16] + ptr [idx];
+
+ if( Kind == inclusive )
+ return ptr [idx ];
+ else
+ return (lane > 0) ? ptr [idx - 1] : 0;
+ }
+
+ template <ScanKind Kind , class T>
+ __device__ __forceinline__ T scan_block( volatile T *ptr , const unsigned int idx = threadIdx.x )
+ {
+ const unsigned int lane = idx & 31;
+ const unsigned int warpid = idx >> 5;
+
+ // Step 1: Intra - warp scan in each warp
+ T val = scan_warp <Kind>( ptr , idx );
+
+ __syncthreads ();
+
+ // Step 2: Collect per - warp partial results
+
+ /* if( warpid == 0 )
+ if( lane == 31 )
+ ptr [ warpid ] = ptr [idx ];
+
+ __syncthreads ();
+
+ if( warpid > 0 ) */
+ if( lane == 31 )
+ ptr [ warpid ] = ptr [idx ];
+
+ __syncthreads ();
+
+ // Step 3: Use 1st warp to scan per - warp results
+ if( warpid == 0 )
+ scan_warp<inclusive>( ptr , idx );
+
+ __syncthreads ();
+
+ // Step 4: Accumulate results from Steps 1 and 3
+ if ( warpid > 0)
+ val = ptr [warpid -1] + val;
+
+ __syncthreads ();
+
+ // Step 5: Write and return the final result
+ ptr[idx] = val;
+
+ __syncthreads ();
+
+ return val ;
+ }
+ }
+}
+
+#endif /* PCL_GPU_OCTREE_SCAN_BLOCK_HPP */
\ No newline at end of file
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_GPU_WARP_REDUCE
+#define PCL_GPU_WARP_REDUCE
+
+
+namespace pcl
+{
+ namespace device
+ {
+ template <class T>
+ __device__ __forceinline__ T warp_reduce ( volatile T *ptr , const unsigned int tid = threadIdx.x )
+ {
+ const unsigned int lane = tid & 31; // index of thread in warp (0..31)
+
+ if (lane < 16)
+ {
+ T partial = ptr[tid];
+
+ ptr[tid] = partial = partial + ptr[tid + 16];
+ ptr[tid] = partial = partial + ptr[tid + 8];
+ ptr[tid] = partial = partial + ptr[tid + 4];
+ ptr[tid] = partial = partial + ptr[tid + 2];
+ ptr[tid] = partial = partial + ptr[tid + 1];
+ }
+ return ptr[tid - lane];
+
+ }
+ }
+}
+
+#endif
\ No newline at end of file
--- /dev/null
+if(BUILD_TESTS)
+ set(the_test_target test_gpu_octree)
+
+ get_filename_component(DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
+ get_filename_component(INC "${DIR}/../../../octree/include" REALPATH)
+ include_directories("${INC}")
+
+ FILE(GLOB test_src *.cpp *.hpp)
+ #PCL_ADD_TEST(a_gpu_octree_test "${the_test_target}" FILES ${test_src} LINK_WITH pcl_octree pcl_gpu_containers pcl_gpu_octree)
+ #add_dependencies("${the_test_target}" pcl_octree pcl_gpu_containes pcl_gpu_octree)
+
+endif(BUILD_TESTS)
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#ifndef _PCL_TEST_GPU_OCTREE_DATAGEN_
+#define _PCL_TEST_GPU_OCTREE_DATAGEN_
+
+#include <vector>
+#include <algorithm>
+#include <iostream>
+#include <Eigen/StdVector>
+#include <cstdlib>
+
+
+#if defined (_WIN32) || defined(_WIN64)
+ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(pcl::PointXYZ)
+#endif
+
+struct DataGenerator
+{
+ typedef pcl::gpu::Octree::PointType PointType;
+
+ size_t data_size;
+ size_t tests_num;
+
+ float cube_size;
+ float max_radius;
+
+ float shared_radius;
+
+ std::vector<PointType> points;
+ std::vector<PointType> queries;
+ std::vector<float> radiuses;
+ std::vector< std::vector<int> > bfresutls;
+
+ std::vector<int> indices;
+
+ DataGenerator() : data_size(871000), tests_num(10000), cube_size(1024.f)
+ {
+ max_radius = cube_size/15.f;
+ shared_radius = cube_size/20.f;
+ }
+
+ void operator()()
+ {
+ srand (0);
+
+ points.resize(data_size);
+ for(size_t i = 0; i < data_size; ++i)
+ {
+ points[i].x = ((float)rand())/RAND_MAX * cube_size;
+ points[i].y = ((float)rand())/RAND_MAX * cube_size;
+ points[i].z = ((float)rand())/RAND_MAX * cube_size;
+ }
+
+
+ queries.resize(tests_num);
+ radiuses.resize(tests_num);
+ for (size_t i = 0; i < tests_num; ++i)
+ {
+ queries[i].x = ((float)rand())/RAND_MAX * cube_size;
+ queries[i].y = ((float)rand())/RAND_MAX * cube_size;
+ queries[i].z = ((float)rand())/RAND_MAX * cube_size;
+ radiuses[i] = ((float)rand())/RAND_MAX * max_radius;
+ };
+
+ for(size_t i = 0; i < tests_num/2; ++i)
+ indices.push_back(i*2);
+ }
+
+ void bruteForceSearch(bool log = false, float radius = -1.f)
+ {
+ if (log)
+ std::cout << "BruteForceSearch";
+
+ int value100 = std::min<int>(tests_num, 50);
+ int step = tests_num/value100;
+
+ bfresutls.resize(tests_num);
+ for(size_t i = 0; i < tests_num; ++i)
+ {
+ if (log && i % step == 0)
+ {
+ std::cout << ".";
+ std::cout.flush();
+ }
+
+ std::vector<int>& curr_res = bfresutls[i];
+ curr_res.clear();
+
+ float query_radius = radius > 0 ? radius : radiuses[i];
+ const PointType& query = queries[i];
+
+ for(size_t ind = 0; ind < points.size(); ++ind)
+ {
+ const PointType& point = points[ind];
+
+ float dx = query.x - point.x;
+ float dy = query.y - point.y;
+ float dz = query.z - point.z;
+
+ if (dx*dx + dy*dy + dz*dz < query_radius * query_radius)
+ curr_res.push_back(ind);
+ }
+
+ std::sort(curr_res.begin(), curr_res.end());
+ }
+ if (log)
+ std::cout << "Done" << std::endl;
+ }
+
+ void printParams() const
+ {
+ std::cout << "Points number = " << data_size << std::endl;
+ std::cout << "Queries number = " << tests_num << std::endl;
+ std::cout << "Cube size = " << cube_size << std::endl;
+ std::cout << "Max radius = " << max_radius << std::endl;
+ std::cout << "Shared radius = " << shared_radius << std::endl;
+ }
+
+ template<typename Dst>
+ struct ConvPoint
+ {
+ Dst operator()(const PointType& src) const
+ {
+ Dst dst;
+ dst.x = src.x;
+ dst.y = src.y;
+ dst.z = src.z;
+ return dst;
+ }
+ };
+
+};
+
+#endif /* _PCL_TEST_GPU_OCTREE_DATAGEN_ */
+
+
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#if defined _MSC_VER
+ #pragma warning (disable : 4996 4530)
+#endif
+
+#include <gtest/gtest.h>
+
+#include<iostream>
+#include<algorithm>
+
+#if defined _MSC_VER
+ #pragma warning (disable: 4521)
+#endif
+
+#include <pcl/point_cloud.h>
+#include <pcl/octree/octree.h>
+
+#if defined _MSC_VER
+ #pragma warning (default: 4521)
+#endif
+
+
+#include <pcl/gpu/octree/octree.hpp>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/common/time.h>
+#include "data_source.hpp"
+
+using namespace pcl::gpu;
+using namespace std;
+
+using pcl::ScopeTime;
+
+#if defined HAVE_OPENCV
+ #include "opencv2/contrib/contrib.hpp"
+#endif
+
+//TEST(PCL_OctreeGPU, DISABLED_perfomance)
+TEST(PCL_OctreeGPU, perfomance)
+{
+ DataGenerator data;
+ data.data_size = 871000;
+ data.tests_num = 10000;
+ data.cube_size = 1024.f;
+ data.max_radius = data.cube_size/15.f;
+ data.shared_radius = data.cube_size/15.f;
+ data.printParams();
+
+ //const int k = 32;
+
+ cout << "sizeof(pcl::gpu::Octree::PointType): " << sizeof(pcl::gpu::Octree::PointType) << endl;
+ //cout << "k = " << k << endl;
+ //generate data
+ data();
+
+ //prepare device cloud
+ pcl::gpu::Octree::PointCloud cloud_device;
+ cloud_device.upload(data.points);
+
+ //prepare queries_device
+ pcl::gpu::Octree::Queries queries_device;
+ pcl::gpu::Octree::Radiuses radiuses_device;
+ queries_device.upload(data.queries);
+ radiuses_device.upload(data.radiuses);
+
+ //prepare host cloud
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_host(new pcl::PointCloud<pcl::PointXYZ>);
+ cloud_host->width = data.points.size();
+ cloud_host->height = 1;
+ cloud_host->points.resize (cloud_host->width * cloud_host->height);
+ std::transform(data.points.begin(), data.points.end(), cloud_host->points.begin(), DataGenerator::ConvPoint<pcl::PointXYZ>());
+
+ float host_octree_resolution = 25.f;
+
+ cout << "[!] Host octree resolution: " << host_octree_resolution << endl << endl;
+
+ cout << "====== Build perfomance =====" << endl;
+ // build device octree
+ pcl::gpu::Octree octree_device;
+ octree_device.setCloud(cloud_device);
+ {
+ ScopeTime up("gpu-build");
+ octree_device.build();
+ }
+ {
+ ScopeTime up("gpu-download");
+ octree_device.internalDownload();
+ }
+
+ //build host octree
+ pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree_host(host_octree_resolution);
+ octree_host.setInputCloud (cloud_host);
+ {
+ ScopeTime t("host-build");
+ octree_host.addPointsFromInputCloud();
+ }
+
+ // build opencv octree
+#ifdef HAVE_OPENCV
+ cv::Octree octree_opencv;
+ const static int opencv_octree_points_per_leaf = 32;
+ vector<cv::Point3f> opencv_points(data.points.size());
+ std::transform(data.points.begin(), data.points.end(), opencv_points.begin(), DataGenerator::ConvPoint<cv::Point3f>());
+
+ {
+ ScopeTime t("opencv-build");
+ octree_opencv.buildTree(opencv_points, 10, opencv_octree_points_per_leaf);
+ }
+#endif
+
+ //// Radius search perfomance ///
+
+ const int max_answers = 500;
+ float dist;
+ int inds;
+
+ //host buffers
+ vector<int> indeces;
+ vector<float> pointRadiusSquaredDistance;
+#ifdef HAVE_OPENCV
+ vector<cv::Point3f> opencv_results;
+#endif
+
+ //reserve
+ indeces.reserve(data.data_size);
+ pointRadiusSquaredDistance.reserve(data.data_size);
+#ifdef HAVE_OPENCV
+ opencv_results.reserve(data.data_size);
+#endif
+
+ //device buffers
+ pcl::gpu::DeviceArray<int> bruteforce_results_device, buffer(cloud_device.size());
+ pcl::gpu::NeighborIndices result_device(queries_device.size(), max_answers);
+
+ //pcl::gpu::Octree::BatchResult distsKNN_device(queries_device.size() * k);
+ //pcl::gpu::Octree::BatchResultSqrDists indsKNN_device(queries_device.size() * k);
+
+ cout << "====== Separate radius for each query =====" << endl;
+
+ {
+ ScopeTime up("gpu--radius-search-batch-all");
+ octree_device.radiusSearch(queries_device, radiuses_device, max_answers, result_device);
+ }
+
+ {
+ ScopeTime up("gpu-radius-search-{host}-all");
+ for(size_t i = 0; i < data.tests_num; ++i)
+ octree_device.radiusSearchHost(data.queries[i], data.radiuses[i], indeces, max_answers);
+ }
+
+ {
+ ScopeTime up("host-radius-search-all");
+ for(size_t i = 0; i < data.tests_num; ++i)
+ octree_host.radiusSearch(pcl::PointXYZ(data.queries[i].x, data.queries[i].y, data.queries[i].z),
+ data.radiuses[i], indeces, pointRadiusSquaredDistance, max_answers);
+ }
+
+ {
+ ScopeTime up("gpu_bruteforce-radius-search-all");
+ for(size_t i = 0; i < data.tests_num; ++i)
+ pcl::gpu::bruteForceRadiusSearchGPU(cloud_device, data.queries[i], data.radiuses[i], bruteforce_results_device, buffer);
+ }
+
+ cout << "====== Shared radius (" << data.shared_radius << ") =====" << endl;
+
+ {
+ ScopeTime up("gpu-radius-search-batch-all");
+ octree_device.radiusSearch(queries_device, data.shared_radius, max_answers, result_device);
+ }
+
+ {
+ ScopeTime up("gpu-radius-search-{host}-all");
+ for(size_t i = 0; i < data.tests_num; ++i)
+ octree_device.radiusSearchHost(data.queries[i], data.shared_radius, indeces, max_answers);
+ }
+
+ {
+ ScopeTime up("host-radius-search-all");
+ for(size_t i = 0; i < data.tests_num; ++i)
+ octree_host.radiusSearch(pcl::PointXYZ(data.queries[i].x, data.queries[i].y, data.queries[i].z),
+ data.radiuses[i], indeces, pointRadiusSquaredDistance, max_answers);
+ }
+
+ {
+ ScopeTime up("gpu-radius-bruteforce-search-all");
+ for(size_t i = 0; i < data.tests_num; ++i)
+ pcl::gpu::bruteForceRadiusSearchGPU(cloud_device, data.queries[i], data.shared_radius, bruteforce_results_device, buffer);
+ }
+
+ cout << "====== Approx nearest search =====" << endl;
+
+ {
+ ScopeTime up("gpu-approx-nearest-batch-all");
+ octree_device.approxNearestSearch(queries_device, result_device);
+ }
+
+ {
+ ScopeTime up("gpu-approx-nearest-search-{host}-all");
+ for(size_t i = 0; i < data.tests_num; ++i)
+ octree_device.approxNearestSearchHost(data.queries[i], inds, dist);
+ }
+
+ {
+ ScopeTime up("host-approx-nearest-search-all");
+ for(size_t i = 0; i < data.tests_num; ++i)
+ octree_host.approxNearestSearch(data.queries[i], inds, dist);
+ }
+
+ /* cout << "====== knn search ( k fixed to " << k << " ) =====" << endl;
+ {
+ ScopeTime up("gpu-knn-batch-all");
+ octree_device.nearestKSearchBatch(queries_device, k, distsKNN_device, indsKNN_device);
+ }
+
+ {
+ ScopeTime up("host-knn-search-all");
+ for(size_t i = 0; i < data.tests_num; ++i)
+ octree_host.nearestKSearch(data.queries[i], k, indeces, pointRadiusSquaredDistance);
+ }*/
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#if defined _MSC_VER
+ #pragma warning (disable : 4996 4530)
+#endif
+
+#include <gtest/gtest.h>
+
+#include<iostream>
+#include<fstream>
+#include<algorithm>
+
+#if defined _MSC_VER
+ #pragma warning (disable: 4521)
+#endif
+#include <pcl/point_cloud.h>
+#include <pcl/octree/octree.h>
+#if defined _MSC_VER
+ #pragma warning (default: 4521)
+#endif
+
+#include <pcl/gpu/octree/octree.hpp>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/utils/safe_call.hpp>
+
+#include "data_source.hpp"
+
+using namespace pcl::gpu;
+using namespace std;
+
+//TEST(PCL_OctreeGPU, DISABLED_approxNearesSearch)
+TEST(PCL_OctreeGPU, approxNearesSearch)
+{
+ DataGenerator data;
+ data.data_size = 871000;
+ data.tests_num = 10000;
+ data.cube_size = 1024.f;
+ data.max_radius = data.cube_size/30.f;
+ data.shared_radius = data.cube_size/30.f;
+ data.printParams();
+
+ const float host_octree_resolution = 25.f;
+
+ //generate
+ data();
+
+ //prepare device cloud
+ pcl::gpu::Octree::PointCloud cloud_device;
+ cloud_device.upload(data.points);
+
+
+ //prepare host cloud
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_host(new pcl::PointCloud<pcl::PointXYZ>);
+ cloud_host->width = data.points.size();
+ cloud_host->height = 1;
+ cloud_host->points.resize (cloud_host->width * cloud_host->height);
+ std::transform(data.points.begin(), data.points.end(), cloud_host->points.begin(), DataGenerator::ConvPoint<pcl::PointXYZ>());
+
+ //gpu build
+ pcl::gpu::Octree octree_device;
+ octree_device.setCloud(cloud_device);
+ octree_device.build();
+
+ //build host octree
+ pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree_host(host_octree_resolution);
+ octree_host.setInputCloud (cloud_host);
+ octree_host.addPointsFromInputCloud();
+
+ //upload queries
+ pcl::gpu::Octree::Queries queries_device;
+ queries_device.upload(data.queries);
+
+
+ //prepare output buffers on device
+ pcl::gpu::NeighborIndices result_device(data.tests_num, 1);
+ vector<int> result_host_pcl(data.tests_num);
+ vector<int> result_host_gpu(data.tests_num);
+ vector<float> dists_pcl(data.tests_num);
+ vector<float> dists_gpu(data.tests_num);
+
+ //search GPU shared
+ octree_device.approxNearestSearch(queries_device, result_device);
+
+ vector<int> downloaded;
+ result_device.data.download(downloaded);
+
+ for(size_t i = 0; i < data.tests_num; ++i)
+ {
+ octree_host.approxNearestSearch(data.queries[i], result_host_pcl[i], dists_pcl[i]);
+ octree_device.approxNearestSearchHost(data.queries[i], result_host_gpu[i], dists_gpu[i]);
+ }
+
+ ASSERT_EQ ( ( downloaded == result_host_gpu ), true );
+
+ int count_gpu_better = 0;
+ int count_pcl_better = 0;
+ float diff_pcl_better = 0;
+ for(size_t i = 0; i < data.tests_num; ++i)
+ {
+ float diff = dists_pcl[i] - dists_gpu[i];
+ bool gpu_better = diff > 0;
+
+ ++(gpu_better ? count_gpu_better : count_pcl_better);
+
+ if (!gpu_better)
+ diff_pcl_better +=fabs(diff);
+ }
+
+ diff_pcl_better /=count_pcl_better;
+
+ cout << "count_gpu_better: " << count_gpu_better << endl;
+ cout << "count_pcl_better: " << count_pcl_better << endl;
+ cout << "avg_diff_pcl_better: " << diff_pcl_better << endl;
+
+}
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include <gtest/gtest.h>
+
+#include <iostream>
+#include <numeric>
+
+#if defined _MSC_VER
+ #pragma warning (disable: 4521)
+#endif
+
+#include <pcl/point_cloud.h>
+#include <pcl/octree/octree.h>
+
+#if defined _MSC_VER
+ #pragma warning (default: 4521)
+#endif
+
+#include <pcl/gpu/octree/octree.hpp>
+#include <pcl/gpu/containers/device_array.h>
+
+#include "data_source.hpp"
+
+using namespace std;
+using namespace pcl::gpu;
+
+
+//TEST (PCL_GPU, DISABLED_bruteForceRadiusSeachGPU)
+TEST (PCL_GPU, bruteForceRadiusSeachGPU)
+{
+ DataGenerator data;
+ data.data_size = 871000;
+ data.tests_num = 100;
+ data.cube_size = 1024.f;
+ data.max_radius = data.cube_size/15.f;
+ data.shared_radius = data.cube_size/20.f;
+ data.printParams();
+
+ //generate
+ data();
+
+ // brute force radius search
+ data.bruteForceSearch();
+
+ //prepare gpu cloud
+ pcl::gpu::Octree::PointCloud cloud_device;
+ cloud_device.upload(data.points);
+
+ pcl::gpu::DeviceArray<int> results_device, buffer(cloud_device.size());
+
+ vector<int> results_host;
+ vector<size_t> sizes;
+ for(size_t i = 0; i < data.tests_num; ++i)
+ {
+ pcl::gpu::bruteForceRadiusSearchGPU(cloud_device, data.queries[i], data.radiuses[i], results_device, buffer);
+
+ results_device.download(results_host);
+ std::sort(results_host.begin(), results_host.end());
+
+ ASSERT_EQ ( (results_host == data.bfresutls[i]), true );
+ sizes.push_back(results_device.size());
+ }
+
+ float avg_size = std::accumulate(sizes.begin(), sizes.end(), (size_t)0) * (1.f/sizes.size());;
+
+ cout << "avg_result_size = " << avg_size << endl;
+ ASSERT_GT(avg_size, 5);
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include <numeric>
+#include <algorithm>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#if defined _MSC_VER
+ #pragma warning (disable: 4521)
+#endif
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/octree/octree.h>
+
+#if defined _MSC_VER
+ #pragma warning (default: 4521)
+#endif
+
+#include <pcl/gpu/octree/octree.hpp>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/containers/initialization.h>
+
+#include "data_source.hpp"
+
+using namespace std;
+using namespace pcl;
+using namespace pcl::gpu;
+
+//TEST(PCL_OctreeGPU, DISABLED_hostRadiusSearch)
+TEST(PCL_OctreeGPU, hostRadiusSearch)
+{
+ DataGenerator data;
+ data.data_size = 871000;
+ data.tests_num = 10000;
+ data.cube_size = 1024.f;
+ data.max_radius = data.cube_size/15.f;
+ data.shared_radius = data.cube_size/20.f;
+ data.printParams();
+
+ //generate
+ data();
+
+ //prepare device cloud
+ pcl::gpu::Octree::PointCloud cloud_device;
+ cloud_device.upload(data.points);
+
+ //prepare host cloud
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_host(new pcl::PointCloud<pcl::PointXYZ>);
+ cloud_host->width = data.points.size();
+ cloud_host->height = 1;
+ cloud_host->points.resize (cloud_host->width * cloud_host->height);
+ std::transform(data.points.begin(), data.points.end(), cloud_host->points.begin(), DataGenerator::ConvPoint<pcl::PointXYZ>());
+
+ // build device octree
+ pcl::gpu::Octree octree_device;
+ octree_device.setCloud(cloud_device);
+ octree_device.build();
+
+
+
+ // build host octree
+ float resolution = 25.f;
+ cout << "[!]Octree resolution: " << resolution << endl;
+ pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree_host(resolution);
+ octree_host.setInputCloud (cloud_host);
+ octree_host.addPointsFromInputCloud ();
+
+ //perform bruteForceSearch
+ data.bruteForceSearch(true);
+
+ vector<int> sizes;
+ sizes.reserve(data.tests_num);
+ octree_device.internalDownload();
+
+ for(size_t i = 0; i < data.tests_num; ++i)
+ {
+ //search host on octree tha was built on device
+ vector<int> results_host_gpu; //host search
+ octree_device.radiusSearchHost(data.queries[i], data.radiuses[i], results_host_gpu);
+
+ //search host
+ vector<float> dists;
+ vector<int> results_host;
+ octree_host.radiusSearch(pcl::PointXYZ(data.queries[i].x, data.queries[i].y, data.queries[i].z), data.radiuses[i], results_host, dists);
+
+ std::sort(results_host_gpu.begin(), results_host_gpu.end());
+ std::sort(results_host.begin(), results_host.end());
+
+ ASSERT_EQ ( (results_host_gpu == results_host ), true );
+ ASSERT_EQ ( (results_host_gpu == data.bfresutls[i]), true );
+ sizes.push_back(results_host.size());
+ }
+
+ float avg_size = std::accumulate(sizes.begin(), sizes.end(), 0) * (1.f/sizes.size());;
+
+ cout << "avg_result_size = " << avg_size << endl;
+ ASSERT_GT(avg_size, 5);
+}
+
+
+int main (int argc, char** argv)
+{
+ const int device = 0;
+ pcl::gpu::setDevice(device);
+ pcl::gpu::printShortCudaDeviceInfo(device);
+ testing::InitGoogleTest (&argc, argv);
+ return (RUN_ALL_TESTS ());
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#if defined _MSC_VER
+ #pragma warning (disable : 4996 4530)
+#endif
+
+#include <gtest/gtest.h>
+
+#include<iostream>
+#include<fstream>
+#include<algorithm>
+
+#if defined _MSC_VER
+ #pragma warning (disable: 4521)
+#endif
+#include <pcl/point_cloud.h>
+#include <pcl/octree/octree.h>
+#if defined _MSC_VER
+ #pragma warning (default: 4521)
+#endif
+
+#include <pcl/gpu/octree/octree.hpp>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/common/time.h>
+#include "data_source.hpp"
+
+using namespace pcl::gpu;
+using namespace std;
+
+
+struct PriorityPair
+{
+ int index;
+ float dist2;
+
+ bool operator<(const PriorityPair& other) const { return dist2 < other.dist2; }
+
+ bool operator==(const PriorityPair& other) const { return dist2 == other.dist2 && index == other.index; }
+};
+
+//TEST(PCL_OctreeGPU, DISABLED_exactNeighbourSearch)
+TEST(PCL_OctreeGPU, exactNeighbourSearch)
+{
+ DataGenerator data;
+ data.data_size = 871000;
+ data.tests_num = 10000;
+ data.cube_size = 1024.f;
+ data.max_radius = data.cube_size/30.f;
+ data.shared_radius = data.cube_size/30.f;
+ data.printParams();
+
+ const float host_octree_resolution = 25.f;
+ const int k = 1; // only this is supported
+
+ //generate
+ data();
+
+ //prepare device cloud
+ pcl::gpu::Octree::PointCloud cloud_device;
+ cloud_device.upload(data.points);
+
+ //prepare host cloud
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_host(new pcl::PointCloud<pcl::PointXYZ>);
+ cloud_host->width = data.points.size();
+ cloud_host->height = 1;
+ cloud_host->points.resize (cloud_host->width * cloud_host->height);
+ std::transform(data.points.begin(), data.points.end(), cloud_host->points.begin(), DataGenerator::ConvPoint<pcl::PointXYZ>());
+
+ //gpu build
+ pcl::gpu::Octree octree_device;
+ octree_device.setCloud(cloud_device);
+ octree_device.build();
+
+ //build host octree
+ pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree_host(host_octree_resolution);
+ octree_host.setInputCloud (cloud_host);
+ octree_host.addPointsFromInputCloud();
+
+ //upload queries
+ pcl::gpu::Octree::Queries queries_device;
+ queries_device.upload(data.queries);
+
+ //prepare output buffers on device
+ pcl::gpu::NeighborIndices result_device(data.tests_num, k);
+
+ //prepare output buffers on host
+ vector<vector< int> > result_host(data.tests_num);
+ vector<vector<float> > dists_host(data.tests_num);
+ for(size_t i = 0; i < data.tests_num; ++i)
+ {
+ result_host[i].reserve(k);
+ dists_host[i].reserve(k);
+ }
+
+ //search GPU shared
+ {
+ pcl::ScopeTime time("1nn-gpu");
+ octree_device.nearestKSearchBatch(queries_device, k, result_device);
+ }
+
+ vector<int> downloaded, downloaded_cur;
+ result_device.data.download(downloaded);
+
+ {
+ pcl::ScopeTime time("1nn-cpu");
+ for(size_t i = 0; i < data.tests_num; ++i)
+ octree_host.nearestKSearch(data.queries[i], k, result_host[i], dists_host[i]);
+ }
+
+ //verify results
+ for(size_t i = 0; i < data.tests_num; ++i)
+ {
+ //cout << i << endl;
+ vector<int>& results_host_cur = result_host[i];
+ vector<float>& dists_host_cur = dists_host[i];
+
+ int beg = i * k;
+ int end = beg + k;
+
+ downloaded_cur.assign(downloaded.begin() + beg, downloaded.begin() + end);
+
+ vector<PriorityPair> pairs_host;
+ vector<PriorityPair> pairs_gpu;
+ for(int n = 0; n < k; ++n)
+ {
+ PriorityPair host;
+ host.index = results_host_cur[n];
+ host.dist2 = dists_host_cur[n];
+ pairs_host.push_back(host);
+
+ PriorityPair gpu;
+ gpu.index = downloaded_cur[n];
+
+ float dist = (data.queries[i].getVector3fMap() - data.points[gpu.index].getVector3fMap()).norm();
+ gpu.dist2 = dist * dist;
+ pairs_gpu.push_back(gpu);
+ }
+
+ std::sort(pairs_host.begin(), pairs_host.end());
+ std::sort(pairs_gpu.begin(), pairs_gpu.end());
+
+ while (pairs_host.size ())
+ {
+ ASSERT_EQ ( pairs_host.back().index , pairs_gpu.back().index );
+ EXPECT_NEAR ( pairs_host.back().dist2 , pairs_gpu.back().dist2, 1e-2 );
+
+ pairs_host.pop_back();
+ pairs_gpu.pop_back();
+ }
+ }
+}
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include <gtest/gtest.h>
+
+#include <iostream>
+#include <fstream>
+#include <numeric>
+
+#if defined _MSC_VER
+ #pragma warning (disable: 4521)
+#endif
+
+#include <pcl/point_cloud.h>
+#include <pcl/octree/octree.h>
+
+#if defined _MSC_VER
+ #pragma warning (default: 4521)
+#endif
+
+#include <pcl/gpu/octree/octree.hpp>
+#include <pcl/gpu/containers/device_array.h>
+
+#include "data_source.hpp"
+
+using namespace std;
+using namespace pcl::gpu;
+
+//TEST(PCL_OctreeGPU, DISABLED_batchRadiusSearch)
+TEST(PCL_OctreeGPU, batchRadiusSearch)
+{
+ DataGenerator data;
+ data.data_size = 871000;
+ data.tests_num = 10000;
+ data.cube_size = 1024.f;
+ data.max_radius = data.cube_size/30.f;
+ data.shared_radius = data.cube_size/30.f;
+ data.printParams();
+
+ const int max_answers = 333;
+
+ //generate
+ data();
+
+ //prepare gpu cloud
+
+ pcl::gpu::Octree::PointCloud cloud_device;
+ cloud_device.upload(data.points);
+
+ //gpu build
+ pcl::gpu::Octree octree_device;
+ octree_device.setCloud(cloud_device);
+ octree_device.build();
+
+ //upload queries
+ pcl::gpu::Octree::Queries queries_device;
+ pcl::gpu::Octree::Radiuses radiuses_device;
+ queries_device.upload(data.queries);
+ radiuses_device.upload(data.radiuses);
+
+ //prepare output buffers on device
+
+ pcl::gpu::NeighborIndices result_device1(queries_device.size(), max_answers);
+ pcl::gpu::NeighborIndices result_device2(queries_device.size(), max_answers);
+ pcl::gpu::NeighborIndices result_device3(data.indices.size(), max_answers);
+
+ //prepare output buffers on host
+ vector< vector<int> > host_search1(data.tests_num);
+ vector< vector<int> > host_search2(data.tests_num);
+ for(size_t i = 0; i < data.tests_num; ++i)
+ {
+ host_search1[i].reserve(max_answers);
+ host_search2[i].reserve(max_answers);
+ }
+
+ //search GPU shared
+ octree_device.radiusSearch(queries_device, data.shared_radius, max_answers, result_device1);
+
+ //search GPU individual
+ octree_device.radiusSearch(queries_device, radiuses_device, max_answers, result_device2);
+
+ //search GPU shared with indices
+ pcl::gpu::Octree::Indices indices;
+ indices.upload(data.indices);
+ octree_device.radiusSearch(queries_device, indices, data.shared_radius, max_answers, result_device3);
+
+ //search CPU
+ octree_device.internalDownload();
+ for(size_t i = 0; i < data.tests_num; ++i)
+ {
+ octree_device.radiusSearchHost(data.queries[i], data.shared_radius, host_search1[i], max_answers);
+ octree_device.radiusSearchHost(data.queries[i], data.radiuses[i], host_search2[i], max_answers);
+ }
+
+ //download results
+ vector<int> sizes1;
+ vector<int> sizes2;
+ vector<int> sizes3;
+ result_device1.sizes.download(sizes1);
+ result_device2.sizes.download(sizes2);
+ result_device3.sizes.download(sizes3);
+
+ vector<int> downloaded_buffer1, downloaded_buffer2, downloaded_buffer3, results_batch;
+ result_device1.data.download(downloaded_buffer1);
+ result_device2.data.download(downloaded_buffer2);
+ result_device3.data.download(downloaded_buffer3);
+
+ //data.bruteForceSearch();
+
+ //verify results
+ for(size_t i = 0; i < data.tests_num; ++i)
+ {
+ vector<int>& results_host = host_search1[i];
+
+ int beg = i * max_answers;
+ int end = beg + sizes1[i];
+
+ results_batch.assign(downloaded_buffer1.begin() + beg, downloaded_buffer1.begin() + end);
+
+ std::sort(results_batch.begin(), results_batch.end());
+ std::sort(results_host.begin(), results_host.end());
+
+ if ((int)results_batch.size() == max_answers && results_batch.size() < results_host.size() && max_answers)
+ results_host.resize(max_answers);
+
+ ASSERT_EQ ( ( results_batch == results_host ), true );
+
+ //vector<int>& results_bf = data.bfresutls[i];
+ //ASSERT_EQ ( ( results_bf == results_batch), true );
+ //ASSERT_EQ ( ( results_bf == results_host ), true );
+ }
+
+ float avg_size1 = std::accumulate(sizes1.begin(), sizes1.end(), 0) * (1.f/sizes1.size());
+
+ cout << "avg_result_size1 = " << avg_size1 << endl;
+ ASSERT_GT(avg_size1, 5);
+
+
+ //verify results
+ for(size_t i = 0; i < data.tests_num; ++i)
+ {
+ vector<int>& results_host = host_search2[i];
+
+ int beg = i * max_answers;
+ int end = beg + sizes2[i];
+
+ results_batch.assign(downloaded_buffer2.begin() + beg, downloaded_buffer2.begin() + end);
+
+ std::sort(results_batch.begin(), results_batch.end());
+ std::sort(results_host.begin(), results_host.end());
+
+ if ((int)results_batch.size() == max_answers && results_batch.size() < results_host.size() && max_answers)
+ results_host.resize(max_answers);
+
+ ASSERT_EQ ( ( results_batch == results_host ), true );
+
+ //vector<int>& results_bf = data.bfresutls[i];
+ //ASSERT_EQ ( ( results_bf == results_batch), true );
+ //ASSERT_EQ ( ( results_bf == results_host ), true );
+ }
+
+ float avg_size2 = std::accumulate(sizes2.begin(), sizes2.end(), 0) * (1.f/sizes2.size());
+
+ cout << "avg_result_size2 = " << avg_size2 << endl;
+ ASSERT_GT(avg_size2, 5);
+
+
+ //verify results
+ for(size_t i = 0; i < data.tests_num; i+=2)
+ {
+ vector<int>& results_host = host_search1[i];
+
+ int beg = i/2 * max_answers;
+ int end = beg + sizes3[i/2];
+
+ results_batch.assign(downloaded_buffer3.begin() + beg, downloaded_buffer3.begin() + end);
+
+ std::sort(results_batch.begin(), results_batch.end());
+ std::sort(results_host.begin(), results_host.end());
+
+ if ((int)results_batch.size() == max_answers && results_batch.size() < results_host.size() && max_answers)
+ results_host.resize(max_answers);
+
+ ASSERT_EQ ( ( results_batch == results_host ), true );
+
+ //vector<int>& results_bf = data.bfresutls[i];
+ //ASSERT_EQ ( ( results_bf == results_batch), true );
+ //ASSERT_EQ ( ( results_bf == results_host ), true );
+ }
+
+ float avg_size3 = std::accumulate(sizes3.begin(), sizes3.end(), 0) * (1.f/sizes3.size());
+
+ cout << "avg_result_size3 = " << avg_size3 << endl;
+ ASSERT_GT(avg_size3, 5);
+}
+
--- /dev/null
+set(SUBSYS_NAME gpu_people)
+set(SUBSYS_PATH gpu/people)
+set(SUBSYS_DESC "Point cloud people library")
+set(SUBSYS_DEPS common features filters geometry gpu_containers gpu_utils io kdtree octree search segmentation surface visualization)
+
+set(build FALSE)
+PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} OFF)
+PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS})
+mark_as_advanced("BUILD_${SUBSYS_NAME}")
+
+PCL_ADD_DOC("${SUBSYS_NAME}")
+
+if(build)
+ REMOVE_VTK_DEFINITIONS()
+ #find_package(OpenCV QUIET)
+
+ #find NPP
+ unset(CUDA_npp_LIBRARY CACHE)
+ if(${CUDA_VERSION} VERSION_LESS "5.5")
+ find_cuda_helper_libs(npp)
+ else()
+ find_cuda_helper_libs(nppc)
+ find_cuda_helper_libs(nppi)
+ find_cuda_helper_libs(npps)
+
+ set(CUDA_npp_LIBRARY ${CUDA_nppc_LIBRARY} ${CUDA_nppi_LIBRARY} ${CUDA_npps_LIBRARY} CACHE STRING "npp library")
+ endif()
+
+ #Label_skeleton
+
+ FILE(GLOB srcs src/*.cpp src/*.h*)
+ FILE(GLOB srcs_cuda src/cuda/*.cu src/cuda/*.h src/cuda/nvidia/*.cu src/cuda/nvidia/*.hpp)
+ #FILE(GLOB srcs_cuda src/cuda/*.cu src/cuda/*.h src/cuda/nvidia/*.cu src/cuda/nvidia/*.hpp)
+ FILE(GLOB hdrs include/pcl/gpu/people/*.h*)
+ FILE(GLOB hdrs_cuda src/cuda/nvidia/*.h*)
+
+ source_group("Source files\\cuda" FILES ${srcs_cuda} )
+ source_group("Source files" FILES ${srcs} )
+
+ include_directories(${VTK_INCLUDE_DIRS}
+ "${CMAKE_CURRENT_SOURCE_DIR}/include" "${CMAKE_CURRENT_SOURCE_DIR}/src" "${CMAKE_CURRENT_SOURCE_DIR}/src/cuda" "${CMAKE_CURRENT_SOURCE_DIR}/src/cuda/nvidia")
+ #include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include" "${CMAKE_CURRENT_SOURCE_DIR}/src" "${CMAKE_CURRENT_SOURCE_DIR}/src/cuda")
+ include_directories(${OpenCV_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS})
+
+ if (UNIX OR APPLE)
+ set (CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "-Xcompiler;-fPIC;")
+ endif()
+ if(NOT UNIX OR APPLE)
+ add_definitions(-DPCLAPI_EXPORTS)
+ endif()
+
+ CUDA_COMPILE(objs_cuda ${srcs_cuda} ${hdrs})
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+
+ PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${hdrs} ${srcs_cuda} ${objs_cuda})
+ target_link_libraries("${LIB_NAME}" pcl_common pcl_io pcl_search pcl_surface pcl_segmentation pcl_features pcl_sample_consensus pcl_gpu_utils pcl_gpu_containers "${CUDA_CUDART_LIBRARY}" ${CUDA_npp_LIBRARY})
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "")
+
+ # install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}" ${hdrs} ${hdrs_cuda})
+
+ add_subdirectory(tools)
+endif(build)
--- /dev/null
+Please download the trees from https://github.com/PointCloudLibrary/data/tree/master/people/results into this directory.
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * @author: Koen Buys, Anatoly Baksheev
+ */
+
+
+#ifndef PCL_GPU_PEOPLE_RDF_BODYPARTS_DETECTOR_H
+#define PCL_GPU_PEOPLE_RDF_BODYPARTS_DETECTOR_H
+
+#include <pcl/pcl_exports.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/people/colormap.h>
+#include <pcl/gpu/people/label_blob2.h>
+#include <pcl/gpu/people/label_common.h>
+#include "pcl/gpu/people/person_attribs.h"
+#include <boost/shared_ptr.hpp>
+#include <string>
+#include <vector>
+
+namespace pcl
+{
+ namespace device
+ {
+ // This just seems hacky ...
+ class MultiTreeLiveProc;
+ }
+
+ namespace gpu
+ {
+ namespace people
+ {
+ class PCL_EXPORTS RDFBodyPartsDetector
+ {
+ public:
+ typedef boost::shared_ptr<RDFBodyPartsDetector> Ptr;
+ typedef std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > > BlobMatrix;
+
+ typedef DeviceArray2D<unsigned char> Labels;
+ typedef DeviceArray2D<unsigned short> Depth;
+ typedef DeviceArray2D<pcl::RGB> Image;
+
+ /** \brief This is the constructor **/
+ RDFBodyPartsDetector(const std::vector<std::string>& tree_files,
+ int default_buffer_rows = 480, int default_buffer_cols = 640);
+
+ /**
+ * \brief This function does the complete RDF evaluation in a discrete manner and builds the blob matrix
+ */
+ void process(const Depth& depth, const PointCloud<PointXYZ>& cloud, int min_pts_per_cluster);
+ // This are the different sub-parts of process()
+ /**
+ * \brief This function processes based on the RDF with probabilistic voting scheme
+ */
+ void processProb (const Depth& depth);
+
+ /**
+ * \brief This smooths the labels and does the connected components
+ */
+ void processSmooth (const Depth& depth, const PointCloud<PointXYZ>& cloud, int min_pts_per_cluster);
+
+ /**
+ * \brief This processes the blob_matrix_
+ * \return negative if failed
+ */
+ int
+ processRelations ();
+
+ /**
+ * \brief This processes the blob_matrix_
+ * \param[in] person_attribs the custom person parameters
+ * \return negative if failed
+ */
+ int
+ processRelations (PersonAttribs::Ptr person_attribs);
+
+ //getters
+ const Labels& getLabels() const;
+ const pcl::device::LabelProbability& getProbability() const;
+ const pcl::device::LabelProbability& getProbability1() const;
+ const pcl::device::LabelProbability& getProbability2() const;
+ const pcl::device::LabelProbability& getPrevProbability1() const;
+ const pcl::device::LabelProbability& getPrevProbability2() const;
+ size_t getNumberTrees() const;
+ const BlobMatrix& getBlobMatrix() const;
+
+
+ /** \brief This contains the final body part labels **/
+ Labels labels_;
+ /** \brief This contains the smoothed final body part labels **/
+ Labels labels_smoothed_;
+
+ /** These contain the histograms of the labels for this detector **/
+ pcl::device::LabelProbability P_l_; // the one is current worked in
+ pcl::device::LabelProbability P_l_Gaus_; // the current Gaussian buffer
+ pcl::device::LabelProbability P_l_Gaus_Temp_; // the current Gaussian buffer to store the intermediate
+ pcl::device::LabelProbability P_l_1_; // storage for the first iteration
+ pcl::device::LabelProbability P_l_2_; // storage for the second iteration
+
+ /** These contain the histograms of the labels for this detector of the previous timestep **/
+ pcl::device::LabelProbability P_l_prev_1_; // for the first iteration
+ pcl::device::LabelProbability P_l_prev_2_; // for the second iteration
+
+ private:
+ boost::shared_ptr<device::MultiTreeLiveProc> impl_;
+
+ int max_cluster_size_;
+ float cluster_tolerance_;
+
+ BlobMatrix blob_matrix_;
+
+ // hide this buffer using pImpl
+ std::vector<unsigned char> lmap_host_;
+ std::vector<int> dst_labels_;
+ std::vector<int> region_sizes_;
+ std::vector<int> remap_;
+ std::vector<float> means_storage_;
+ DeviceArray2D<int> comps_;
+ DeviceArray2D<unsigned char> edges_;
+
+ void allocate_buffers(int rows = 480, int cols = 640);
+ };
+ }
+ }
+}
+
+#endif /* PCL_GPU_PEOPLE_RDF_BODYPARTS_DETECTOR_H */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * @author: Koen Buys, Anatoly Baksheev
+ */
+
+#ifndef PCL_GPU_PEOPLE_COLORMAP_H_
+#define PCL_GPU_PEOPLE_COLORMAP_H_
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/gpu/people/tree.h>
+#include <pcl/gpu/containers/device_array.h>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace people
+ {
+
+ /** @brief gives a label and returns the color out of the colormap */
+ pcl::RGB getLColor(unsigned char l);
+
+ /** @brief gives a label and returns the color out of the colormap */
+ pcl::RGB getLColor (pcl::Label l);
+
+ void colorLMap(int W, int H, const trees::Label* l, unsigned char* c);
+ void colorLMap(const PointCloud<pcl::Label>& cloud_in, PointCloud<pcl::RGB>& colormap_out);
+
+
+ extern const unsigned char LUT_COLOR_LABEL[];
+ extern const int LUT_COLOR_LABEL_LENGTH;
+
+ PCL_EXPORTS void uploadColorMap(DeviceArray<pcl::RGB>& color_map);
+ PCL_EXPORTS void colorizeLabels(const DeviceArray<pcl::RGB>& color_map, const DeviceArray2D<unsigned char>& labels, DeviceArray2D<pcl::RGB>& color_labels);
+
+
+
+ PCL_EXPORTS void colorizeMixedLabels(const DeviceArray<RGB>& color_map, const DeviceArray2D<unsigned char>& labels,
+ const DeviceArray2D<RGB>& image, DeviceArray2D<RGB>& color_labels);
+
+
+ inline void colorFG ( int W, int H, const unsigned char* labels, unsigned char* c )
+ {
+ int numPix = W*H;
+ for(int pi = 0; pi < numPix; ++pi)
+ if(labels[pi] !=0 )
+ {
+ c[3*pi+0] = 0xFF;
+ c[3*pi+1] = 0x00;
+ c[3*pi+2] = 0x00;
+ }
+ }
+
+
+
+ } // end namespace people
+ } // end namespace gpu
+} // end namespace pcl
+#endif //PCL_GPU_PEOPLE_COLORMAP_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * @author: Koen Buys
+ */
+
+#ifndef PCL_GPU_PEOPLE_FACE_DETECTOR_H_
+#define PCL_GPU_PEOPLE_FACE_DETECTOR_H_
+
+#include <pcl/pcl_exports.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+#include <boost/shared_ptr.hpp>
+#include <string>
+#include <vector>
+
+#include <cuda_runtime_api.h>
+
+#include "NCVHaarObjectDetection.hpp"
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace people
+ {
+ class FaceDetector
+ {
+ public:
+ typedef boost::shared_ptr<FaceDetector> Ptr;
+ //typedef DeviceArray2D<unsigned char> Labels;
+ //typedef DeviceArray2D<unsigned short> Depth;
+ //typedef DeviceArray2D<pcl::RGB> Image;
+
+ /** \brief This is the constructor **/
+ FaceDetector ( int cols, int rows);
+
+ NCVStatus
+ loadFromXML2(const std::string &filename,
+ HaarClassifierCascadeDescriptor &haar,
+ std::vector<HaarStage64> &haar_stages,
+ std::vector<HaarClassifierNode128> &haarClassifierNodes,
+ std::vector<HaarFeature64> &haar_features);
+
+ static NCVStatus
+ loadFromNVBIN(const std::string &filename,
+ HaarClassifierCascadeDescriptor &haar,
+ std::vector<HaarStage64> &haar_stages,
+ std::vector<HaarClassifierNode128> &haarClassifierNodes,
+ std::vector<HaarFeature64> &haar_features);
+
+ NCVStatus
+ ncvHaarLoadFromFile_host(const std::string &filename,
+ HaarClassifierCascadeDescriptor &haar,
+ NCVVector<HaarStage64> &h_haar_stages,
+ NCVVector<HaarClassifierNode128> &h_haar_nodes,
+ NCVVector<HaarFeature64> &h_haar_features);
+
+ NCVStatus
+ ncvHaarGetClassifierSize(const std::string &filename, Ncv32u &numStages,
+ Ncv32u &numNodes, Ncv32u &numFeatures);
+
+ NCVStatus
+ NCVprocess(pcl::PointCloud<pcl::RGB>& cloud_in,
+ pcl::PointCloud<pcl::Intensity32u>& cloud_out,
+ HaarClassifierCascadeDescriptor &haar,
+ NCVVector<HaarStage64> &d_haar_stages,
+ NCVVector<HaarClassifierNode128> &d_haar_nodes,
+ NCVVector<HaarFeature64> &d_haar_features,
+ NCVVector<HaarStage64> &h_haar_stages,
+ INCVMemAllocator &gpu_allocator,
+ INCVMemAllocator &cpu_allocator,
+ cudaDeviceProp &device_properties,
+ Ncv32u width=640,
+ Ncv32u height=480,
+ NcvBool bFilterRects=false,
+ NcvBool bLargestFace=true);
+
+ int
+ configure (std::string cascade_file_name);
+
+ /** \brief Process step, this wraps the Nvidia code **/
+ void process (pcl::PointCloud<pcl::RGB>& cloud,
+ pcl::PointCloud<pcl::Intensity32u>& cloud_out);
+
+ /** \brief largest object sets return configuration **/
+ inline void setLargestObject (bool largest_object)
+ {
+ largest_object_ = largest_object;
+ }
+
+ inline bool getLargestObject () const
+ {
+ return largest_object_;
+ }
+
+ /** \brief Set the cuda GPU to use **/
+ void setDeviceId (int id);
+
+ int getCols () const
+ {
+ return cols_;
+ }
+
+ void setCols (int cols)
+ {
+ cols_ = cols;
+ }
+
+ int getRows () const
+ {
+ return rows_;
+ }
+
+ void setRows (int rows)
+ {
+ rows_ = rows;
+ }
+
+ std::string
+ getCascadeFileName () const
+ {
+ return cascade_file_name_;
+ }
+
+ void
+ setCascadeFileName (std::string cascadeFileName)
+ {
+ cascade_file_name_ = cascadeFileName;
+ }
+
+ /** \brief Get the cuda GPU device id in use **/
+ int
+ getDeviceId() {return cuda_dev_id_;}
+
+ private:
+ bool largest_object_; /** \brief only give back largest object **/
+ bool filter_rects_; /** \brief rectangular filter **/
+
+ int cuda_dev_id_; /** \brief indicates which GPU to use for this **/
+ cudaDeviceProp cuda_dev_prop_;
+
+ std::string cascade_file_name_;
+
+ int rows_; // should default to 480
+ int cols_; // should default to 640
+
+ HaarClassifierCascadeDescriptor haar_clas_casc_descr_;
+ NCVVectorAlloc<HaarStage64>* haar_stages_dev_;
+ NCVVectorAlloc<HaarStage64>* haar_stages_host_;
+ NCVVectorAlloc<HaarClassifierNode128>* haar_nodes_dev_;
+ NCVVectorAlloc<HaarClassifierNode128>* haar_nodes_host_;
+ NCVVectorAlloc<HaarFeature64>* haar_features_dev_;
+ NCVVectorAlloc<HaarFeature64>* haar_features_host_;
+
+ INCVMemAllocator* gpu_allocator_;
+ INCVMemAllocator* cpu_allocator_;
+
+ NCVMemStackAllocator* gpu_stack_allocator_;
+ NCVMemStackAllocator* cpu_stack_allocator_;
+
+ NCVMemStackAllocator* gpu_counter_;
+ NCVMemStackAllocator* cpu_counter_;
+
+ };
+ }
+ }
+}
+
+
+#endif /* PCL_GPU_PEOPLE_FACE_DETECTOR_H_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * @author: Koen Buys
+ * @file blob2.h
+ * @brief This file contains the Blob2 structure and the inline <<-operator for it
+ */
+
+#ifndef PCL_GPU_PEOPLE_LABEL_BLOB2_H_
+#define PCL_GPU_PEOPLE_LABEL_BLOB2_H_
+
+#include <pcl/PointIndices.h>
+#include <pcl/gpu/people/label_common.h>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace people
+ {
+ /**
+ * @brief This structure containts all parameters to describe blobs and their parent/child relations
+ * @todo: clean this out in the end, perhaps place the children in a separate struct
+ */
+ struct Blob2
+ {
+ int id; // specific identification number of this blob
+ part_t label; // labels which part this blob is, defined in common.
+ int lid; // label id, which number of this type of part is this
+
+ Eigen::Vector4f mean; // mean in xyz
+ Eigen::Matrix3f cov; // covariance in 3x3 matrix
+ Eigen::Vector3f eigenval; // eigenvalue of blob
+ Eigen::Matrix3f eigenvect; // eigenvector of blob
+
+ //These variables are added in order to be able to build trees with them
+ int child_id[MAX_CHILD]; // id of the best found child
+ int child_lid[MAX_CHILD]; // lid of the best found child
+ float child_dist[MAX_CHILD]; // result of evaluation function of this child
+ char child_label[MAX_CHILD]; // makes displaying the tree easier
+
+ pcl::PointIndices indices; // The indices of the pointcloud
+ Eigen::Vector4f min; // The min of the bounding box
+ Eigen::Vector4f max; // The max of the bounding box
+ };
+
+ inline std::ostream& operator << (std::ostream& os, const Blob2& b)
+ {
+ os << " Blob2 id " << b.id << " label " << b.label << " lid " << b.lid << std::endl;
+ os << " mean " << b.mean(0) << " , " << b.mean(1) << " , " << b.mean(2) << " , " << b.mean(3) << std::endl;
+ os << " cov " << std::endl << b.cov << std::endl;
+ os << " eigenval " << b.eigenval(0) << " , " << b.eigenval(1) << " , " << b.eigenval(2) << std::endl;
+ os << " eigenvect " << std::endl << b.eigenvect << std::endl;
+ os << " min " << b.min(0) << " , " << b.min(1) << " , " << b.min(2) << " , " << b.min(3) << std::endl;
+ os << " max " << b.max(0) << " , " << b.max(1) << " , " << b.max(2) << " , " << b.max(3) << std::endl;
+ os << " indices length " << b.indices.indices.size() << std::endl;
+ for(int i = 0; i < MAX_CHILD; i++)
+ {
+ os << " child " << i << " id " << b.child_id[i] << " lid " << b.child_lid[i] << " dist " << b.child_dist[i] << " label " << b.child_label[i] << std::endl;
+ }
+ return (os);
+ }
+ } // end namespace people
+ } // end namespace gpu
+} // end namespace pcl
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * @author: Koen Buys
+ */
+
+#ifndef PCL_GPU_PEOPLE_LABEL_COMMON_H_
+#define PCL_GPU_PEOPLE_LABEL_COMMON_H_
+
+#include <pcl/gpu/containers/device_array.h>
+#include <cuda_runtime.h> // for float4, uchar4, delete this in future
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace people
+ {
+ // Some types for the tree
+ enum
+ {
+ NO_CHILD = -3,
+ LEAF = -2,
+ ROOT = -1,
+ NO_DATA = 255 /** \brief We have no depth data for this part of the image **/
+ };
+
+ enum { NUM_PARTS = 25 }; /** \brief We have 25 body parts defined **/
+ enum { MAX_CHILD = 4 }; /** \brief a parent node has maximum 4 children **/
+ enum { NR_TREES = 4 }; /** \brief The maximum supported number of trees **/
+
+ // Some defines for geometry
+ enum { FOCAL = 525 }; /** \brief Focal length of rgb camera in pixels **/
+ enum { WIDTH = 640 };
+ enum { HEIGHT = 480 };
+ enum { RATIO = WIDTH/HEIGHT };
+ enum { XML_VERSION = 1}; /** \brief This indicates the current used xml file version (for people lib only) **/
+
+ enum { NUM_ATTRIBS = 2000 };
+ enum { NUM_LABELS = 32 }; /** \brief Our code is forseen to use maximal use 32 labels **/
+
+ /** @todo implement label 25 to 29 **/
+ enum part_t
+ {
+ Lfoot = 0,
+ Lleg = 1,
+ Lknee = 2,
+ Lthigh = 3,
+ Rfoot = 4,
+ Rleg = 5,
+ Rknee = 6,
+ Rthigh = 7,
+ Rhips = 8,
+ Lhips = 9,
+ Neck = 10,
+ Rarm = 11,
+ Relbow = 12,
+ Rforearm = 13,
+ Rhand = 14,
+ Larm = 15,
+ Lelbow = 16,
+ Lforearm = 17,
+ Lhand = 18,
+ FaceLB = 19,
+ FaceRB = 20,
+ FaceLT = 21,
+ FaceRT = 22,
+ Rchest = 23,
+ Lchest = 24,
+ Lshoulder = 25,
+ Rshoulder = 26,
+ Groundplane = 27,
+ Ceiling = 28,
+ Background = 29,
+ Plane = 30,
+ NOLABEL = 31
+ };
+
+ typedef DeviceArray2D<float4> Cloud;
+ typedef DeviceArray2D<uchar4> Image;
+
+ typedef DeviceArray2D<unsigned short> Depth;
+ typedef DeviceArray2D<unsigned char> Labels;
+ typedef DeviceArray2D<float> HueImage;
+ typedef DeviceArray2D<unsigned char> Mask;
+
+ /**
+ * @brief This LUT contains the max primary eigenvalue for each part
+ * @todo read this from XML file
+ **/
+ static const float LUT_max_part_size[] =
+ {
+ 0.5f, // 0 Lfoot
+ 0.7f, // 1 Lleg
+ 0.6f, // 2 Lknee
+ 0.6f, // 3 Lthigh
+ 0.5f, // 4 Rfoot
+ 0.7f, // 5 Rleg
+ 0.6f, // 6 Rknee
+ 0.6f, // 7 Rthigh
+ 0.9f, // 8 Rhips
+ 0.9f, // 9 Lhips
+ 0.5f, // 10 Neck
+ 0.7f, // 11 Rarm
+ 0.5f, // 12 Relbow
+ 0.7f, // 13 Rforearm
+ 0.5f, // 14 Rhand
+ 0.7f, // 15 Larm
+ 0.5f, // 16 Lelbow
+ 0.7f, // 17 Lforearm
+ 0.5f, // 18 Lhand
+ 0.5f, // 19 FaceLB
+ 0.5f, // 20 FaceRB
+ 0.5f, // 21 FaceLT
+ 0.5f, // 22 FaceRT
+ 0.9f, // 23 Rchest
+ 0.9f // 24 Lchest
+ };
+
+ /**
+ * @brief This LUT contains the ideal lenght between this part and his children
+ **/
+ static const float LUT_ideal_length[][4] =
+ {
+ { -1.0f, -1.0f, -1.0f, -1.0f}, // 0 Lfoot
+ { 0.2f, -1.0f, -1.0f, -1.0f}, // 1 Lleg
+ { 0.2f, -1.0f, -1.0f, -1.0f}, // 2 Lknee
+ { 0.3f, -1.0f, -1.0f, -1.0f}, // 3 Lthigh
+ { -1.0f, -1.0f, -1.0f, -1.0f}, // 4 Rfoot
+ { 0.2f, -1.0f, -1.0f, -1.0f}, // 5 Rleg
+ { 0.2f, -1.0f, -1.0f, -1.0f}, // 6 Rknee
+ { 0.3f, -1.0f, -1.0f, -1.0f}, // 7 Rthigh
+ { 0.3f, -1.0f, -1.0f, -1.0f}, // 8 Rhips
+ { 0.3f, -1.0f, -1.0f, -1.0f}, // 9 Lhips
+ { 0.15f, 0.15f, 0.2f, 0.2f}, // 10 Neck
+ { 0.15f, -1.0f, -1.0f, -1.0f}, // 11 Rarm
+ { 0.1f, -1.0f, -1.0f, -1.0f}, // 12 Relbow
+ { 0.15f, -1.0f, -1.0f, -1.0f}, // 13 Rforearm
+ { -1.0f, -1.0f, -1.0f, -1.0f}, // 14 Rhand
+ { 0.15f, -1.0f, -1.0f, -1.0f}, // 15 Larm
+ { 0.1f, -1.0f, -1.0f, -1.0f}, // 16 Lelbow
+ { 0.15f, -1.0f, -1.0f, -1.0f}, // 17 Lforearm
+ { -1.0f, -1.0f, -1.0f, -1.0f}, // 18 Lhand
+ { 0.15f, -1.0f, -1.0f, -1.0f}, // 19 FaceLB
+ { 0.15f, -1.0f, -1.0f, -1.0f}, // 20 FaceRB
+ { -1.0f, -1.0f, -1.0f, -1.0f}, // 21 FaceLT
+ { -1.0f, -1.0f, -1.0f, -1.0f}, // 22 FaceRT
+ { 0.3f, 0.3f, -1.0f, -1.0f}, // 23 Rchest
+ { 0.3f, 0.3f, -1.0f, -1.0f} // 24 Lchest
+ };
+
+ /**
+ * @brief This LUT contains the max lenght between this part and his children
+ **/
+ static const float LUT_max_length_offset[][4] =
+ {
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 0 Lfoot
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 1 Lleg
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 2 Lknee
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 3 Lthigh
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 4 Rfoot
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 5 Rleg
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 6 Rknee
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 7 Rthigh
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 8 Rhips
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 9 Lhips
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 10 Neck
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 11 Rarm
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 12 Relbow
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 13 Rforearm
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 14 Rhand
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 15 Larm
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 16 Lelbow
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 17 Lforearm
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 18 Lhand
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 19 FaceLB
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 20 FaceRB
+ { 0.3f, 0.15f, 0.15f, 0.15f}, // 21 FaceLT
+ { 0.3f, 0.15f, 0.15f, 0.15f}, // 22 FaceRT
+ { 0.15f, 0.15f, 0.15f, 0.15f}, // 23 Rchest
+ { 0.15f, 0.15f, 0.15f, 0.15f} // 24 Lchest
+ };
+
+ /**
+ * @brief This LUT contains the number of children for each parent
+ **/
+ static const unsigned int LUT_nr_children[] =
+ {
+ 0, // 0 Lfoot
+ 1, // 1 Lleg
+ 1, // 2 Lknee
+ 1, // 3 Lthigh
+ 0, // 4 Rfoot
+ 1, // 5 Rleg
+ 1, // 6 Rknee
+ 1, // 7 Rthigh
+ 1, // 8 Rhips
+ 1, // 9 Lhips
+ 4, // 10 Neck
+ 1, // 11 Rarm
+ 1, // 12 Relbow
+ 1, // 13 Rforearm
+ 0, // 14 Rhand
+ 1, // 15 Larm
+ 1, // 16 Lelbow
+ 1, // 17 Lforearm
+ 0, // 18 Lhand
+ 1, // 19 FaceLB
+ 1, // 20 FaceRB
+ 0, // 21 FaceLT
+ 0, // 22 FaceRT
+ 2, // 23 Rchest
+ 2 // 24 Lchest
+ };
+ } // End namespace people
+ } // End namespace gpu
+} // End namespace pcl
+
+
+// All typedefs to be used in device name space:
+// Moved to common because of multiple declarations conflicting
+// TODO solve this for Image, also declared as pcl::RGB
+namespace pcl
+{
+ namespace device
+ {
+ struct prob_histogram
+ {
+ float probs[pcl::gpu::people::NUM_LABELS]; /** \brief A single float probability for each body part **/
+ };
+
+ typedef DeviceArray2D<prob_histogram> LabelProbability;
+
+ }
+}
+
+/** @TODO get this to work:
+std::string part_k[NUM_PARTS] = {"Lfoot","Lleg", "Lknee","Lthigh",
+ "Rfoot","Rleg","Rknee","Rthigh",
+ "Rhips","Lhips","Neck",
+ "Rarm","Relbow","Rforearm","Rhand",
+ "Larm","Lelbow","Lforearm","Lhand",
+ "FaceLB","FaceRB","FaceLT","FaceRT",
+ "Rchest","Lchest"};
+
+inline std::ostream& operator << (std::ostream& os, const part_t& p)
+{
+ os << part_k[(int) p];
+ return (os);
+}
+ **/
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * @author Koen Buys
+ * @file segment.h
+ * @brief This file contains the function prototypes for the segmentation functions
+ */
+
+#ifndef PCL_GPU_PEOPLE_LABEL_SEGMENT_H_
+#define PCL_GPU_PEOPLE_LABEL_SEGMENT_H_
+
+// our headers
+#include "pcl/gpu/people/label_blob2.h"
+#include "pcl/gpu/people/label_common.h"
+
+// std
+#include <vector>
+
+// opencv drawing stuff
+//#include <opencv2/highgui/highgui.hpp>
+
+// PCL specific includes
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/conversions.h>
+#include <pcl/common/eigen.h>
+#include <pcl/common/common.h>
+#include <pcl/common/centroid.h>
+#include <pcl/PointIndices.h>
+
+#include <pcl/common/time.h>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace people
+ {
+ namespace label_skeleton
+ {
+ /*
+ * \brief this function smooths the label image based on label and depth
+ * \param[in] lmap_in the cvMat with the labels, must be CV_8UC1
+ * \param[in] dmap the cvMat with the depths, must be CV_16U in mm
+ * \param[out] lmap_out the smoothed output labelmap as cvMat
+ * \param[in] patch_size make the patch size for smoothing
+ * \param[in] depthThres the z-distance thresshold
+ * \todo add a Gaussian contribution function to depth and vote
+ **/
+ //inline void smoothLabelImage ( cv::Mat& lmap_in,
+ // cv::Mat& dmap,
+ // cv::Mat& lmap_out,
+ // unsigned int patch_size,
+ // unsigned int depthThres)
+ //{
+ // // check depth
+ // assert(lmap_in.depth() == CV_8UC1);
+ // assert(dmap.depth() == CV_16U);
+ // assert(lmap_out.depth() == CV_8UC1);
+ // // check size
+ // assert(lmap_in.rows == dmap.rows);
+ // assert(lmap_in.cols == dmap.cols);
+ // assert(lmap_out.rows == dmap.rows);
+ // assert(lmap_out.cols == dmap.cols);
+
+ // unsigned int half_patch = static_cast<int> (patch_size/2);
+
+ // // iterate over the height of the image (from 2 till 478)
+ // for(unsigned int h = (0 + half_patch); h < (lmap_in.rows - half_patch); h++)
+ // {
+ // // iterate over the width of the image (from 2 till 638)
+ // for(unsigned int w = (0 + half_patch); w < (lmap_in.cols - half_patch); w++)
+ // {
+ // short depth = dmap.at<short>(h, w);
+ // unsigned int votes[NUM_PARTS];
+ // //this should be unneeded but to test
+ // for(int j = 0 ; j< NUM_PARTS; j++)
+ // votes[j] = 0;
+
+ // // iterate over the size of the patch in the height
+ // for(unsigned int h_l = (h - half_patch); h_l <= (h + half_patch); h_l++)
+ // {
+ // // iterate over the size of the patch in the width
+ // for(unsigned int w_l = (w - half_patch); w_l <= (w + half_patch); w_l++)
+ // {
+ // // get the depth of this part of the patch
+ // short depth_l = dmap.at<short>(h_l,w_l);
+ // // evaluate the difference to the centroid
+ // if(abs(depth - depth_l) < static_cast<int> (depthThres))
+ // {
+ // char label = lmap_in.at<char>(h_l,w_l);
+ // if(label >= 0 && label < NUM_PARTS)
+ // votes[static_cast<unsigned int> (label)]++;
+ // else
+ // std::cout << "(E) : smoothLabelImage(): I've read a label that is non valid" << std::endl;
+ // }
+ // }
+ // }
+
+ // unsigned int max = 0;
+ // char label = lmap_in.at<char>(h,w);
+
+ // // iterate over the bin to find the max
+ // for(char i=0; i<NUM_PARTS; i++)
+ // {
+ // if(votes[static_cast<int> (i)] > max)
+ // {
+ // max = votes[static_cast<int> (i)];
+ // label = i;
+ // }
+ // }
+ // lmap_out.at<char>(h,w) = label;
+ // }
+ // }
+ //}
+
+ /*
+ * \brief this function smooths the label image based on label and depth
+ * \param[in] lmap_in the cvMat with the labels, must be CV_8UC1
+ * \param[in] dmap the cvMat with the depths, must be CV_16U in mm
+ * \param[out] lmap_out the smoothed output labelmap as cvMat
+ * \todo make the patch size a parameter
+ * \todo make the z-distance a parameter
+ * \todo add a Gaussian contribution function to depth and vote
+ **/
+ //inline void smoothLabelImage2 ( cv::Mat& lmap_in,
+ // cv::Mat& dmap,
+ // cv::Mat& lmap_out)
+ //{
+ // // check depth
+ // assert(lmap_in.depth() == CV_8UC1);
+ // assert(dmap.depth() == CV_16U);
+ // assert(lmap_out.depth() == CV_8UC1);
+ // // check size
+ // assert(lmap_in.rows == dmap.rows);
+ // assert(lmap_in.cols == dmap.cols);
+ // assert(lmap_out.rows == dmap.rows);
+ // assert(lmap_out.cols == dmap.cols);
+
+ // //unsigned int patch_size = 5;
+ // unsigned int half_patch = 2;
+ // unsigned int depthThres = 300; // Think this is in mm, verify this!!!!!
+
+ // // iterate over the height of the image (from 2 till 478)
+ // unsigned int endrow = (lmap_in.rows - half_patch);
+ // unsigned int endcol = (lmap_in.cols - half_patch);
+ // for(unsigned int h = (0 + half_patch); h < endrow; h++)
+ // {
+ // unsigned int endheight = (h + half_patch);
+
+ // // iterate over the width of the image (from 2 till 638)
+ // for(unsigned int w = (0 + half_patch); w < endcol; w++)
+ // {
+ // unsigned int endwidth = (w + half_patch);
+
+ // short depth = dmap.at<short>(h, w);
+ // unsigned int votes[NUM_PARTS];
+ // //this should be unneeded but to test
+ // for(int j = 0 ; j< NUM_PARTS; j++)
+ // votes[j] = 0;
+
+ // // iterate over the size of the patch in the height
+ // for(unsigned int h_l = (h - half_patch); h_l <= endheight; h_l++)
+ // {
+ // // iterate over the size of the patch in the width
+ // for(unsigned int w_l = (w - half_patch); w_l <= endwidth; w_l++)
+ // {
+ // // get the depth of this part of the patch
+ // short depth_l = dmap.at<short>(h_l,w_l);
+ // // evaluate the difference to the centroid
+ // if(abs(depth - depth_l) < static_cast<int> (depthThres))
+ // {
+ // char label = lmap_in.at<char>(h_l,w_l);
+ // if(label >= 0 && label < NUM_PARTS)
+ // votes[static_cast<unsigned int>(label)]++;
+ // else
+ // std::cout << "(E) : smoothLabelImage(): I've read a label that is non valid" << std::endl;
+ // }
+ // }
+ // }
+
+ // unsigned int max = 0;
+ // char label = lmap_in.at<char>(h,w);
+
+ // // iterate over the bin to find the max
+ // for(char i=0; i<NUM_PARTS; i++)
+ // {
+ // if(votes[static_cast<unsigned int>(i)] > max)
+ // {
+ // max = votes[static_cast<unsigned int>(i)];
+ // label = i;
+ // }
+ // }
+ // lmap_out.at<char>(h,w) = label;
+ // }
+ // }
+ //}
+
+ /**
+ * @brief this function smooths the label image based on label and depth
+ * @param[in] lmap_in the cvMat with the labels, must be CV_8UC1
+ * @param[in] dmap the cvMat with the depths, must be CV_16U in mm
+ * @param[out] lmap_out the smoothed output labelmap as cvMat
+ * @todo make the patch size a parameter
+ * @todo make the z-distance a parameter
+ * @todo add a Gaussian contribution function to depth and vote
+ **/
+ inline void smoothLabelImage ( cv::Mat& lmap_in,
+ cv::Mat& dmap,
+ cv::Mat& lmap_out)
+ {
+ // check depth
+ assert(lmap_in.depth() == CV_8UC1);
+ assert(dmap.depth() == CV_16U);
+ assert(lmap_out.depth() == CV_8UC1);
+ // check size
+ assert(lmap_in.rows == dmap.rows);
+ assert(lmap_in.cols == dmap.cols);
+ assert(lmap_out.rows == dmap.rows);
+ assert(lmap_out.cols == dmap.cols);
+
+ //unsigned int patch_size = 5;
+ unsigned int half_patch = 2;
+ unsigned int depthThres = 300; // Think this is in mm, verify this!!!!!
+
+ // iterate over the height of the image (from 2 till 478)
+ unsigned int endrow = (lmap_in.rows - half_patch);
+ unsigned int endcol = (lmap_in.cols - half_patch);
+ unsigned int votes[NUM_PARTS];
+ unsigned int endheight, endwidth;
+ const short* drow;
+ char *loutrow;
+ short depth;
+ const short* drow_offset;
+ const char* lrow_offset;
+ short depth_l;
+ char label;
+ for(unsigned int h = (0 + half_patch); h < endrow; h++)
+ {
+ endheight = (h + half_patch);
+
+ drow = dmap.ptr<short>(h);
+ loutrow = lmap_out.ptr<char>(h);
+
+ // iterate over the width of the image (from 2 till 638)
+ for(unsigned int w = (0 + half_patch); w < endcol; w++)
+ {
+ endwidth = (w + half_patch);
+
+ depth = drow[w];
+ // reset votes
+ for(int j = 0 ; j< NUM_PARTS; j++)
+ votes[j] = 0;
+
+ // iterate over the size of the patch in the height
+ for(unsigned int h_l = (h - half_patch); h_l <= endheight; h_l++)
+ {
+ drow_offset = dmap.ptr<short>(h_l);
+ lrow_offset = lmap_in.ptr<char>(h_l);
+
+ // iterate over the size of the patch in the width
+ for(unsigned int w_l = (w - half_patch); w_l <= endwidth; w_l++)
+ {
+ // get the depth of this part of the patch
+ depth_l = drow_offset[w_l];
+ // evaluate the difference to the centroid
+ if(abs(depth - depth_l) < static_cast<int> (depthThres))
+ {
+ label = lrow_offset[w_l];
+ votes[static_cast<unsigned int>(label)]++;
+ }
+ }
+ }
+
+ unsigned int max = 0;
+
+ // iterate over the bin to find the max
+ for(char i=0; i<NUM_PARTS; i++)
+ {
+ if(votes[static_cast<unsigned int>(i)] > max)
+ {
+ max = votes[static_cast<unsigned int>(i)];
+ loutrow[w] = i;
+ }
+ }
+ }
+ }
+ }
+
+ /**
+ * @brief This function takes a number of indices with label and sorts them in the blob matrix
+ * @param[in] cloud_in the original input pointcloud from which everything was calculated
+ * @param[in] sizeThres the minimal size needed to be considered as a valid blob
+ * @param[out] sorted the matrix in which every blob will be pushed back
+ * @param[in] indices the matrix of PointIndices
+ * @todo implement the eigenvalue evaluation again
+ * @todo do we still need sizeThres?
+ **/
+ inline void sortIndicesToBlob2 ( const pcl::PointCloud<pcl::PointXYZ>& cloud_in,
+ unsigned int sizeThres,
+ std::vector< std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
+ std::vector< std::vector<pcl::PointIndices> >& indices)
+ {
+ assert(sorted.size () == indices.size ());
+
+ unsigned int id = 0;
+ // Iterate over all labels
+ for(unsigned int lab = 0; lab < indices.size(); ++lab)
+ {
+ unsigned int lid = 0;
+ // Iterate over all blobs of this label
+ for(unsigned int l = 0; l < indices[lab].size(); l++)
+ {
+ // If the blob has enough pixels
+ if(indices[lab][l].indices.size() > sizeThres)
+ {
+ Blob2 b;
+
+ b.indices = indices[lab][l];
+
+ pcl::compute3DCentroid(cloud_in, b.indices, b.mean);
+#ifdef NEED_STATS
+ pcl::computeCovarianceMatrixNormalized(cloud_in, b.indices, b.mean, b.cov);
+ pcl::getMinMax3D(cloud_in, b.indices, b.min, b.max);
+ pcl::eigen33(b.cov, b.eigenvect, b.eigenval);
+#endif
+ // Check if it is a valid blob
+ //if((b.eigenval(0) < LUT_max_part_size[(int) lab]) && (b.mean(2) != 0))
+ if((b.mean(2) != 0))
+ {
+ b.id = id;
+ id++;
+ b.label = static_cast<part_t> (lab);
+ b.lid = lid;
+ lid++;
+ sorted[lab].push_back(b);
+ }
+ }
+ }
+ }
+ }
+
+ /**
+ * @brief This function is a stupid helper function to debug easilier, it print out how the matrix was sorted
+ * @param[in] sorted the matrix of blobs
+ * @return Zero if everything went well
+ **/
+ inline int giveSortedBlobsInfo ( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted)
+ {
+ std::cout << "(I) : giveSortedBlobsInfo(): Size of outer vector: " << sorted.size() << std::endl;
+ for(unsigned int i = 0; i < sorted.size(); i++)
+ {
+ std::cout << "(I) : giveSortedBlobsInfo(): Found " << sorted[i].size() << " parts of type " << i << std::endl;
+ std::cout << "(I) : giveSortedBlobsInfo(): indices : ";
+ for(unsigned int j = 0; j < sorted[i].size(); j++)
+ {
+ std::cout << " id:" << sorted[i][j].id << " lid:" << sorted[i][j].lid;
+ }
+ std::cout << std::endl;
+ }
+ return 0;
+ }
+ } // end namespace label_skeleton
+ } // end namespace people
+ } // end namespace gpu
+} // end namespace pcl
+
+#endif //#ifndef LABELSKEL_SEGMENT_H
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * @author Koen Buys
+ * @file tree.h
+ * @brief This file contains the function prototypes for the tree building functions.
+ */
+
+#ifndef PCL_GPU_PEOPLE_LABEL_TREE_H_
+#define PCL_GPU_PEOPLE_LABEL_TREE_H_
+
+// our headers
+#include "pcl/gpu/people/label_blob2.h" //this one defines the blob structure
+#include "pcl/gpu/people/label_common.h" //this one defines the LUT's
+#include "pcl/gpu/people/person_attribs.h"
+
+// std
+#include <vector>
+#include <iostream>
+#include <algorithm>
+#include <stdexcept>
+
+// PCL specific includes
+#include <pcl/conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/console/print.h>
+
+#include <pcl/common/eigen.h>
+#include <pcl/common/common.h>
+#include <pcl/common/centroid.h>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace people
+ {
+ /**
+ * @brief This structure containts all parameters to describe the segmented tree
+ */
+ struct Tree2
+ {
+ //Inline constructor
+ Tree2() : id(NO_CHILD), lid(NO_CHILD), nr_parts(0)
+ {
+ for(int i=0;i<NUM_PARTS;i++)
+ parts_lid[i] = NO_CHILD;
+ }
+
+ int id; // specific identification number of this tree
+ part_t label; // labels which part the root of this tree is
+ int lid; // label id, which number of this type of part is this
+ int nr_parts; // the number of parts in this tree
+ int parts_lid[NUM_PARTS]; // Indicate the used parts
+ float total_dist_error; // sum of all distance errors
+ float norm_dist_error; // total_dist_error/nr_parts
+
+ Eigen::Vector4f mean; // mean in xyz
+ Eigen::Matrix3f cov; // covariance in 3x3 matrix
+ Eigen::Vector3f eigenval; // eigenvalue of blob
+ Eigen::Matrix3f eigenvect; // eigenvector of blob
+
+ pcl::PointIndices indices; // The indices of the pointcloud
+ Eigen::Vector4f min; // The min of the bounding box
+ Eigen::Vector4f max; // The max of the bounding box
+ };
+
+ inline std::ostream& operator << (std::ostream& os, const Tree2& t)
+ {
+ os << " Tree2 id " << t.id << " label " << t.label << " lid " << t.lid << " nr_parts " << t.nr_parts << std::endl;
+ os << " total_dist_error " << t.total_dist_error << " norm_dist_error " << t.norm_dist_error << std::endl;
+ os << " mean " << t.mean(0) << " , " << t.mean(1) << " , " << t.mean(2) << " , " << t.mean(3) << std::endl;
+ os << " cov " << std::endl << t.cov << std::endl;
+ os << " eigenval " << t.eigenval(0) << " , " << t.eigenval(1) << " , " << t.eigenval(2) << std::endl;
+ os << " eigenvect " << std::endl << t.eigenvect << std::endl;
+ os << " min " << t.min(0) << " , " << t.min(1) << " , " << t.min(2) << " , " << t.min(3) << std::endl;
+ os << " max " << t.max(0) << " , " << t.max(1) << " , " << t.max(2) << " , " << t.max(3) << std::endl;
+ os << " indices length " << t.indices.indices.size() << std::endl;
+ return (os);
+ }
+
+ /**
+ * @brief This function sets the children of the leaf nodes to leaf, meaning that we came to the correct end
+ * @param[in] sorted The matrix of all blobs
+ * @param[in] label The label of which all children are to be set as leafs
+ * @return Zero if everything went well
+ **/
+ inline int
+ leafBlobVector( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
+ int label )
+ {
+ if(sorted[label].size() == 0)
+ return 0;
+ for(size_t i = 0; i < sorted[label].size(); i++)
+ {
+ for(int j = 0; j < MAX_CHILD; j++)
+ sorted[label][i].child_id[j] = LEAF;
+ }
+ return 0;
+ }
+
+ /**
+ * @brief This function sets the specific child of the vector to no child, meaning that there are no such children
+ * @param[in] sorted The matrix of all blobs
+ * @param[in] label The label of which the child must be set to NO_CHILD
+ * @param[in] child_number The index of the respective child that must be set
+ * @return Zero if everything went well
+ **/
+ inline int
+ noChildBlobVector( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
+ int label,
+ int child_number)
+ {
+ if(sorted[label].size() == 0)
+ return 0;
+ for(size_t i = 0; i < sorted[label].size(); i++){
+ sorted[label][i].child_id[child_number] = NO_CHILD;
+ }
+ return 0;
+ }
+
+ /**
+ * @brief This function test if children were found for this label
+ * @return True if this label has valid children
+ **/
+ inline bool
+ hasThisLabelChildren ( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
+ part_t label,
+ int child_number)
+ {
+ if(sorted[label].size() == 0)
+ return 0;
+ for(size_t i = 0; i < sorted[label].size(); i++)
+ if((sorted[label][i].child_id[child_number] != NO_CHILD) && (sorted[label][i].child_id[child_number] != LEAF))
+ return 1;
+ return 0;
+ }
+
+ /**
+ * @brief This is the evaluation function used to compare two blobs
+ * @param[in] parent pointer to the parent blob
+ * @param[in] child pointer to the child blob
+ * @param[in] child_nr the number of the child
+ * @return it returns the distance error from the ideal parent child distance, it returns -1.0 if it goes over threshold
+ * @todo what if child is second link in stead of first link (ea forearm in stead of elbow for arm)
+ **/
+ inline float
+ evaluateBlobs (Blob2& parent, Blob2& child, int child_nr)
+ {
+ float root = sqrt(pow(parent.mean(0) - child.mean(0), 2) +
+ pow(parent.mean(1) - child.mean(1), 2) +
+ pow(parent.mean(2) - child.mean(2), 2));
+ float offset = fabs(LUT_ideal_length[(int)parent.label][child_nr] - root);
+ if(offset > LUT_max_length_offset[(int)parent.label][child_nr])
+ return -1.0;
+ else
+ return offset;
+ }
+
+ /**
+ * @brief This is the evaluation function used to compare two blobs
+ * @param[in] parent pointer to the parent blob
+ * @param[in] child pointer to the child blob
+ * @param[in] child_nr the number of the child
+ * @param person_attribs
+ * @return it returns the distance error from the ideal parent child distance, it returns -1.0 if it goes over threshold
+ * @todo what if child is second link in stead of first link (ea forearm in stead of elbow for arm)
+ **/
+ inline float
+ evaluateBlobs (Blob2& parent,
+ Blob2& child,
+ int child_nr,
+ PersonAttribs::Ptr person_attribs)
+ {
+ float root = sqrt(pow(parent.mean(0) - child.mean(0), 2) +
+ pow(parent.mean(1) - child.mean(1), 2) +
+ pow(parent.mean(2) - child.mean(2), 2));
+ float offset = fabs(person_attribs->part_ideal_length_[(int)parent.label][child_nr] - root);
+ if(offset > person_attribs->max_length_offset_[(int)parent.label][child_nr])
+ return -1.0;
+ else
+ return offset;
+ }
+
+ /**
+ * @brief This function evaluates an entire row of parent segments for the best child segments
+ * @param[in] sorted this is the array of all blobs
+ * @param[in] parent_label this is the part label that indicates the row
+ * @param[in] child_label this is the part label that indicates the childs needed to be investigated
+ * @param[in] child_number the number of this child in the parent, some parents have multiple childs
+ * @return zero if successfull
+ * @todo once we have good evaluation function reconsider best_value
+ **/
+ inline int
+ evaluateBlobVector( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
+ unsigned int parent_label,
+ int child_label,
+ int child_number)
+ {
+ // Check the input data
+ assert(parent_label < NUM_PARTS);
+ assert(child_label >= 0);
+ assert(child_number >= 0);
+ assert(child_number < MAX_CHILD);
+
+ if(sorted[parent_label].size() == 0){
+ return 0; //if my size is 0, this is solved by my parent in his iteration
+ }
+ if(sorted[child_label].size() == 0){
+ noChildBlobVector(sorted, parent_label, child_number);
+ return 0;
+ }
+ // go over all parents in this vector
+ for(size_t p = 0; p < sorted[parent_label].size(); p++){
+ float best_value = std::numeric_limits<float>::max();
+ int best_child_id = NO_CHILD;
+ int best_child_lid = 0; // this must be as low as possible, still overruled by id
+ float value = 0.0;
+
+ // go over all children in this vector
+ for(size_t c = 0; c < sorted[child_label].size(); c++){
+ value = evaluateBlobs(sorted[parent_label][p], sorted[child_label][c], child_number);
+ // Value should contain offset from the ideal position
+ // Is -1 if it goes above threshold
+ if(value < best_value && value != -1.0){
+ best_child_id = sorted[child_label][c].id;
+ best_child_lid = c;
+ best_value = value;
+ }
+ }
+ assert(parent_label < sorted.size());
+ assert(p < sorted[parent_label].size());
+ assert(child_label < (int) sorted.size());
+ //Set the correct child in the parent
+ sorted[parent_label][p].child_id[child_number] = best_child_id;
+ sorted[parent_label][p].child_lid[child_number] = best_child_lid;
+ sorted[parent_label][p].child_dist[child_number] = best_value;
+ sorted[parent_label][p].child_label[child_number] = child_label;
+ }
+ return 0;
+ }
+
+ /**
+ * @brief This function evaluates an entire row of parent segments for the best child segments
+ * @param[in] sorted this is the array of all blobs
+ * @param[in] parent_label this is the part label that indicates the row
+ * @param[in] child_label this is the part label that indicates the childs needed to be investigated
+ * @param[in] child_number the number of this child in the parent, some parents have multiple childs
+ * @param person_attribs
+ * @return zero if successfull
+ * @todo once we have good evaluation function reconsider best_value
+ **/
+ inline int
+ evaluateBlobVector( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
+ unsigned int parent_label,
+ int child_label,
+ int child_number,
+ PersonAttribs::Ptr person_attribs)
+ {
+ // Check the input data
+ assert(parent_label < NUM_PARTS);
+ assert(child_label >= 0);
+ assert(child_number >= 0);
+ assert(child_number < MAX_CHILD);
+
+ if(sorted[parent_label].size() == 0){
+ return 0; //if my size is 0, this is solved by my parent in his iteration
+ }
+ if(sorted[child_label].size() == 0){
+ noChildBlobVector(sorted, parent_label, child_number);
+ return 0;
+ }
+ // go over all parents in this vector
+ for(size_t p = 0; p < sorted[parent_label].size(); p++){
+ float best_value = std::numeric_limits<float>::max();
+ int best_child_id = NO_CHILD;
+ int best_child_lid = 0; // this must be as low as possible, still overruled by id
+ float value = 0.0;
+
+ // go over all children in this vector
+ for(size_t c = 0; c < sorted[child_label].size(); c++){
+ value = evaluateBlobs(sorted[parent_label][p], sorted[child_label][c], child_number, person_attribs);
+ // Value should contain offset from the ideal position
+ // Is -1 if it goes above threshold
+ if(value < best_value && value != -1.0){
+ best_child_id = sorted[child_label][c].id;
+ best_child_lid = c;
+ best_value = value;
+ }
+ }
+ assert(parent_label < sorted.size());
+ assert(p < sorted[parent_label].size());
+ assert(child_label < (int) sorted.size());
+ //Set the correct child in the parent
+ sorted[parent_label][p].child_id[child_number] = best_child_id;
+ sorted[parent_label][p].child_lid[child_number] = best_child_lid;
+ sorted[parent_label][p].child_dist[child_number] = best_value;
+ sorted[parent_label][p].child_label[child_number] = child_label;
+ }
+ return 0;
+ }
+
+ /**
+ * @brief This function goes over the sorted matrix and fills in the optimal parent and child relations
+ * @param[in] sorted a matrix with all found good blobs arranged according to label and order
+ * @return zero if everything went well, negative on an error
+ * @todo This function also fixes the kinematic chain, we should implement this in a xml or LUT
+ * @todo look if we can't get a more efficient implementation (iterator together with sortBlobs perhaps?)
+ */
+ inline int
+ buildRelations( std::vector<std::vector<Blob2, Eigen::aligned_allocator<pcl::gpu::people::Blob2> > >& sorted)
+ {
+ PCL_VERBOSE("[pcl::gpu::people::buildRelations] : (I) : buildRelations : regular version\n");
+ if(sorted.size() == 0){
+ std::cout << "(E) : Damn you, you gave me an empty matrix!" << std::endl;
+ return (-1);
+ }
+ // Iterate over all parts
+ for(size_t p = 0; p < sorted.size(); p ++)
+ {
+ switch(p){
+ // These are multinodes and end nodes ///
+ case Neck:
+ evaluateBlobVector(sorted, p, FaceRB, 0);
+ evaluateBlobVector(sorted, p, FaceLB, 1);
+ evaluateBlobVector(sorted, p, Rchest, 2);
+ evaluateBlobVector(sorted, p, Lchest, 3);
+ break;
+ case 0: // this is the Lfoot
+ case 4: // this is the Rfoot
+ case 14: // this is the Rhand
+ case 18: // this is the Lhand
+ case 21: // this is the FaceLT
+ case 22: // this is the FaceRT
+ leafBlobVector(sorted, p); //fill in the children of leafs
+ break;
+ case 23: // this is the Rchest
+ evaluateBlobVector(sorted, p, 11, 0); //Child 0 is Rarm
+ evaluateBlobVector(sorted, p, 8, 1); //Child 1 is Rhips
+ break;
+ case 24: // this is the Lchest
+ evaluateBlobVector(sorted, p, 15, 0); //Child 0 is Larm
+ evaluateBlobVector(sorted, p, 9, 1); //Child 1 is Lhips
+ break;
+ // FROM HERE ALL THE REGULAR MIDDLE NODES ///
+ case 1: //this is the Lleg
+ evaluateBlobVector(sorted,p, 0, 0); //Child 0 is Lfeet
+ break;
+ case 2: //this is the Lknee
+ evaluateBlobVector(sorted,p, 1, 0); //Child 0 is Lleg
+ break;
+ case 3: //this is the Lthigh
+ evaluateBlobVector(sorted,p, 2, 0); //Child 0 is Lknee
+ break;
+ case 5: //this is the Rleg
+ evaluateBlobVector(sorted,p, 4, 0); //Child Rfoot
+ break;
+ case 6: //this is the Rknee
+ evaluateBlobVector(sorted,p, 5, 0); //Child Rleg
+ break;
+ case 7: //this is the Rthigh
+ evaluateBlobVector(sorted,p, 6, 0); //Child Rknee
+ break;
+ case 8: //this is the Rhips
+ evaluateBlobVector(sorted,p, 7, 0); //Child Rthigh
+ break;
+ case 9: //this is the Lhips
+ evaluateBlobVector(sorted,p, 3, 0); //Child Lthigh
+ break;
+ case Rarm:
+ evaluateBlobVector(sorted,p, Relbow, 0);
+ if(!hasThisLabelChildren(sorted, Rarm, 0))
+ evaluateBlobVector(sorted,p, Rforearm, 0);
+ break;
+ case 12: //this is the Relbow
+ evaluateBlobVector(sorted,p, 13, 0); //Child Rforearm
+ break;
+ case 13: //this is the Rforearm
+ evaluateBlobVector(sorted,p, 14, 0); //Child Rhand
+ break;
+ case Larm:
+ evaluateBlobVector(sorted,p, Lelbow, 0);
+ if(!hasThisLabelChildren(sorted, Larm, 0))
+ evaluateBlobVector(sorted,p, Lforearm, 0);
+ break;
+ case 16: //this is the Lelbow
+ evaluateBlobVector(sorted,p, 17, 0); //Child Lforearm
+ break;
+ case 17: //this is the Lforearm
+ evaluateBlobVector(sorted,p, 18, 0); //Child Lhand
+ break;
+ case 19: //this is the FaceLB
+ evaluateBlobVector(sorted,p, 21, 0); //Child FaceLT
+ break;
+ case 20: //this is the FaceRB
+ evaluateBlobVector(sorted,p, 22, 0); //Child FaceRT
+ break;
+ default:
+ break;
+ }
+ }
+ return 0;
+ }
+
+ /**
+ * @brief This function goes over the sorted matrix and fills in the optimal parent and child relations
+ * @param[in] sorted a matrix with all found good blobs arranged according to label and order
+ * @param person_attribs
+ * @return zero if everything went well, negative on an error
+ * @todo This function also fixes the kinematic chain, we should implement this in a xml or LUT
+ * @todo look if we can't get a more efficient implementation (iterator together with sortBlobs perhaps?)
+ */
+ inline int
+ buildRelations( std::vector<std::vector<Blob2, Eigen::aligned_allocator<pcl::gpu::people::Blob2> > >& sorted,
+ PersonAttribs::Ptr person_attribs)
+ {
+ PCL_DEBUG("[pcl::gpu::people::buildRelations] : (D) : person specific version\n");
+ if(sorted.size() == 0){
+ PCL_ERROR("[pcl::gpu::people::buildRelations] : (E) : Damn you, you gave me an empty matrix!\n");
+ return (-1);
+ }
+ // Iterate over all parts
+ for(size_t p = 0; p < sorted.size(); p ++)
+ {
+ switch(p){
+ // These are multinodes and end nodes ///
+ case Neck:
+ evaluateBlobVector(sorted, p, FaceRB, 0, person_attribs);
+ evaluateBlobVector(sorted, p, FaceLB, 1, person_attribs);
+ evaluateBlobVector(sorted, p, Rchest, 2, person_attribs);
+ evaluateBlobVector(sorted, p, Lchest, 3, person_attribs);
+ break;
+ case 0: // this is the Lfoot
+ case 4: // this is the Rfoot
+ case 14: // this is the Rhand
+ case 18: // this is the Lhand
+ case 21: // this is the FaceLT
+ case 22: // this is the FaceRT
+ leafBlobVector(sorted, p); //fill in the children of leafs
+ break;
+ case 23: // this is the Rchest
+ evaluateBlobVector(sorted, p, 11, 0, person_attribs); //Child 0 is Rarm
+ evaluateBlobVector(sorted, p, 8, 1, person_attribs); //Child 1 is Rhips
+ break;
+ case 24: // this is the Lchest
+ evaluateBlobVector(sorted, p, 15, 0, person_attribs); //Child 0 is Larm
+ evaluateBlobVector(sorted, p, 9, 1, person_attribs); //Child 1 is Lhips
+ break;
+ // FROM HERE ALL THE REGULAR MIDDLE NODES ///
+ case 1: //this is the Lleg
+ evaluateBlobVector(sorted,p, 0, 0, person_attribs); //Child 0 is Lfeet
+ break;
+ case 2: //this is the Lknee
+ evaluateBlobVector(sorted,p, 1, 0, person_attribs); //Child 0 is Lleg
+ break;
+ case 3: //this is the Lthigh
+ evaluateBlobVector(sorted,p, 2, 0, person_attribs); //Child 0 is Lknee
+ break;
+ case 5: //this is the Rleg
+ evaluateBlobVector(sorted,p, 4, 0, person_attribs); //Child Rfoot
+ break;
+ case 6: //this is the Rknee
+ evaluateBlobVector(sorted,p, 5, 0, person_attribs); //Child Rleg
+ break;
+ case 7: //this is the Rthigh
+ evaluateBlobVector(sorted,p, 6, 0, person_attribs); //Child Rknee
+ break;
+ case 8: //this is the Rhips
+ evaluateBlobVector(sorted,p, 7, 0, person_attribs); //Child Rthigh
+ break;
+ case 9: //this is the Lhips
+ evaluateBlobVector(sorted,p, 3, 0, person_attribs); //Child Lthigh
+ break;
+ case Rarm:
+ evaluateBlobVector(sorted,p, Relbow, 0, person_attribs);
+ if(!hasThisLabelChildren(sorted, Rarm, 0))
+ evaluateBlobVector(sorted,p, Rforearm, 0, person_attribs);
+ break;
+ case 12: //this is the Relbow
+ evaluateBlobVector(sorted,p, 13, 0, person_attribs); //Child Rforearm
+ break;
+ case 13: //this is the Rforearm
+ evaluateBlobVector(sorted,p, 14, 0, person_attribs); //Child Rhand
+ break;
+ case Larm:
+ evaluateBlobVector(sorted,p, Lelbow, 0, person_attribs);
+ if(!hasThisLabelChildren(sorted, Larm, 0))
+ evaluateBlobVector(sorted,p, Lforearm, 0, person_attribs);
+ break;
+ case 16: //this is the Lelbow
+ evaluateBlobVector(sorted,p, 17, 0, person_attribs); //Child Lforearm
+ break;
+ case 17: //this is the Lforearm
+ evaluateBlobVector(sorted,p, 18, 0, person_attribs); //Child Lhand
+ break;
+ case 19: //this is the FaceLB
+ evaluateBlobVector(sorted,p, 21, 0, person_attribs); //Child FaceLT
+ break;
+ case 20: //this is the FaceRB
+ evaluateBlobVector(sorted,p, 22, 0, person_attribs); //Child FaceRT
+ break;
+ default:
+ break;
+ }
+ }
+ return 0;
+ }
+
+
+
+ inline int browseTree (const std::vector<std::vector <Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
+ Tree2& tree,
+ int part_label,
+ int part_lid)
+ {
+ int nr_children = LUT_nr_children[part_label];
+ tree.nr_parts++;
+ tree.parts_lid[part_label] = part_lid;
+
+ const Blob2& blob = sorted[part_label][part_lid];
+
+ // iterate over the number of pixels that are part of this label
+ const std::vector<int>& indices = blob.indices.indices;
+ tree.indices.indices.insert(tree.indices.indices.end(), indices.begin(), indices.end());
+
+ if(nr_children == 0)
+ return 0;
+
+ // iterate over all possible children
+ for(int i = 0; i < nr_children; i++)
+ {
+ // check if this child has a valid child_id, leaf test should be redundant
+ if(blob.child_id[i] != NO_CHILD && blob.child_id[i] != LEAF)
+ {
+ tree.total_dist_error += blob.child_dist[i];
+ browseTree( sorted, tree, blob.child_label[i], blob.child_lid[i]);
+ }
+ }
+ return 0;
+ }
+
+ inline int browseTree (const std::vector<std::vector <Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
+ Tree2& tree,
+ int part_label,
+ int part_lid,
+ PersonAttribs::Ptr person_attribs)
+ {
+ int nr_children = person_attribs->nr_of_children_[part_label];
+ tree.nr_parts++;
+ tree.parts_lid[part_label] = part_lid;
+
+ const Blob2& blob = sorted[part_label][part_lid];
+
+ // iterate over the number of pixels that are part of this label
+ const std::vector<int>& indices = blob.indices.indices;
+ tree.indices.indices.insert(tree.indices.indices.end(), indices.begin(), indices.end());
+
+ if(nr_children == 0)
+ return 0;
+
+ // iterate over all possible children
+ for(int i = 0; i < nr_children; i++)
+ {
+ // check if this child has a valid child_id, leaf test should be redundant
+ if(blob.child_id[i] != NO_CHILD && blob.child_id[i] != LEAF)
+ {
+ tree.total_dist_error += blob.child_dist[i];
+ browseTree( sorted, tree, blob.child_label[i], blob.child_lid[i]);
+ }
+ }
+ return 0;
+ }
+
+ inline int buildTree ( const std::vector<std::vector <Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
+ const pcl::PointCloud<pcl::PointXYZ>& cloud_in,
+ part_t part_label,
+ int part_lid,
+ Tree2& tree)
+ {
+ if(sorted.size() <= 0)
+ {
+ std::cout << "(E) : buildTree(): hey man, don't fool me, you gave me an empty blob matrix" << std::endl;
+ return -1;
+ }
+ tree.label = part_label;
+ tree.lid = part_lid;
+ tree.total_dist_error = 0;
+ tree.nr_parts = 0;
+
+ browseTree(sorted, tree, part_label, part_lid);
+
+ pcl::getMinMax3D(cloud_in, tree.indices, tree.min, tree.max);
+ pcl::compute3DCentroid(cloud_in, tree.indices, tree.mean);
+ pcl::computeCovarianceMatrixNormalized(cloud_in, tree.indices, tree.mean, tree.cov);
+
+ pcl::eigen33(tree.cov, tree.eigenvect, tree.eigenval);
+
+ tree.norm_dist_error = tree.total_dist_error/tree.nr_parts;
+
+ return 0;
+ }
+
+ inline int buildTree ( const std::vector<std::vector <Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
+ const pcl::PointCloud<pcl::PointXYZ>& cloud_in,
+ part_t part_label,
+ int part_lid,
+ Tree2& tree,
+ PersonAttribs::Ptr person_attribs)
+ {
+ if(sorted.size() <= 0)
+ {
+ std::cout << "(E) : buildTree(): hey man, don't fool me, you gave me an empty blob matrix" << std::endl;
+ return -1;
+ }
+ tree.label = part_label;
+ tree.lid = part_lid;
+ tree.total_dist_error = 0;
+ tree.nr_parts = 0;
+
+ browseTree(sorted, tree, part_label, part_lid, person_attribs);
+
+ pcl::getMinMax3D(cloud_in, tree.indices, tree.min, tree.max);
+ pcl::compute3DCentroid(cloud_in, tree.indices, tree.mean);
+ pcl::computeCovarianceMatrixNormalized(cloud_in, tree.indices, tree.mean, tree.cov);
+
+ pcl::eigen33(tree.cov, tree.eigenvect, tree.eigenval);
+
+ tree.norm_dist_error = tree.total_dist_error/tree.nr_parts;
+
+ return 0;
+ }
+
+ } //end namespace people
+ } // end namespace gpu
+} // end namespace pcl
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * @author: Koen Buys
+ */
+
+#ifndef PCL_GPU_PEOPLE_ORGANIZED_PLANE_DETECTOR_H_
+#define PCL_GPU_PEOPLE_ORGANIZED_PLANE_DETECTOR_H_
+
+#include <pcl/pcl_exports.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+#include <pcl/features/integral_image_normal.h>
+#include <pcl/segmentation/organized_multi_plane_segmentation.h>
+#include <pcl/common/transforms.h>
+#include <pcl/gpu/people/label_common.h>
+
+#include <boost/shared_ptr.hpp>
+#include <string>
+#include <vector>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace people
+ {
+ class OrganizedPlaneDetector
+ {
+ public:
+ typedef boost::shared_ptr<OrganizedPlaneDetector> Ptr;
+
+ typedef pcl::PointXYZRGBA PointTC;
+ typedef pcl::PointXYZ PointT;
+
+ typedef pcl::PointCloud<pcl::device::prob_histogram> HostLabelProbability;
+
+ //typedef DeviceArray2D<unsigned char> Labels;
+ //typedef DeviceArray2D<unsigned short> Depth;
+ //typedef DeviceArray2D<pcl::RGB> Image;
+
+ HostLabelProbability P_l_host_; // This is a HOST histogram!
+ HostLabelProbability P_l_host_prev_;
+
+ pcl::device::LabelProbability P_l_dev_; // This is a DEVICE histogram!
+ pcl::device::LabelProbability P_l_dev_prev_;
+
+ protected:
+ pcl::IntegralImageNormalEstimation<PointTC, pcl::Normal> ne_;
+ pcl::OrganizedMultiPlaneSegmentation<PointTC, pcl::Normal, pcl::Label> mps_;
+
+ float ne_NormalSmoothingSize_;
+ float ne_MaxDepthChangeFactor_;
+
+ int mps_MinInliers_;
+ double mps_AngularThreshold_;
+ double mps_DistanceThreshold_;
+ bool mps_use_planar_refinement_;
+
+ public:
+ /** \brief This is the constructor **/
+ OrganizedPlaneDetector (int rows = 480, int cols = 640);
+
+ /** \brief Process step, this wraps Organized Plane Segmentation code **/
+ void process (const PointCloud<PointTC>::ConstPtr &cloud);
+
+ double getMpsAngularThreshold () const
+ {
+ return mps_AngularThreshold_;
+ }
+
+ void setMpsAngularThreshold (double mpsAngularThreshold)
+ {
+ mps_AngularThreshold_ = mpsAngularThreshold;
+ mps_.setAngularThreshold (mps_AngularThreshold_);
+ }
+
+ double getMpsDistanceThreshold () const
+ {
+ return mps_DistanceThreshold_;
+ }
+
+ void setMpsDistanceThreshold (double mpsDistanceThreshold)
+ {
+ mps_DistanceThreshold_ = mpsDistanceThreshold;
+ mps_.setDistanceThreshold (mps_DistanceThreshold_);
+ }
+
+ int getMpsMinInliers () const
+ {
+ return mps_MinInliers_;
+ }
+
+ void setMpsMinInliers (int mpsMinInliers)
+ {
+ mps_MinInliers_ = mpsMinInliers;
+ mps_.setMinInliers (mps_MinInliers_);
+
+
+ }
+
+ float getNeMaxDepthChangeFactor () const
+ {
+ return ne_MaxDepthChangeFactor_;
+ }
+
+ void setNeMaxDepthChangeFactor (float neMaxDepthChangeFactor)
+ {
+ ne_MaxDepthChangeFactor_ = neMaxDepthChangeFactor;
+ ne_.setMaxDepthChangeFactor (ne_MaxDepthChangeFactor_);
+ }
+
+ float getNeNormalSmoothingSize () const
+ {
+ return ne_NormalSmoothingSize_;
+ }
+
+ void setNeNormalSmoothingSize (float neNormalSmoothingSize)
+ {
+ ne_NormalSmoothingSize_ = neNormalSmoothingSize;
+ ne_.setNormalSmoothingSize (ne_NormalSmoothingSize_);
+ }
+
+ void
+ emptyHostLabelProbability(HostLabelProbability& histogram);
+
+ int
+ copyHostLabelProbability(HostLabelProbability& src,
+ HostLabelProbability& dst);
+
+ int
+ copyAndClearHostLabelProbability(HostLabelProbability& src,
+ HostLabelProbability& dst);
+
+ private:
+ void allocate_buffers(int rows = 480, int cols = 640);
+
+ };
+ }
+ }
+}
+
+#endif /* PCL_GPU_PEOPLE_FACE_DETECTOR_H_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * @author: Koen Buys
+ */
+
+#ifndef PCL_GPU_PEOPLE_PERSON_H_
+#define PCL_GPU_PEOPLE_PERSON_H_
+
+#include <iostream>
+#include <sstream>
+#include <fstream>
+
+#include <pcl/point_types.h>
+#include <pcl/console/print.h>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/people/label_common.h>
+#include <pcl/gpu/people/tree.h>
+#include <pcl/gpu/people/person_attribs.h>
+//#include <opencv2/core/core.hpp>
+
+#include <pcl/gpu/people/bodyparts_detector.h>
+#include <pcl/gpu/people/face_detector.h>
+#include <pcl/gpu/people/organized_plane_detector.h>
+#include <pcl/gpu/people/probability_processor.h>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace people
+ {
+ /*
+ struct OtherDetector
+ {
+ typedef boost::shared_ptr<OtherDetector> Ptr;
+ };
+ */
+ class PCL_EXPORTS PeopleDetector
+ {
+ public:
+ typedef boost::shared_ptr<PeopleDetector> Ptr;
+
+ typedef pcl::PointXYZRGBA PointTC;
+ typedef pcl::PointXYZ PointT;
+ typedef DeviceArray2D<unsigned short> Depth;
+ typedef DeviceArray2D<pcl::RGB> Image;
+
+ // ALL THE DETECTOR OBJECTS
+ RDFBodyPartsDetector::Ptr rdf_detector_;
+ OrganizedPlaneDetector::Ptr org_plane_detector_;
+ FaceDetector::Ptr face_detector_;
+ //OtherDetector::Ptr other_detector_;
+
+ // ALL THE OTHER PEOPLE STUFF
+ PersonAttribs::Ptr person_attribs_;
+ ProbabilityProcessor::Ptr probability_processor_;
+
+ /** \brief Class constructor. */
+ PeopleDetector ();
+
+ /** \brief Class destructor. */
+ ~PeopleDetector () {}
+
+ /** \brief User must set non standard intrinsics */
+ void
+ setIntrinsics (float fx, float fy, float cx = -1, float cy = -1);
+
+ /** \brief Possible will be removed because of extra overheads */
+ int
+ process (const PointCloud<PointTC>::ConstPtr &cloud);
+
+ int
+ processProb (const PointCloud<PointTC>::ConstPtr &cloud);
+
+ int
+ process (const Depth& depth, const Image& rgba);
+
+ /** \brief Set the tolerance for the delta on the Hue in Seeded Hue Segmentation step */
+ inline void
+ setDeltaHueTolerance (unsigned int delta_hue_tolerance)
+ {
+ delta_hue_tolerance_ = delta_hue_tolerance;
+ }
+
+ /** \brief Get the tolerance for the delta on the Hue in Seeded Hue Segmentation step, defaults to 5 */
+ inline unsigned int
+ getDeltaHueTolerance () const
+ {
+ return (delta_hue_tolerance_);
+ }
+
+ /** \brief Class getName method. */
+ inline const std::string getClassName () const { return "PeopleDetector"; }
+
+ typedef DeviceArray2D<unsigned char> Labels;
+ typedef DeviceArray2D<unsigned char> Mask;
+ typedef DeviceArray2D<float> Hue;
+
+ /** \brief indicates first time callback (allows for tracking features to start from second frame) **/
+ bool first_iteration_;
+ float fx_, fy_, cx_, cy_;
+ unsigned int delta_hue_tolerance_;
+
+ DeviceArray<unsigned char> kernelRect5x5_;
+
+ PointCloud<PointT> cloud_host_;
+ PointCloud<PointTC> cloud_host_color_;
+ PointCloud<float> hue_host_;
+ PointCloud<unsigned short> depth_host_;
+ PointCloud<unsigned char> flowermat_host_;
+
+ DeviceArray2D<PointT> cloud_device_;
+
+ Hue hue_device_;
+
+ Depth depth_device1_;
+ Depth depth_device2_;
+
+ Mask fg_mask_;
+ Mask fg_mask_grown_;
+
+ int
+ process ();
+
+ /**
+ * \brief Process the depth based on probabilities supporting tracking, person specific files used
+ **/
+ int
+ processProb ();
+
+ void
+ allocate_buffers (int rows = 480, int cols = 640);
+
+ void
+ shs5 (const pcl::PointCloud<PointT> &cloud, const std::vector<int>& indices, unsigned char *mask);
+
+ //!!! only for debug purposes TODO: remove this.
+ friend class PeoplePCDApp;
+ };
+ }
+ }
+}
+#endif // PCL_GPU_PEOPLE_PERSON_H_
--- /dev/null
+#ifndef PLC_GPU_PEOPLE_PERSON_ATTRIBS_H_
+#define PLC_GPU_PEOPLE_PERSON_ATTRIBS_H_
+
+#include <string>
+#include <vector>
+#include <iosfwd>
+#include <boost/shared_ptr.hpp>
+
+#include <pcl/pcl_exports.h>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace people
+ {
+ class PCL_EXPORTS PersonAttribs
+ {
+ public:
+ typedef boost::shared_ptr<PersonAttribs> Ptr;
+
+ /** \brief Constructor creates generic values from **/
+ PersonAttribs();
+
+ /**
+ * \brief Read XML configuration file for a specific person
+ * \param[in] is input stream of file
+ * \return 0 when successfull, -1 when an error occured, datastructure might become corrupted in the process
+ **/
+ int
+ readPersonXMLConfig (std::istream& is);
+
+ /**
+ * \brief Write XML configuration file for a specific person
+ * \param[in] os output stream of file, extension determines format
+ **/
+ void
+ writePersonXMLConfig (std::ostream& os);
+
+ std::string name_; // Name of the person
+ std::vector<float> max_part_size_; // Max primary eigenvalue for each body part
+ std::vector<std::vector<float> > part_ideal_length_; // Ideal length between two body parts
+ std::vector<std::vector<float> > max_length_offset_; // Max allowed length offset between two body parts
+ std::vector<int> nr_of_children_; // The number of children for each part
+ };
+ }
+ }
+}
+
+#endif /* PLC_GPU_PEOPLE_PERSON_ATTRIBS_H_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * @author: Koen Buys
+ */
+
+#ifndef PCL_GPU_PROBABILITY_PROCESSOR_H_
+#define PCL_GPU_PROBABILITY_PROCESSOR_H_
+
+#include <iostream>
+#include <sstream>
+#include <fstream>
+
+#include <pcl/point_types.h>
+#include <pcl/console/print.h>
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/people/label_common.h>
+
+namespace pcl
+{
+ namespace device
+ {
+ class ProbabilityProc;
+ }
+
+ namespace gpu
+ {
+ namespace people
+ {
+ class PCL_EXPORTS ProbabilityProcessor
+ {
+ public:
+ typedef boost::shared_ptr<ProbabilityProcessor> Ptr;
+ typedef DeviceArray2D<unsigned short> Depth;
+ typedef DeviceArray2D<unsigned char> Labels;
+
+ ProbabilityProcessor();
+
+ /** \brief This will merge the votes from the different trees into one final vote, including probabilistic's **/
+ void
+ SelectLabel (const Depth& depth, Labels& labels, pcl::device::LabelProbability& probabilities);
+
+ /** \brief This will combine two probabilities according their weight **/
+ void
+ CombineProb ( const Depth& depth,
+ pcl::device::LabelProbability& probIn1,
+ float weight1,
+ pcl::device::LabelProbability& probIn2,
+ float weight2,
+ pcl::device::LabelProbability& probOut);
+
+ /** \brief This will sum a probability multiplied with it's weight **/
+ void
+ WeightedSumProb ( const Depth& depth, pcl::device::LabelProbability& probIn, float weight, pcl::device::LabelProbability& probOut);
+
+ /** \brief This will create a Gaussian Kernel **/
+ float*
+ CreateGaussianKernel ( float sigma,
+ int kernelSize);
+
+ /** \brief This will do a GaussianBlur over the LabelProbability **/
+ int
+ GaussianBlur( const Depth& depth,
+ pcl::device::LabelProbability& probIn,
+ DeviceArray<float>& kernel,
+ pcl::device::LabelProbability& probOut);
+
+ /** \brief This will do a GaussianBlur over the LabelProbability **/
+ int
+ GaussianBlur( const Depth& depth,
+ pcl::device::LabelProbability& probIn,
+ DeviceArray<float>& kernel,
+ pcl::device::LabelProbability& probTemp,
+ pcl::device::LabelProbability& probOut);
+
+ private:
+ boost::shared_ptr<pcl::device::ProbabilityProc> impl_;
+
+ };
+ }
+ }
+}
+
+#endif // PCL_GPU_PROBABILITY_PROCESSOR_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * @authors: Cedric Cagniart, Koen Buys
+ */
+
+#ifndef PCL_GPU_PEOPLE_TREE_H_
+#define PCL_GPU_PEOPLE_TREE_H_
+
+#include "label_common.h"
+#include <boost/cstdint.hpp>
+#include <iostream>
+#include <vector>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace people
+ {
+ namespace trees
+ {
+ // this has nothing to do here...
+ static const double focal = 1000.;
+
+ // ###############################################
+ // compile type values
+ enum { NUM_ATTRIBS = 2000 };
+ enum { NUM_LABELS = 32 };
+
+ // ###############################################
+ // base data types used in the structures
+
+ using boost::uint8_t;
+ using boost::int16_t;
+ using boost::uint16_t;
+ using boost::int32_t;
+ using boost::uint32_t;
+
+ typedef int16_t Attrib;
+ typedef uint8_t Label;
+ typedef uint32_t Label32;
+ typedef uint16_t Depth;
+
+ struct AttribLocation
+ {
+ inline AttribLocation () {du1=dv1=du2=dv2=0;}
+ inline AttribLocation (int u1, int v1, int u2, int v2): du1 (static_cast<int16_t>(u1)),
+ dv1 (static_cast<int16_t>(v1)),
+ du2 (static_cast<int16_t>(u2)),
+ dv2 (static_cast<int16_t>(v2))
+ {}
+
+ int16_t du1,dv1,du2,dv2;
+ };
+
+ ////////////////////////////////////////////////
+ // Tree basic Structure
+ struct Node
+ {
+ Node () {}
+ Node (const AttribLocation& l, const Attrib& t) : loc(l), thresh(t) {}
+ AttribLocation loc;
+ Attrib thresh;
+ };
+
+ struct Histogram
+ {
+ float label_prob[NUM_PARTS];
+ };
+
+ ////////////////////////////////////////////////
+ // tree_io - Reading and writing AttributeLocations
+ inline std::ostream& operator << (std::ostream& os, const AttribLocation& aloc ) { return os<<aloc.du1<<" "<<aloc.dv1<<" "<<aloc.du2<<" "<<aloc.dv2<<"\n"; }
+ inline std::istream& operator >> (std::istream& is, AttribLocation& aloc ) { return is >> aloc.du1 >> aloc.dv1 >> aloc.du2 >> aloc.dv2; }
+ inline std::istream& operator >> (std::istream& is, Node& n) { return is >> n.loc >> n.thresh; }
+
+ void writeAttribLocs( const std::string& filename, const std::vector<AttribLocation>& alocs );
+ void readAttribLocs( const std::string& filename, std::vector<AttribLocation>& alocs );
+ void readThreshs( const std::string& filename, std::vector<Attrib>& threshs );
+ void writeThreshs( const std::string& filename, const std::vector<Attrib>& threshs );
+
+ ////////////////////////////////////////////////
+ // tree_run
+
+ /** The stream points to ascii data that goes:
+ * ##################
+ * TreeHeight
+ * du1 dv1 du2 dv2 thresh
+ * du1 dv1 du2 dv2 thresh
+ * ............
+ * label
+ * label
+ * ##################
+ *
+ * there are 2^threeheight -1 nodes ( [du1 dv1 du2 dv2 thresh] lines )
+ * there are 2^threeheight leaves ( [label] lines )
+ */
+ int loadTree( std::istream& is, std::vector<Node>& tree, std::vector<Label>& leaves );
+ int loadTree( const std::string& filename, std::vector<Node>& tree, std::vector<Label>& leaves );
+ void runThroughTree( int maxDepth, const std::vector<Node>& tree, const std::vector<Label>& leaves, int W, int H, const uint16_t* dmap, Label* lmap );
+
+ } // end namespace Trees
+ } // end namespace people
+ } // end namespace gpu
+} // end namespace pcl
+#endif // PCL_GPU_PEOPLE_TREES_TREE_H_
--- /dev/null
+/* *************************************************
+ *
+ * Copyright (2011) Willow Garage
+ *
+ * Author : Cedric Cagniart
+ * ************************************************* */
+
+#ifndef PCL_GPU_PEOPLE_TREE_TRAIN_H_
+#define PCL_GPU_PEOPLE_TREE_TRAIN_H_
+
+#include "tree.h"
+#include <boost/array.hpp>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace people
+ {
+ namespace trees
+ {
+ // ################################################
+ // ################################################
+ // histogram stuff
+ class Histogram : public boost::array<uint32_t,NUMLABELS> {
+ public :
+ inline Histogram() { std::fill(begin(), end(), 0); }
+ };
+
+ struct HistogramPair {
+ public :
+ // accumulate on the histograms
+ inline void accumTrue(const Label label ) {
+ m_h_true[label]++;
+ }
+ inline void accumFalse(const Label label ) {
+ m_h_false[label]++;
+ }
+
+ inline Histogram& h_false() { return m_h_false; }
+ inline Histogram& h_true() { return m_h_true; }
+
+ inline const Histogram h_false() const { return m_h_false; }
+ inline const Histogram h_true() const { return m_h_true; }
+
+ protected :
+ Histogram m_h_false;
+ Histogram m_h_true;
+ };
+
+ // ###############################################
+ // ###############################################
+ // SplitPoint
+ struct SplitPoint{
+ inline SplitPoint( int ai, Attrib t):attribId(ai), threshold(t){}
+ int attribId;
+ Attrib threshold;
+ };
+
+ // ###############################################
+ // ###############################################
+ // Data Structures as stored in binary files
+ struct LabeledAttrib {
+ inline LabeledAttrib(){}
+ inline LabeledAttrib( const Label& label, const Attrib& attrib): l(label), a(attrib){}
+ Label l;
+ Attrib a;
+ };
+
+ // this is only going to be a helper structure
+ struct LabeledFeature { // : boost::noncopyable {
+ // constructors
+ inline LabeledFeature(): l(NOLABEL){
+ }
+ inline LabeledFeature( const LabeledFeature& B){
+ l = B.l;
+ std::copy( B.attribs, B.attribs + NUMATTRIBS, attribs );
+ }
+ Label l; // WARNING the compiler will pad here
+ Attrib attribs[NUMATTRIBS];
+ };
+
+
+ // compute the number of elements
+ static inline uint64_t numElements( const Histogram& h ) {
+ uint64_t Ntotal = 0;
+ for(int li=0;li<NUMLABELS;++li) Ntotal += uint64_t(h[li]);
+ return Ntotal;
+ }
+
+ /**
+ * This is cool
+ */
+ static inline double entropy( const Histogram& h ) {
+ double Ntotal = numElements(h);
+ double entropy = 0.;
+ for(int li=0;li<NUMLABELS;++li) {
+ if( h[li] != 0 ) {
+ double p = double(h[li]) / Ntotal;
+ entropy -= p*log(p);
+ }
+ }
+ return entropy;
+ }
+
+ /**
+ * This is a little weird.. it will just compute the entropy of the merged histograms
+ */
+ static inline double entropy_merged( const HistogramPair& hp ) {
+ const Histogram& htrue = hp.h_true();
+ const Histogram& hfalse = hp.h_false();
+
+ double Ntotal = numElements(htrue) + numElements(hfalse);
+ double entropy = 0.;
+ for(int li=0;li<NUMLABELS;++li) {
+ uint64_t Ni = uint64_t(htrue[li]) + uint64_t(hfalse[li]);
+ if( Ni != 0) {
+ double p = double(Ni) / Ntotal;
+ entropy -= p*log(p);
+ }
+ }
+ return entropy;
+ }
+
+ /**
+ * This will compute the gain in information resulting from the split
+ */
+ static inline double informationGain( const HistogramPair& hp) {
+ double e0 = entropy_merged(hp);
+ double etrue = entropy(hp.h_true());
+ double efalse = entropy(hp.h_false());
+
+ double Ntrue = numElements(hp.h_true());
+ double Nfalse = numElements(hp.h_false());
+ double Ntotal = Ntrue + Nfalse;
+
+ // lets avoid division by 0
+ if( Ntotal == 0 ) return 0.;
+ return e0 - (Ntrue/Ntotal)*etrue - (Nfalse/Ntotal)*efalse;
+ }
+
+ // #########################################
+ // #########################################
+ // Reading and writing histograms
+ static inline std::ostream& operator << (std::ostream& os, const Histogram& h) {
+ for(int li=0;li<NUMLABELS;++li) os<< h[li]<<" ";
+ os<<"\n";
+ return os;
+ }
+
+ static inline std::istream& operator >> (std::istream& is, Histogram& h) {
+ for(int li=0;li<NUMLABELS;++li) is >> h[li];
+ return is;
+ }
+
+ // #######################################
+ // #######################################
+ // reading and writing histogram Pairs
+ static inline std::ostream& operator << ( std::ostream& os, const HistogramPair& hp) {
+ os << hp.h_false();
+ os << hp.h_true();
+ return os;
+ }
+
+ static inline std::istream& operator >> ( std::istream& is, HistogramPair& hp) {
+ is >> hp.h_false();
+ is >> hp.h_true();
+ return is;
+ }
+
+ // #########################################
+ // #########################################
+ // Reading and writing LabeledFeature Vectors ( label + collection of attrib )
+ static void writeLabeledFeatureVec( std::ostream& os, const std::vector<LabeledFeature>& lfs ){
+ os.write( (const char*)&lfs[0], sizeof(LabeledFeature)*lfs.size() );
+ }
+
+ // static void readLabeledFeature( std::istream& is, LabeledFeature& lf)
+ // {
+ // is.read( (char*)&lf, sizeof(LabeledFeature) );
+ // if( is.fail() ) throw std::runtime_error();
+ // }
+
+ // #######################################
+ // #######################################
+ // reading and writing split points
+ inline std::ostream& operator << ( std::ostream& os, const SplitPoint& sp){
+ os<<sp.attribId<<" "<<sp.threshold<<"\n";
+ return os;
+ }
+
+ inline std::istream& operator >> ( std::istream& is, SplitPoint& sp){
+ is >> sp.attribId >> sp.threshold;
+ return is;
+ }
+
+ // #######################################
+ // #######################################
+ // reading and writing info files
+ inline void writeInfoFile( const std::string& filename,
+ int attribId,
+ Attrib threshold,
+ double gain,
+ const HistogramPair& HP){
+ std::ofstream fout(filename.c_str() );
+ if( !fout.is_open() ) throw std::runtime_error(std::string("(E) could not open ") + filename );
+
+ fout<<int(attribId)<<" "<<int(threshold)<<"\n";
+ fout<<gain<<"\n";
+ fout<<HP;
+ }
+
+ inline void readInfoFile( const std::string& filename,
+ int& attribId,
+ Attrib& threshold,
+ double& gain,
+ HistogramPair& HP ) {
+ std::ifstream fin(filename.c_str() );
+ if( !fin.is_open() ) throw std::runtime_error(std::string("(E) could not open") + filename );
+
+ fin>>attribId >>threshold>>gain>>HP;
+ if( fin.fail() ) throw std::runtime_error(std::string("(E) malformed splitInfo file ") + filename );
+ }
+
+
+ } // end namespace trees
+ } // end namespace people
+ } // end namespace gpu
+} // end namespace pcl
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * @author: Koen Buys, Anatoly Baksheev
+ */
+
+#include <pcl/gpu/people/bodyparts_detector.h>
+//#include <pcl/gpu/people/conversions.h>
+#include <pcl/gpu/people/label_common.h>
+//#include <pcl/gpu/people/label_segment.h>
+#include <pcl/gpu/people/label_tree.h>
+
+#include <pcl/common/time.h>
+#include <pcl/console/print.h>
+#include "cuda.h"
+#include "emmintrin.h"
+
+#include <cassert>
+#include "internal.h"
+#include "cuda_async_copy.h"
+
+using namespace std;
+
+const int MAX_CLUST_SIZE = 25000;
+const float CLUST_TOL = 0.05f;
+
+pcl::gpu::people::RDFBodyPartsDetector::RDFBodyPartsDetector( const vector<string>& tree_files, int rows, int cols)
+: max_cluster_size_(MAX_CLUST_SIZE), cluster_tolerance_(CLUST_TOL)
+{
+ PCL_DEBUG("[pcl::gpu::people::RDFBodyPartsDetector::RDFBodyPartsDetector] : (D) : Constructor called\n");
+ //TODO replace all asserts with exceptions
+ assert(!tree_files.empty());
+
+ impl_.reset ( new device::MultiTreeLiveProc(rows, cols) );
+
+ for(size_t i = 0; i < tree_files.size(); ++i)
+ {
+ // load the tree file
+ vector<trees::Node> nodes;
+ vector<trees::Label> leaves;
+
+ // this might throw but we haven't done any malloc yet
+ int height = loadTree (tree_files[i], nodes, leaves );
+ impl_->trees.push_back(device::CUDATree(height, nodes, leaves));
+ }
+
+ allocate_buffers(rows, cols);
+}
+
+////////////////////////////////////////////////////////////////////////////////////
+/// getters
+
+size_t
+pcl::gpu::people::RDFBodyPartsDetector::getNumberTrees() const
+{
+ return impl_->trees.size();
+}
+
+const pcl::device::Labels&
+pcl::gpu::people::RDFBodyPartsDetector::getLabels() const
+{
+ return labels_smoothed_;
+}
+
+const pcl::device::LabelProbability&
+pcl::gpu::people::RDFBodyPartsDetector::getProbability() const
+{
+ return P_l_;
+}
+
+const pcl::device::LabelProbability&
+pcl::gpu::people::RDFBodyPartsDetector::getProbability1() const
+{
+ return P_l_1_;
+}
+
+const pcl::device::LabelProbability&
+pcl::gpu::people::RDFBodyPartsDetector::getProbability2() const
+{
+ return P_l_2_;
+}
+
+const pcl::device::LabelProbability&
+pcl::gpu::people::RDFBodyPartsDetector::getPrevProbability1() const
+{
+ return P_l_prev_1_;
+}
+
+const pcl::device::LabelProbability&
+pcl::gpu::people::RDFBodyPartsDetector::getPrevProbability2() const
+{
+ return P_l_prev_2_;
+}
+
+const pcl::gpu::people::RDFBodyPartsDetector::BlobMatrix&
+pcl::gpu::people::RDFBodyPartsDetector::getBlobMatrix() const
+{
+ return blob_matrix_;
+}
+
+////////////////////////////////////////////////////////////////////////////////////
+
+void
+pcl::gpu::people::RDFBodyPartsDetector::allocate_buffers(int rows, int cols)
+{
+ //std::cout << "(I) : RDFBodyPartsDetector::allocate_buffers called with: " << cols << "x" << rows << std::endl;
+
+ labels_.create(rows, cols);
+ labels_smoothed_.create(rows, cols);
+
+ // Create all the label probabilities
+ P_l_.create(rows,cols);
+ P_l_Gaus_.create(rows,cols);
+ P_l_Gaus_Temp_.create(rows,cols);
+ P_l_1_.create(rows,cols);
+ P_l_2_.create(rows,cols);
+ P_l_prev_1_.create(rows,cols);
+ P_l_prev_2_.create(rows,cols);
+
+ lmap_host_.resize(rows * cols);
+
+ dst_labels_.resize(rows * cols);
+ region_sizes_.resize(rows*cols+1);
+ remap_.resize(rows*cols);
+
+ comps_.create(rows, cols);
+ device::ConnectedComponents::initEdges(rows, cols, edges_);
+
+ means_storage_.resize((cols * rows + 1) * 3); // float3 * cols * rows and float3 for cc == -1.
+
+ blob_matrix_.resize(NUM_PARTS);
+ for(size_t i = 0; i < blob_matrix_.size(); ++i)
+ {
+ blob_matrix_[i].clear();
+ blob_matrix_[i].reserve(5000);
+ }
+}
+
+void
+pcl::gpu::people::RDFBodyPartsDetector::process (const pcl::device::Depth& depth, const PointCloud<PointXYZ>& cloud, int min_pts_per_cluster)
+{
+ //ScopeTime time("ev");
+
+ int cols = depth.cols();
+ int rows = depth.rows();
+
+ allocate_buffers(rows, cols);
+
+ {
+ {
+ //ScopeTime time("--");
+ // Process the depthimage (CUDA)
+ impl_->process(depth, labels_);
+ device::smoothLabelImage(labels_, depth, labels_smoothed_, NUM_PARTS, 5, 300);
+ }
+
+ //AsyncCopy<unsigned char> async_labels_download(lmap_host_);
+
+ int c;
+ labels_smoothed_.download(lmap_host_, c);
+ //async_labels_download.download(labels_smoothed_);
+
+ // cc = generalized floodfill = approximation of euclidian clusterisation
+ device::ConnectedComponents::computeEdges(labels_smoothed_, depth, NUM_PARTS, cluster_tolerance_ * cluster_tolerance_, edges_);
+ device::ConnectedComponents::labelComponents(edges_, comps_);
+
+ comps_.download(dst_labels_, c);
+
+ //async_labels_download.waitForCompeltion();
+ }
+
+ // This was sort indices to blob (sortIndicesToBlob2) method (till line 236)
+ {
+ //ScopeTime time("cvt");
+ std::fill(remap_.begin(), remap_.end(), -1);
+ std::fill(region_sizes_.begin(), region_sizes_.end(), 0);
+
+ std::fill(means_storage_.begin(), means_storage_.end(), 0);
+ float3* means = (float3*) &means_storage_[3];
+ int *rsizes = ®ion_sizes_[1];
+
+ for(size_t i = 0; i < blob_matrix_.size(); ++i)
+ blob_matrix_[i].clear();
+
+ for(size_t k = 0; k < dst_labels_.size(); ++k)
+ {
+ const PointXYZ& p = cloud.points[k];
+ int cc = dst_labels_[k];
+ means[cc].x += p.x;
+ means[cc].y += p.y;
+ means[cc].z += p.z;
+ ++rsizes[cc];
+ }
+
+ means[-1].z = 0; // cc == -1 means invalid
+
+ for(size_t k = 0; k < dst_labels_.size(); ++k)
+ {
+ int label = lmap_host_[k];
+ int cc = dst_labels_[k];
+
+ if (means[cc].z != 0 && min_pts_per_cluster <= rsizes[cc] && rsizes[cc] <= max_cluster_size_)
+ {
+ int ccindex = remap_[cc];
+ if (ccindex == -1)
+ {
+ ccindex = static_cast<int> (blob_matrix_[label].size ());
+ blob_matrix_[label].resize(ccindex + 1);
+ remap_[cc] = ccindex;
+
+ blob_matrix_[label][ccindex].label = static_cast<part_t> (label);
+ blob_matrix_[label][ccindex].mean.coeffRef(0) = means[cc].x / static_cast<float> (rsizes[cc]);
+ blob_matrix_[label][ccindex].mean.coeffRef(1) = means[cc].y / static_cast<float> (rsizes[cc]);
+ blob_matrix_[label][ccindex].mean.coeffRef(2) = means[cc].z / static_cast<float> (rsizes[cc]);
+ blob_matrix_[label][ccindex].indices.indices.reserve(rsizes[cc]);
+ }
+ blob_matrix_[label][ccindex].indices.indices.push_back(static_cast<int> (k));
+ }
+ }
+
+ int id = 0;
+ for(size_t label = 0; label < blob_matrix_.size(); ++label)
+ for(size_t b = 0; b < blob_matrix_[label].size(); ++b)
+ {
+ blob_matrix_[label][b].id = id++;
+ blob_matrix_[label][b].lid = static_cast<int> (b);
+ }
+
+ buildRelations ( blob_matrix_ );
+ }
+}
+
+void
+pcl::gpu::people::RDFBodyPartsDetector::processProb (const pcl::device::Depth& depth)
+{
+ int cols = depth.cols();
+ int rows = depth.rows();
+
+ allocate_buffers(rows, cols);
+
+ // Process the depthimage into probabilities (CUDA)
+ //impl_->process(depth, labels_);
+ //impl_->processProb(depth, labels_, P_l_, (int) std::numeric_limits<int16_t>::max());
+ impl_->processProb(depth, labels_, P_l_, std::numeric_limits<int>::max());
+}
+
+void
+pcl::gpu::people::RDFBodyPartsDetector::processSmooth (const pcl::device::Depth& depth, const PointCloud<PointXYZ>& cloud, int min_pts_per_cluster)
+{
+ device::smoothLabelImage(labels_, depth, labels_smoothed_, NUM_PARTS, 5, 300);
+
+ //AsyncCopy<unsigned char> async_labels_download(lmap_host_);
+
+ int c;
+ labels_smoothed_.download(lmap_host_, c);
+ //async_labels_download.download(labels_smoothed_);
+
+ // cc = generalized floodfill = approximation of euclidian clusterisation
+ device::ConnectedComponents::computeEdges(labels_smoothed_, depth, NUM_PARTS, cluster_tolerance_ * cluster_tolerance_, edges_);
+ device::ConnectedComponents::labelComponents(edges_, comps_);
+
+ comps_.download(dst_labels_, c);
+
+ //async_labels_download.waitForCompeltion();
+
+ // This was sort indices to blob (sortIndicesToBlob2) method (till line 236)
+ {
+ ScopeTime time("[pcl::gpu::people::RDFBodyPartsDetector::processSmooth] : cvt");
+ std::fill(remap_.begin(), remap_.end(), -1);
+ std::fill(region_sizes_.begin(), region_sizes_.end(), 0);
+
+ std::fill(means_storage_.begin(), means_storage_.end(), 0);
+ float3* means = (float3*) &means_storage_[3];
+ int *rsizes = ®ion_sizes_[1];
+
+ for(size_t i = 0; i < blob_matrix_.size(); ++i)
+ blob_matrix_[i].clear();
+
+ for(size_t k = 0; k < dst_labels_.size(); ++k)
+ {
+ const PointXYZ& p = cloud.points[k];
+ int cc = dst_labels_[k];
+ means[cc].x += p.x;
+ means[cc].y += p.y;
+ means[cc].z += p.z;
+ ++rsizes[cc];
+ }
+
+ means[-1].z = 0; // cc == -1 means invalid
+
+ for(size_t k = 0; k < dst_labels_.size(); ++k)
+ {
+ int label = lmap_host_[k];
+ int cc = dst_labels_[k];
+
+ if (means[cc].z != 0 && min_pts_per_cluster <= rsizes[cc] && rsizes[cc] <= max_cluster_size_)
+ {
+ int ccindex = remap_[cc];
+ if (ccindex == -1)
+ {
+ ccindex = static_cast<int> (blob_matrix_[label].size ());
+ blob_matrix_[label].resize(ccindex + 1);
+ remap_[cc] = ccindex;
+
+ blob_matrix_[label][ccindex].label = static_cast<part_t> (label);
+ blob_matrix_[label][ccindex].mean.coeffRef(0) = means[cc].x / static_cast<float> (rsizes[cc]);
+ blob_matrix_[label][ccindex].mean.coeffRef(1) = means[cc].y / static_cast<float> (rsizes[cc]);
+ blob_matrix_[label][ccindex].mean.coeffRef(2) = means[cc].z / static_cast<float> (rsizes[cc]);
+ blob_matrix_[label][ccindex].indices.indices.reserve(rsizes[cc]);
+ }
+ blob_matrix_[label][ccindex].indices.indices.push_back(static_cast<int> (k));
+ }
+ }
+
+ int id = 0;
+ for(size_t label = 0; label < blob_matrix_.size(); ++label)
+ for(size_t b = 0; b < blob_matrix_[label].size(); ++b)
+ {
+ blob_matrix_[label][b].id = id++;
+ blob_matrix_[label][b].lid = static_cast<int> (b);
+ }
+ }
+}
+
+int
+pcl::gpu::people::RDFBodyPartsDetector::processRelations ()
+{
+ return buildRelations ( blob_matrix_ );
+}
+
+int
+pcl::gpu::people::RDFBodyPartsDetector::processRelations (PersonAttribs::Ptr person_attribs)
+{
+ return buildRelations ( blob_matrix_, person_attribs );
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * @author: Koen Buys, Anatoly Baksheev
+ */
+
+#include <pcl/gpu/people/colormap.h>
+#include <vector>
+#include "internal.h"
+
+using namespace std;
+using namespace pcl::gpu::people;
+
+pcl::RGB pcl::gpu::people::getLColor (unsigned char l)
+{
+ const unsigned char* c = LUT_COLOR_LABEL + 3 * l;
+ pcl::RGB p;
+ p.r = c[0];
+ p.g = c[1];
+ p.b = c[2];
+ return p;
+}
+
+pcl::RGB pcl::gpu::people::getLColor (pcl::Label l)
+{
+ return getLColor(static_cast<unsigned char> (l.label));
+}
+
+void pcl::gpu::people::colorLMap ( int W, int H, const trees::Label* l, unsigned char* c )
+{
+ int numPix = W * H;
+ for(int pi = 0; pi < numPix; ++pi)
+ {
+ const unsigned char* color = LUT_COLOR_LABEL + 3 * l[pi];
+ c[3*pi+0] = color[0];
+ c[3*pi+1] = color[1];
+ c[3*pi+2] = color[2];
+ }
+}
+
+void pcl::gpu::people::colorLMap (const pcl::PointCloud<pcl::Label>& cloud_in, pcl::PointCloud<pcl::RGB>& colormap_out)
+{
+ colormap_out.resize(cloud_in.size());
+ for(size_t i = 0; i < cloud_in.size (); i++)
+ colormap_out.points[i] = getLColor(cloud_in.points[i]);
+
+ colormap_out.width = cloud_in.width;
+ colormap_out.height = cloud_in.height;
+}
+
+void pcl::gpu::people::uploadColorMap(DeviceArray<pcl::RGB>& color_map)
+{
+ // Copy the list of label colors into the devices
+ vector<pcl::RGB> rgba(LUT_COLOR_LABEL_LENGTH);
+ for(int i = 0; i < LUT_COLOR_LABEL_LENGTH; ++i)
+ {
+ // !!!! generate in RGB format, not BGR
+ rgba[i].r = LUT_COLOR_LABEL[i*3 + 2];
+ rgba[i].g = LUT_COLOR_LABEL[i*3 + 1];
+ rgba[i].b = LUT_COLOR_LABEL[i*3 + 0];
+ rgba[i].a = 255;
+ }
+ color_map.upload(rgba);
+}
+
+void pcl::gpu::people::colorizeLabels(const DeviceArray<pcl::RGB>& color_map, const DeviceArray2D<unsigned char>& labels, DeviceArray2D<pcl::RGB>& color_labels)
+{
+ color_labels.create(labels.rows(), labels.cols());
+
+ const DeviceArray<uchar4>& map = (const DeviceArray<uchar4>&)color_map;
+ device::Image& img = (device::Image&)color_labels;
+ device::colorLMap(labels, map, img);
+}
+
+void pcl::gpu::people::colorizeMixedLabels(const DeviceArray<pcl::RGB>& color_map, const DeviceArray2D<unsigned char>& labels,
+ const DeviceArray2D<pcl::RGB>& image, DeviceArray2D<pcl::RGB>& color_labels)
+{
+ color_labels.create(labels.rows(), labels.cols());
+
+ const DeviceArray<uchar4>& map = (const DeviceArray<uchar4>&)color_map;
+ device::Image& img = (device::Image&)color_labels;
+ device::Image& rgba = (device::Image&)image;
+ device::mixedColorMap(labels, map, rgba, img);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+const unsigned char pcl::gpu::people::LUT_COLOR_LABEL[] =
+{
+ 50, 34, 22,
+ 24,247,196,
+ 33,207,189,
+ 254,194,127,
+ 88,115,175,
+ 158, 91, 64,
+ 14, 90, 2,
+ 100,156,227,
+ 243,167, 17,
+ 145,194,184,
+ 234,171,147,
+ 220,112, 93,
+ 93,132,163,
+ 122, 4, 85,
+ 75,168, 46,
+ 15, 5,108,
+ 180,125,107,
+ 157, 77,167,
+ 214, 89, 73,
+ 52,183, 58,
+ 54,155, 75,
+ 249, 61,187,
+ 143, 57, 11,
+ 246,198, 0,
+ 202,177,251,
+ 229,115, 80,
+ 159,185, 1,
+ 186,213,229,
+ 82, 47,144,
+ 140, 69,139,
+ 189,115,117,
+ 80, 57,150
+};
+
+const int pcl::gpu::people::LUT_COLOR_LABEL_LENGTH = sizeof(LUT_COLOR_LABEL)/(sizeof(LUT_COLOR_LABEL[0]) * 3);
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * @authors: Anatoly Baksheev
+ *
+ */
+
+#ifndef PCL_GPU_PEOPLE_CUDA_DEVICE_H_
+#define PCL_GPU_PEOPLE_CUDA_DEVICE_H_
+
+#include <cuda_runtime.h>
+
+namespace pcl
+{
+ namespace device
+ {
+ template<typename Point>
+ __device__ __forceinline__ float sqnorm(const Point& p1, const Point& p2)
+ {
+ float dx = (p1.x - p2.x);
+ float dy = (p1.y - p2.y);
+ float dz = (p1.z - p2.z);
+ return dx * dx + dy * dy + dz * dz;
+ }
+
+ __device__ __forceinline__ float3 computePoint(unsigned short depth, int x, int y, const Intr& intr)
+ {
+ float z = depth * 0.001f; // mm -> meters
+ float3 result;
+
+ result.x = z * (x - intr.cx) / intr.fx;
+ result.y = z * (y - intr.cy) / intr.fy;
+ result.z = z;
+
+ return result;
+ }
+
+ __device__ __forceinline__ bool
+ isFinite(const float3& p)
+ {
+ return isfinite(p.x) && isfinite(p.y) && isfinite(p.z);
+ }
+ }
+}
+
+#endif /* PCL_GPU_PEOPLE_CUDA_DEVICE_H_ */
\ No newline at end of file
--- /dev/null
+#include "internal.h"
+
+#include <pcl/gpu/utils/texture_binder.hpp>
+#include <pcl/gpu/utils/device/block.hpp>
+
+#include <cassert>
+
+///////////////////////////////////////////////////////////////////////////////////////
+/////////////////////////// Constants /////////////////////////////////////////////////
+
+namespace pcl
+{
+ namespace device
+ {
+ struct Edges
+ {
+ enum { UP = 1, DOWN = 2, LEFT = 4, RIGHT = 8, INVALID = 0xF0 };
+
+ unsigned char data;
+
+ __device__ __host__ __forceinline__
+ Edges() : data(0) {};
+ };
+
+ enum
+ {
+ CTA_SIZE_X = 32,
+ CTA_SIZE_Y = 8,
+
+ TILES_PER_BLOCK_X = 1,
+ TILES_PER_BLOCK_Y = 4,
+
+ TPB_X = TILES_PER_BLOCK_X,
+ TPB_Y = TILES_PER_BLOCK_Y,
+
+ TILE_COLS = CTA_SIZE_X * TPB_X,
+ TILE_ROWS = CTA_SIZE_Y * TPB_Y,
+
+ MERGE_CTA_SIZE = 256
+ };
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////
+////////////////////// Edges initialization ///////////////////////////////////////////
+
+namespace pcl
+{
+ namespace device
+ {
+ __global__ void
+ fillInvalidEdges(PtrStepSz<Edges> output)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if( x < output.cols && y < output.rows)
+ output.ptr(y)[x].data = Edges::INVALID;
+ }
+ }
+}
+
+void
+pcl::device::ConnectedComponents::initEdges(int rows, int cols, DeviceArray2D<unsigned char>& edges)
+{
+ int ecols = divUp(cols, TILE_COLS) * TILE_COLS;
+ int erows = divUp(rows, TILE_ROWS) * TILE_ROWS;
+
+ edges.create(erows, ecols);
+
+ dim3 block(32, 8);
+ dim3 grid(divUp(ecols, block.x), divUp(erows, block.y));
+
+ fillInvalidEdges<<<grid, block>>>(edges);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
+
+///////////////////////////////////////////////////////////////////////////////////////
+///////////////////////// Compute Edges //////////////////////////////////////////////
+
+namespace pcl
+{
+ namespace device
+ {
+ template<typename Point>
+ __device__ __forceinline__ float sqnorm(const Point& p1, const Point& p2)
+ {
+ float dx = (p1.x - p2.x);
+ float dy = (p1.y - p2.y);
+ float dz = (p1.z - p2.z);
+ return dx * dx + dy * dy + dz * dz;
+ }
+
+ __device__ __forceinline__ float3 computePoint(unsigned short depth, int x, int y, const Intr& intr)
+ {
+ float z = depth * 0.001f; // mm -> meters
+ float3 result;
+
+ result.x = z * (x - intr.cx) / intr.fx;
+ result.y = z * (y - intr.cy) / intr.fy;
+ result.z = z;
+
+ return result;
+ }
+
+ __global__ void
+ computeEdgesKernel(const PtrStepSz<unsigned char> labels, const PtrStep<unsigned short> depth, const Intr intr,
+ const int num_parts, const float sq_radius, PtrStep<Edges> output)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if( x < labels.cols && y < labels.rows)
+ {
+ unsigned char label = labels.ptr(y)[x];
+
+ if (label < num_parts)
+ {
+ unsigned short d = depth.ptr(y)[x];
+
+ if (d)
+ {
+ float3 point = computePoint(d, x, y, intr);
+ Edges edges;
+
+ if (x > 0)
+ if(labels.ptr(y)[x-1] == label)
+ {
+ float3 pl = computePoint(depth.ptr(y)[x-1], x-1, y, intr);
+ if (sqnorm(pl, point) < sq_radius)
+ edges.data |= Edges::LEFT;
+ }
+
+ if (x < labels.cols - 1)
+ if(labels.ptr(y)[x+1] == label)
+ {
+ float3 pr = computePoint(depth.ptr(y)[x+1], x+1, y, intr);
+ if (sqnorm(pr, point) < sq_radius)
+ edges.data |= Edges::RIGHT;
+ }
+
+ if (y > 0)
+ if(labels.ptr(y-1)[x] == label)
+ {
+ float3 pu = computePoint(depth.ptr(y-1)[x], x, y-1, intr);
+ if(sqnorm(pu, point) < sq_radius)
+ edges.data |= Edges::UP;
+ }
+
+ if (y < labels.rows - 1)
+ if(labels.ptr(y+1)[x] == label)
+ {
+ float3 pd = computePoint(depth.ptr(y+1)[x], x, y+1, intr);
+ if (sqnorm(pd, point) < sq_radius)
+ edges.data |= Edges::DOWN;
+ }
+
+ output.ptr(y)[x] = edges;
+ }
+ } /* if (label < num_parts) */
+ }
+ } /* __global__ */
+ }
+}
+
+void
+pcl::device::ConnectedComponents::computeEdges(const Labels& labels, const Depth& depth, int num_parts, float sq_radius, DeviceArray2D<unsigned char>& edges)
+{
+ device::Intr intr(525.f, 525.f, 319.5, 239.5);
+
+ initEdges(labels.rows(), labels.cols(), edges);
+
+ dim3 block(32, 8);
+ dim3 grid(divUp(labels.cols(), block.x), divUp(labels.rows(), block.y));
+
+ computeEdgesKernel<<<grid, block>>>(labels, depth, intr, num_parts, sq_radius, edges);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
+
+///////////////////////////////////////////////////////////////////////////////////////
+///////////// Connected components - shared memory tiles //////////////////////////////
+
+namespace pcl
+{
+ namespace device
+ {
+ __global__ void
+ smemTilesKernel(const PtrStepSz<unsigned char> edges, PtrStep<int> comps)
+ {
+ int x = threadIdx.x + blockIdx.x * TILE_COLS;
+ int y = threadIdx.y + blockIdx.y * TILE_ROWS;
+
+ if (x >= edges.cols || y >= edges.rows)
+ return;
+
+ __shared__ int labels_arr[TILE_ROWS][TILE_COLS];
+ __shared__ int edges_arr[TILE_ROWS][TILE_COLS];
+
+
+ int new_labels[TPB_Y][TPB_X];
+ int old_labels[TPB_Y][TPB_X];
+
+ // load data and compute indeces
+ #pragma unroll
+ for(int i = 0; i < TPB_Y; ++i)
+ {
+ #pragma unroll
+ for(int j = 0; j < TPB_X; ++j)
+ {
+ int xloc = threadIdx.x+CTA_SIZE_X*j;
+ int yloc = threadIdx.y+CTA_SIZE_Y*i;
+
+ int index = yloc * TILE_COLS + xloc;
+ unsigned char e = edges.ptr(y + CTA_SIZE_Y*i)[x + CTA_SIZE_X*j];
+
+ //disable edges to prevent out of shared memory access for tile
+ //will account such edges on global step
+ if (xloc == 0)
+ e &= ~Edges::LEFT;
+
+ if (yloc == 0)
+ e &= ~Edges::UP;
+
+ if (xloc == CTA_SIZE_X * TPB_X - 1)
+ e &= ~Edges::RIGHT;
+
+ if (yloc == CTA_SIZE_Y * TPB_Y - 1)
+ e &= ~Edges::DOWN;
+
+ new_labels[i][j] = index;
+ edges_arr[yloc][xloc] = e;
+ }
+ }
+
+ // main loop until no changes in shared memory
+ for(int i = 0; ;++i)
+ {
+ //copy data to shared memory and to old array
+ #pragma unroll
+ for(int i = 0; i < TPB_Y; ++i)
+ {
+ #pragma unroll
+ for(int j = 0; j < TPB_X; ++j)
+ {
+ int xloc = threadIdx.x+CTA_SIZE_X*j;
+ int yloc = threadIdx.y+CTA_SIZE_Y*i;
+ old_labels[i][j] = new_labels[i][j];
+ labels_arr[yloc][xloc] = new_labels[i][j];
+ }
+ }
+
+ __syncthreads();
+
+ // test for neighbours
+ #pragma unroll
+ for(int i = 0; i < TPB_Y; ++i)
+ {
+ #pragma unroll
+ for(int j = 0; j < TPB_X; ++j)
+ {
+ int xloc = threadIdx.x+CTA_SIZE_X*j;
+ int yloc = threadIdx.y+CTA_SIZE_Y*i;
+
+ int edges = edges_arr[yloc][xloc];
+ int label = new_labels[i][j];
+
+ if (edges & Edges::UP)
+ label = min(label, labels_arr[yloc-1][xloc]);
+
+ if (edges & Edges::DOWN)
+ label = min(label, labels_arr[yloc+1][xloc]);
+
+ if (edges & Edges::LEFT)
+ label = min(label, labels_arr[yloc][xloc-1]);
+
+ if (edges & Edges::RIGHT)
+ label = min(label, labels_arr[yloc][xloc+1]);
+
+ new_labels[i][j] = label;
+ }
+ }
+
+ __syncthreads();
+
+ int changed = 0;
+ #pragma unroll
+ for(int i = 0; i < TPB_Y; ++i)
+ {
+ #pragma unroll
+ for(int j = 0; j < TPB_X; ++j)
+ {
+ int old_label = old_labels[i][j];
+ int new_label = new_labels[i][j];
+
+ //if there is a neigboring element with a smaller label, update the equivalence tree of the processed element
+ //(the tree is always flattened in this stage so there is no need to use findRoot to find the root)
+ if (new_label < old_label)
+ {
+ changed = 1;
+ atomicMin(&labels_arr[0][0] + old_label, new_label);
+ }
+ }
+ }
+
+ changed = __syncthreads_or(changed);
+ if (!changed)
+ break;
+
+ //flatten the equivalence tree
+ const int *cmem = &labels_arr[0][0];
+ #pragma unroll
+ for(int i = 0; i < TPB_Y; ++i)
+ {
+ #pragma unroll
+ for(int j = 0; j < TPB_X; ++j)
+ {
+ int label = new_labels[i][j];
+
+ while( cmem[label] < label )
+ label = cmem[label];
+
+ new_labels[i][j] = label;
+ }
+
+ }
+ __syncthreads();
+
+ }
+
+ //transfer the label into global coordinates and store to global memory
+ #pragma unroll
+ for(int i = 0; i < TPB_Y; ++i)
+ {
+ #pragma unroll
+ for(int j = 0; j < TPB_X; ++j)
+ {
+ int label = new_labels[i][j];
+
+ int yloc = label / TILE_COLS;
+ int xloc = label - yloc * TILE_COLS;
+
+ xloc += blockIdx.x * TILE_COLS;
+ yloc += blockIdx.y * TILE_ROWS;
+
+ label = yloc * edges.cols + xloc;
+
+ comps.ptr(y + CTA_SIZE_Y*i)[x + CTA_SIZE_X*j] = label;
+ }
+ }
+ }
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////
+///////////////// Connected components - merge tiles //////////////////////////////////
+
+namespace pcl
+{
+ namespace device
+ {
+
+//// Register modifier for pointer-types (for inlining PTX assembly)
+//#if defined(_WIN64) || defined(__LP64__)
+// // 64-bit register modifier for inlined asm
+// #define _ASM_PTR_ "l"
+//#else
+// // 32-bit register modifier for inlined asm
+// #define _ASM_PTR_ "r"
+//#endif
+// struct GlobalLoad
+// {
+// static __device__ __forceinline__ void CG(int &val, int* ptr)
+// {
+// #if (__CUDA_ARCH__ >= 200)
+// asm("ld.global.cg.s32 %0, [%1];" : "=r"(reinterpret_cast<int&>(val)) : _ASM_PTR_(ptr));
+// #else
+// val = *ptr;
+// #endif
+// }
+// };
+//#undef _ASM_PTR_
+
+ __device__ __forceinline__ int findRoot(const PtrStepSz<int>& comps, int label)
+ {
+ for(;;)
+ {
+ int y = label / comps.cols;
+ int x = label - y * comps.cols;
+
+ int parent = comps.ptr(y)[x];
+
+ if (label == parent)
+ break;
+
+ label = parent;
+ }
+ return label;
+ }
+
+ texture<unsigned char, 2, cudaReadModeElementType> edgesTex;
+
+ struct TilesMerge
+ {
+ int tileSizeX;
+ int tileSizeY;
+ int tilesNumX;
+ int tilesNumY;
+
+ mutable PtrStepSz<int> comps;
+
+ __device__ __forceinline__ void
+ unionF(int l1, int l2, bool& changed) const
+ {
+ int r1 = findRoot(comps, l1);
+ int r2 = findRoot(comps, l2);
+
+ if (r1 != r2)
+ {
+ int mi = min(r1, r2);
+ int ma = max(r1, r2);
+
+ int y = ma / comps.cols;
+ int x = ma - y * comps.cols;
+ atomicMin(&comps.ptr(y)[x], mi);
+ changed = true;
+ }
+ }
+
+ __device__ __forceinline__ void
+ operator()() const
+ {
+ int tid = Block::flattenedThreadId();
+ int stride = Block::stride();
+
+ int xbeg = blockIdx.x * tilesNumX * tileSizeX;
+ int xend = xbeg + tilesNumX * tileSizeX;
+
+ int ybeg = blockIdx.y * tilesNumY * tileSizeY;
+ int yend = ybeg + tilesNumY * tileSizeY;
+
+ int tasksH = (tilesNumY - 1) * (xend - xbeg);
+ int tasksV = (tilesNumX - 1) * (yend - ybeg);
+
+ int total = tasksH + tasksV;
+
+ bool changed;
+ do
+ {
+ changed = false;
+
+ for( int task_index = tid; task_index < total; task_index += stride)
+ if (task_index < tasksH)
+ {
+ int indexH = task_index;
+
+ int row = indexH / (xend - xbeg);
+ int col = indexH - row * (xend - xbeg);
+
+ int y = ybeg + (row + 1) * tileSizeY;
+ int x = xbeg + col;
+
+ int e = tex2D(edgesTex, x, y);
+ if (e & Edges::UP)
+ {
+ int lc = comps.ptr(y )[x];
+ int lu = comps.ptr(y-1)[x];
+ unionF(lc, lu, changed);
+ }
+ }
+ else
+ {
+ int indexV = task_index - tasksH;
+
+ int col = indexV / (yend - ybeg);
+ int row = indexV - col * (yend - ybeg);
+
+ int x = xbeg + (col + 1) * tileSizeX;
+ int y = ybeg + row;
+
+ int e = tex2D(edgesTex, x, y);
+ if (e & Edges::LEFT)
+ {
+ int lc = comps.ptr(y)[x ];
+ int ll = comps.ptr(y)[x-1];
+ unionF(lc, ll, changed);
+ }
+ }
+ }
+ while(__syncthreads_or(changed));
+ }
+
+ };
+
+ __global__ void mergeKernel(const TilesMerge tm) { tm(); }
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////
+//////////////// Connected components - flatten trees /////////////////////////////////
+
+namespace pcl
+{
+ namespace device
+ {
+ __global__ void flattenTreesKernel(PtrStepSz<int> comps, const PtrStep<Edges> edges)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if( x < comps.cols && y < comps.rows)
+ {
+ int invalid = edges.ptr(y)[x].data & Edges::INVALID;
+ comps.ptr(y)[x] = invalid ? -1 : findRoot(comps, comps.ptr(y)[x]);
+ }
+ }
+ }
+}
+
+
+void pcl::device::ConnectedComponents::labelComponents(const DeviceArray2D<unsigned char>& edges, DeviceArray2D<int>& comps)
+{
+ comps.create(edges.rows(), edges.cols());
+
+ dim3 block(CTA_SIZE_X, CTA_SIZE_Y);
+ dim3 grid(divUp(edges.cols(), TILE_COLS), divUp(edges.rows(), TILE_ROWS));
+
+ smemTilesKernel<<<grid, block>>>(edges, comps);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+
+ TextureBinder binder(edges, edgesTex);
+
+ // the code below assumes such resolution
+ assert(edges.cols() == 640 && edges.rows() == 480);
+
+ TilesMerge tm;
+ tm.comps = comps;
+
+ //merge 4x3 -> 5x5 grid
+ tm.tileSizeX = TILE_COLS;
+ tm.tileSizeY = TILE_ROWS;
+ tm.tilesNumX = 4;
+ tm.tilesNumY = 3;
+
+ grid.x = edges.cols()/(tm.tileSizeX * tm.tilesNumX);
+ grid.y = edges.rows()/(tm.tileSizeY * tm.tilesNumY);
+
+ mergeKernel<<<grid, 768>>>(tm);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+
+ //merge 5x5 -> 1x1 grid
+ tm.tileSizeX = TILE_COLS * tm.tilesNumX;
+ tm.tileSizeY = TILE_ROWS * tm.tilesNumY;
+ tm.tilesNumX = 5;
+ tm.tilesNumY = 5;
+
+ grid.x = edges.cols()/(tm.tileSizeX * tm.tilesNumX);
+ grid.y = edges.rows()/(tm.tileSizeY * tm.tilesNumY);
+
+ mergeKernel<<<grid, 1024>>>(tm);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+
+ grid.x = divUp(edges.cols(), block.x);
+ grid.y = divUp(edges.rows(), block.y);
+ flattenTreesKernel<<<grid, block>>>(comps, edges);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * @authors: Cedric Cagniart, Koen Buys, Anatoly Baksheev
+ *
+ */
+
+#include <pcl/gpu/people/tree.h>
+#include <pcl/gpu/people/label_common.h>
+#include <pcl/gpu/utils/device/limits.hpp>
+#include <pcl/gpu/utils/safe_call.hpp>
+#include <pcl/gpu/utils/texture_binder.hpp>
+#include <stdio.h>
+#include <limits>
+#include <assert.h>
+#include "internal.h"
+
+using pcl::gpu::people::trees::Node;
+using pcl::gpu::people::trees::Label;
+using pcl::gpu::people::trees::AttribLocation;
+using pcl::gpu::people::trees::Attrib;
+using pcl::gpu::people::trees::focal;
+using pcl::gpu::people::trees::NUM_LABELS;
+
+using namespace std;
+typedef unsigned int uint;
+
+#ifdef __CDT_PARSER__ // This is an eclipse specific hack, does nothing to the code
+#define __global__
+#define __device__
+#define __shared__
+#define __forceinline__
+#define __constant__
+#endif
+
+namespace pcl
+{
+ namespace device
+ {
+ texture<unsigned short, 2, cudaReadModeElementType> depthTex;
+ texture<char4, 2, cudaReadModeElementType> multilabelTex;
+
+ __constant__ int constFGThresh;
+
+ template<bool testFG> __device__ __forceinline__ Label
+ evaluateTree(int u, int v, float f, int treeHeight, int numNodes, const Node* nodes, const Label* leaves)
+ {
+ int depth = tex2D(depthTex, u, v);
+
+ float scale = f / depth;
+
+ // go down the tree
+ int nid = 0;
+ for(int nodeDepth = 0; nodeDepth < treeHeight; ++nodeDepth)
+ {
+ const Node node = nodes[nid];
+
+ const AttribLocation& loc = node.loc;
+ int d1 = tex2D (depthTex, u + loc.du1 * scale, v + loc.dv1 * scale);
+ int d2 = tex2D (depthTex, u + loc.du2 * scale, v + loc.dv2 * scale);
+
+ if (testFG)
+ {
+ if( d1 - depth > constFGThresh )
+ d1 = numeric_limits<short>::max();
+
+ if( d2 - depth > constFGThresh )
+ d2 = numeric_limits<short>::max();
+ }
+
+ int delta = d1-d2;
+ bool test = delta > (int)node.thresh;
+ if( test ) nid = nid*2+2;
+ else nid = nid*2+1;
+ }
+ return leaves[nid-numNodes];
+ }
+
+ /** \brief This is the CUDA kernel doing the actual RDF evaluation */
+ __global__ void
+ KernelCUDA_runTree( const float f,
+ const int treeHeight,
+ const int numNodes,
+ const Node* nodes,
+ const Label* leaves,
+ PtrStepSz<Label> labels)
+ {
+ int u = blockIdx.x * blockDim.x + threadIdx.x;
+ int v = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if( u < labels.cols && v < labels.rows)
+ labels.ptr(v)[u] = evaluateTree<false>(u, v, f, treeHeight, numNodes, nodes, leaves);
+ }
+
+ template<bool testFG> __global__ void
+ KernelCUDA_MultiTreePass( const int treeId,
+ const float f,
+ const int treeHeight,
+ const int numNodes,
+ const Node* nodes,
+ const Label* leaves,
+ PtrStepSz<unsigned short> depth,
+ PtrStepSz<char4> multiLabels)
+ {
+ int u = blockIdx.x * blockDim.x + threadIdx.x;
+ int v = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if(u < multiLabels.cols && v < multiLabels.rows)
+ {
+ // This maps a char4 pointer on a char pointer
+ char* pixel = (char*)&multiLabels.ptr(v)[u];
+ // This test assures that in next iterations the FGPreperation is taking into account see utils.cu
+ if(depth.ptr(v)[u] == numeric_limits<unsigned short>::max())
+ pixel[treeId] = 29; // see label_common.h for Background label (=29)
+ // TODO remove this hardcoded label with enum part_t label
+ else
+ pixel[treeId] = evaluateTree<testFG>(u, v, f, treeHeight, numNodes, nodes, leaves);
+ }
+ }
+
+ /** \brief This function wraps the actual CUDA kernel doing the RDF evaluation */
+ void CUDA_runTree ( float focal, int treeHeight, int numNodes, const Node* nodes, const Label* leaves, const Depth& depth, Labels& labels )
+ {
+ labels.create( depth.rows(), depth.cols() );
+
+ depthTex.addressMode[0] = cudaAddressModeClamp;
+ TextureBinder binder(depth, depthTex);
+
+ dim3 block(32, 8);
+ dim3 grid(divUp(depth.cols(), block.x), divUp(depth.rows(), block.y) );
+
+ KernelCUDA_runTree<<< grid, block >>>( focal, treeHeight, numNodes, nodes, leaves, labels);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaThreadSynchronize() );
+ }
+
+ void CUDA_runMultiTreePass ( int FGThresh,
+ int treeId,
+ float focal,
+ int treeHeight,
+ int numNodes,
+ const Node* nodes_device,
+ const Label* leaves_device,
+ const Depth& depth,
+ MultiLabels& multilabel )
+ {
+ //std::cout << "(I) : CUDA_runMultiTreePass() called" << std::endl;
+ depthTex.addressMode[0] = cudaAddressModeClamp;
+ TextureBinder binder(depth, depthTex);
+
+ dim3 block(32, 8);
+ dim3 grid( divUp(depth.cols(), block.x), divUp(depth.rows(), block.y) );
+
+ if(FGThresh == std::numeric_limits<int>::max())
+ {
+ KernelCUDA_MultiTreePass<false><<< grid, block >>>( treeId, focal, treeHeight,
+ numNodes, nodes_device, leaves_device, depth, multilabel);
+ }
+ else
+ {
+ cudaSafeCall( cudaMemcpyToSymbol(constFGThresh, &FGThresh, sizeof(FGThresh)) );
+
+ KernelCUDA_MultiTreePass<true><<< grid, block >>>( treeId, focal, treeHeight,
+ numNodes, nodes_device, leaves_device, depth, multilabel);
+ }
+
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaThreadSynchronize() );
+ }
+
+ ///////////////////////////////////////////////////////////////////////////////////////
+
+ __device__ int findMaxId( int numBins, char* bins )
+ {
+ // HACK .. not testing against numBins = 0
+ int maxId = 0;
+ char maxVal = bins[0];
+ for(int i=1;i<numBins;++i)
+ {
+ char val = bins[i];
+ if( val > maxVal ) { maxId = i; maxVal = val; }
+ }
+ return maxId;
+ }
+
+ //this will find the max Index but return -1 if there is a tie
+ __device__ int findMaxId_testTie(int numBins, char* bins)
+ {
+ int maxId = 0;
+ int maxId_other = -1;
+ char maxVal = bins[0];
+
+ for(int i=1;i<numBins;++i) {
+ char val = bins[i];
+ if( val == maxVal ) { maxId_other = i; }
+ if( val > maxVal ) { maxId = i; maxId_other = -1; maxVal = val; }
+ }
+
+ if( maxId_other != -1) return -1;
+ else return maxId;
+ }
+
+ __global__ void KernelCUDA_MultiTreeMerge( const int numTrees, PtrStepSz<Label> labels )
+ {
+ int u = blockIdx.x * blockDim.x + threadIdx.x;
+ int v = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if( u >= labels.cols || v >= labels.rows)
+ return;
+
+ // reset the bins
+ char bins[NUM_LABELS];
+ for(int li = 0; li < NUM_LABELS; ++li)
+ bins[li] = 0;
+
+ // find a consensus with the current trees
+ {
+ char4 pixlabels = tex2D(multilabelTex, u ,v);
+ char* bob = (char*)&pixlabels; //horrible but char4's have xyzw members
+ for(int ti = 0; ti < numTrees; ++ti)
+ bins[ bob[ti] ]++;
+ }
+
+ int res = findMaxId_testTie(NUM_LABELS, bins);
+
+ // if this fails... find a consensus in a 1 neighbourhood
+ if( res < 0 )
+ {
+ int depth = tex2D(depthTex, u,v);
+ for(int i = -1 ; i <= 1; ++i)
+ {
+ for(int j = -1; j <= 1; ++j)
+ {
+ int depth_neighbor = tex2D(depthTex,u+i,v+j);
+ char4 labels_neighbor = tex2D(multilabelTex, u+i,v+j);
+ char* bob = (char*)&labels_neighbor; //horrible but char4's have xyzw members
+ //TODO: redo this part
+ int weight = abs(depth-depth_neighbor) < 50 ? 1:0; // 5cms
+ for(int ti = 0; ti < numTrees; ++ti)
+ bins[ bob[ti] ] += weight;
+ }
+ }
+ res = findMaxId( NUM_LABELS, bins );
+ }
+ labels.ptr(v)[u] = res;
+ }
+
+ /** \brief This merges the labels from all trees into a histogram of probabilities **/
+ __global__ void KernelCUDA_MultiTreeCreateProb (const int numTrees, PtrStepSz<prob_histogram> prob)
+ {
+ // map block and thread onto image coordinates
+ int u = blockIdx.x * blockDim.x + threadIdx.x;
+ int v = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if( u >= prob.cols || v >= prob.rows )
+ return;
+
+ char4 pixlabels = tex2D (multilabelTex, u ,v);
+ char* bob = (char*)&pixlabels; //horrible but char4's have xyzw members
+
+ // Reset prob first, this should become NUM_LABELS
+ for(int in = 0; in < NUM_LABELS; in++)
+ {
+ prob.ptr(v)[u].probs[in] = 0;
+ }
+
+ for(int ti = 0; ti < numTrees; ++ti)
+ {
+ // Each tree casts a vote to the probability
+ // TODO: replace this with a histogram copy
+ prob.ptr(v)[u].probs[bob[ti]] += 0.25;
+ }
+ }
+
+ /** \brief This will merge the votes from the different trees into one final vote */
+ void CUDA_runMultiTreeMerge( int numTrees, const Depth& depth, const MultiLabels& multilabel, Labels& labels)
+ {
+ //std::cout << "(I) : CUDA_runMultiTreeMerge() called" << std::endl;
+ labels.create(depth.rows(), depth.cols());
+
+ depthTex.addressMode[0] = cudaAddressModeClamp;
+ TextureBinder binder(depth, depthTex);
+
+ multilabelTex.addressMode[0] = cudaAddressModeClamp;
+ TextureBinder mlabels_binder(multilabel, multilabelTex);
+
+ dim3 block(32, 8);
+ dim3 grid(divUp(depth.cols(), block.x), divUp(depth.rows(), block.y) );
+
+ KernelCUDA_MultiTreeMerge<<< grid, block >>>( numTrees, labels );
+
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaThreadSynchronize() );
+ }
+
+ /** \brief This will merge the votes from the different trees into one final vote, including probabilistic's */
+ void CUDA_runMultiTreeProb ( int numTrees,
+ const Depth& depth,
+ const MultiLabels& multilabel,
+ Labels& labels,
+ LabelProbability& probabilities)
+ {
+ std::cout << "(I) : CUDA_runMultiTreeProb() called" << std::endl;
+
+ //labels.create(depth.rows(), depth.cols());
+ //depthTex.addressMode[0] = cudaAddressModeClamp;
+ //TextureBinder binder(depth, depthTex);
+
+ multilabelTex.addressMode[0] = cudaAddressModeClamp;
+ TextureBinder mlabels_binder(multilabel, multilabelTex);
+
+ dim3 block(32, 8);
+ dim3 grid(divUp(depth.cols(), block.x), divUp(depth.rows(), block.y) );
+
+ KernelCUDA_MultiTreeCreateProb<<< grid, block >>>( numTrees, probabilities);
+
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaThreadSynchronize() );
+ }
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+pcl::device::CUDATree::CUDATree (int treeHeight_arg, const vector<Node>& nodes, const vector<Label>& leaves)
+{
+ treeHeight = treeHeight_arg;
+ numNodes = (1 << treeHeight) - 1;
+ assert (static_cast<int> (nodes.size ()) == numNodes );
+ assert (static_cast<int> (leaves.size ()) == (1 << treeHeight) );
+
+ nodes_device.upload(nodes);
+ leaves_device.upload(leaves);
+}
+
+void
+pcl::device::MultiTreeLiveProc::process (const Depth& dmap, Labels& lmap)
+{
+ // TODO: is this assert needed if we only call process?
+ //assert(!trees.empty());
+
+ // TODO is this iteration needed when we call multitreepass in the process step?
+ /* if (trees.size() == 1)
+ {
+ const CUDATree& t = trees[0];
+ CUDA_runTree( focal, t.treeHeight, t.numNodes, t.nodes_device, t.leaves_device, dmap, lmap );
+ return;
+ }
+ */
+ process(dmap, lmap, std::numeric_limits<int>::max());
+}
+
+void
+pcl::device::MultiTreeLiveProc::process (const Depth& dmap, Labels& lmap, int FGThresh)
+{
+ assert(!trees.empty());
+
+ unsigned int numTrees = static_cast<int> (trees.size ());
+
+ multilmap.create(dmap.rows(), dmap.cols());
+
+ // 1 - run the multi passes
+ for( int ti = 0; ti < numTrees; ++ti )
+ {
+ const CUDATree& t = trees[ti];
+
+ CUDA_runMultiTreePass ( FGThresh, ti, static_cast<float> (focal), t.treeHeight, t.numNodes, t.nodes_device, t.leaves_device, dmap, multilmap );
+ }
+ // 2 - run the merging
+ assert( numTrees <= 4 );
+
+ device::CUDA_runMultiTreeMerge(numTrees, dmap, multilmap, lmap);
+}
+
+void
+pcl::device::MultiTreeLiveProc::processProb (const Depth& dmap, Labels& lmap, LabelProbability& prob, int FGThresh)
+{
+ assert(!trees.empty());
+
+ unsigned int numTrees = static_cast<unsigned int> (trees.size ());
+ assert( numTrees <= 4 );
+
+ multilmap.create(dmap.rows(), dmap.cols());
+
+ // 1 - run the multi passes
+ for( int ti = 0; ti < numTrees; ++ti )
+ {
+ const CUDATree& t = trees[ti];
+ CUDA_runMultiTreePass ( FGThresh, ti, static_cast<float> (focal), t.treeHeight, t.numNodes, t.nodes_device, t.leaves_device, dmap, multilmap );
+ }
+
+ device::CUDA_runMultiTreeProb(numTrees, dmap, multilmap, lmap, prob);
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (C) 2009-2010, NVIDIA Corporation, all rights reserved.
+ * Third party copyrights are property of their respective owners.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * Ported to PCL by Koen Buys : Attention Work in progress!
+ */
+
+#include <iostream>
+#include <string>
+#include <vector>
+#include <algorithm>
+#include "NCV.hpp"
+
+using namespace std;
+
+
+//==============================================================================
+//
+// Error handling helpers
+//
+//==============================================================================
+
+
+static void stdDebugOutput(const string &msg)
+{
+ cout << msg;
+}
+
+
+static NCVDebugOutputHandler *debugOutputHandler = stdDebugOutput;
+
+
+void ncvDebugOutput(const string &msg)
+{
+ debugOutputHandler(msg);
+}
+
+
+void ncvSetDebugOutputHandler(NCVDebugOutputHandler *func)
+{
+ debugOutputHandler = func;
+}
+
+
+//==============================================================================
+//
+// Memory wrappers and helpers
+//
+//==============================================================================
+
+
+Ncv32u alignUp(Ncv32u what, Ncv32u alignment)
+{
+ Ncv32u alignMask = alignment-1;
+ Ncv32u inverseAlignMask = ~alignMask;
+ Ncv32u res = (what + alignMask) & inverseAlignMask;
+ return res;
+}
+
+
+void NCVMemPtr::clear()
+{
+ ptr = NULL;
+ memtype = NCVMemoryTypeNone;
+}
+
+
+void NCVMemSegment::clear()
+{
+ begin.clear();
+ size = 0;
+}
+
+
+NCVStatus memSegCopyHelper(void *dst, NCVMemoryType dstType, const void *src, NCVMemoryType srcType, size_t sz, cudaStream_t cuStream)
+{
+ NCVStatus ncvStat;
+ switch (dstType)
+ {
+ case NCVMemoryTypeHostPageable:
+ case NCVMemoryTypeHostPinned:
+ switch (srcType)
+ {
+ case NCVMemoryTypeHostPageable:
+ case NCVMemoryTypeHostPinned:
+ memcpy(dst, src, sz);
+ ncvStat = NCV_SUCCESS;
+ break;
+ case NCVMemoryTypeDevice:
+ if (cuStream != 0)
+ {
+ ncvAssertCUDAReturn(cudaMemcpyAsync(dst, src, sz, cudaMemcpyDeviceToHost, cuStream), NCV_CUDA_ERROR);
+ }
+ else
+ {
+ ncvAssertCUDAReturn(cudaMemcpy(dst, src, sz, cudaMemcpyDeviceToHost), NCV_CUDA_ERROR);
+ }
+ ncvStat = NCV_SUCCESS;
+ break;
+ default:
+ ncvStat = NCV_MEM_RESIDENCE_ERROR;
+ }
+ break;
+ case NCVMemoryTypeDevice:
+ switch (srcType)
+ {
+ case NCVMemoryTypeHostPageable:
+ case NCVMemoryTypeHostPinned:
+ if (cuStream != 0)
+ {
+ ncvAssertCUDAReturn(cudaMemcpyAsync(dst, src, sz, cudaMemcpyHostToDevice, cuStream), NCV_CUDA_ERROR);
+ }
+ else
+ {
+ ncvAssertCUDAReturn(cudaMemcpy(dst, src, sz, cudaMemcpyHostToDevice), NCV_CUDA_ERROR);
+ }
+ ncvStat = NCV_SUCCESS;
+ break;
+ case NCVMemoryTypeDevice:
+ if (cuStream != 0)
+ {
+ ncvAssertCUDAReturn(cudaMemcpyAsync(dst, src, sz, cudaMemcpyDeviceToDevice, cuStream), NCV_CUDA_ERROR);
+ }
+ else
+ {
+ ncvAssertCUDAReturn(cudaMemcpy(dst, src, sz, cudaMemcpyDeviceToDevice), NCV_CUDA_ERROR);
+ }
+ ncvStat = NCV_SUCCESS;
+ break;
+ default:
+ ncvStat = NCV_MEM_RESIDENCE_ERROR;
+ }
+ break;
+ default:
+ ncvStat = NCV_MEM_RESIDENCE_ERROR;
+ }
+
+ return ncvStat;
+}
+
+
+NCVStatus memSegCopyHelper2D(void *dst, Ncv32u dstPitch, NCVMemoryType dstType,
+ const void *src, Ncv32u srcPitch, NCVMemoryType srcType,
+ Ncv32u widthbytes, Ncv32u height, cudaStream_t cuStream)
+{
+ NCVStatus ncvStat;
+ switch (dstType)
+ {
+ case NCVMemoryTypeHostPageable:
+ case NCVMemoryTypeHostPinned:
+ switch (srcType)
+ {
+ case NCVMemoryTypeHostPageable:
+ case NCVMemoryTypeHostPinned:
+ for (Ncv32u i=0; i<height; i++)
+ {
+ memcpy((char*)dst + i * dstPitch, (char*)src + i * srcPitch, widthbytes);
+ }
+ ncvStat = NCV_SUCCESS;
+ break;
+ case NCVMemoryTypeDevice:
+ if (cuStream != 0)
+ {
+ ncvAssertCUDAReturn(cudaMemcpy2DAsync(dst, dstPitch, src, srcPitch, widthbytes, height, cudaMemcpyDeviceToHost, cuStream), NCV_CUDA_ERROR);
+ }
+ else
+ {
+ ncvAssertCUDAReturn(cudaMemcpy2D(dst, dstPitch, src, srcPitch, widthbytes, height, cudaMemcpyDeviceToHost), NCV_CUDA_ERROR);
+ }
+ ncvStat = NCV_SUCCESS;
+ break;
+ default:
+ ncvStat = NCV_MEM_RESIDENCE_ERROR;
+ }
+ break;
+ case NCVMemoryTypeDevice:
+ switch (srcType)
+ {
+ case NCVMemoryTypeHostPageable:
+ case NCVMemoryTypeHostPinned:
+ if (cuStream != 0)
+ {
+ ncvAssertCUDAReturn(cudaMemcpy2DAsync(dst, dstPitch, src, srcPitch, widthbytes, height, cudaMemcpyHostToDevice, cuStream), NCV_CUDA_ERROR);
+ }
+ else
+ {
+ ncvAssertCUDAReturn(cudaMemcpy2D(dst, dstPitch, src, srcPitch, widthbytes, height, cudaMemcpyHostToDevice), NCV_CUDA_ERROR);
+ }
+ ncvStat = NCV_SUCCESS;
+ break;
+ case NCVMemoryTypeDevice:
+ if (cuStream != 0)
+ {
+ ncvAssertCUDAReturn(cudaMemcpy2DAsync(dst, dstPitch, src, srcPitch, widthbytes, height, cudaMemcpyDeviceToDevice, cuStream), NCV_CUDA_ERROR);
+ }
+ else
+ {
+ ncvAssertCUDAReturn(cudaMemcpy2D(dst, dstPitch, src, srcPitch, widthbytes, height, cudaMemcpyDeviceToDevice), NCV_CUDA_ERROR);
+ }
+ ncvStat = NCV_SUCCESS;
+ break;
+ default:
+ ncvStat = NCV_MEM_RESIDENCE_ERROR;
+ }
+ break;
+ default:
+ ncvStat = NCV_MEM_RESIDENCE_ERROR;
+ }
+
+ return ncvStat;
+}
+
+
+//===================================================================
+//
+// NCVMemStackAllocator class members implementation
+//
+//===================================================================
+
+
+NCVMemStackAllocator::NCVMemStackAllocator(Ncv32u alignment)
+ :
+ currentSize(0),
+ _maxSize(0),
+ allocBegin(NULL),
+ begin(NULL),
+ end(NULL),
+ _memType(NCVMemoryTypeNone),
+ _alignment(alignment),
+ bReusesMemory(false)
+{
+ NcvBool bProperAlignment = (alignment & (alignment-1)) == 0;
+ ncvAssertPrintCheck(bProperAlignment, "NCVMemStackAllocator ctor:: alignment not power of 2");
+}
+
+
+NCVMemStackAllocator::NCVMemStackAllocator(NCVMemoryType memT, size_t capacity, Ncv32u alignment, void *reusePtr)
+ :
+ currentSize(0),
+ _maxSize(0),
+ allocBegin(NULL),
+ _memType(memT),
+ _alignment(alignment)
+{
+ NcvBool bProperAlignment = (alignment & (alignment-1)) == 0;
+ ncvAssertPrintCheck(bProperAlignment, "NCVMemStackAllocator ctor:: _alignment not power of 2");
+ ncvAssertPrintCheck(memT != NCVMemoryTypeNone, "NCVMemStackAllocator ctor:: Incorrect allocator type");
+
+ allocBegin = NULL;
+
+ if (reusePtr == NULL && capacity != 0)
+ {
+ bReusesMemory = false;
+ switch (memT)
+ {
+ case NCVMemoryTypeDevice:
+ ncvAssertCUDAReturn(cudaMalloc(&allocBegin, capacity), );
+ break;
+ case NCVMemoryTypeHostPinned:
+ ncvAssertCUDAReturn(cudaMallocHost(&allocBegin, capacity), );
+ break;
+ case NCVMemoryTypeHostPageable:
+ allocBegin = (Ncv8u *)malloc(capacity);
+ break;
+ default:;
+ }
+ }
+ else
+ {
+ bReusesMemory = true;
+ allocBegin = (Ncv8u *)reusePtr;
+ }
+
+ if (capacity == 0)
+ {
+ allocBegin = (Ncv8u *)(0x1);
+ }
+
+ if (!isCounting())
+ {
+ begin = allocBegin;
+ end = begin + capacity;
+ }
+}
+
+
+NCVMemStackAllocator::~NCVMemStackAllocator()
+{
+ if (allocBegin != NULL)
+ {
+ ncvAssertPrintCheck(currentSize == 0, "NCVMemStackAllocator dtor:: not all objects were deallocated properly, forcing destruction");
+
+ if (!bReusesMemory && (allocBegin != (Ncv8u *)(0x1)))
+ {
+ switch (_memType)
+ {
+ case NCVMemoryTypeDevice:
+ ncvAssertCUDAReturn(cudaFree(allocBegin), );
+ break;
+ case NCVMemoryTypeHostPinned:
+ ncvAssertCUDAReturn(cudaFreeHost(allocBegin), );
+ break;
+ case NCVMemoryTypeHostPageable:
+ free(allocBegin);
+ break;
+ default:;
+ }
+ }
+
+ allocBegin = NULL;
+ }
+}
+
+
+NCVStatus NCVMemStackAllocator::alloc(NCVMemSegment &seg, size_t size)
+{
+ seg.clear();
+ ncvAssertReturn(isInitialized(), NCV_ALLOCATOR_BAD_ALLOC);
+
+ size = alignUp(size, this->_alignment);
+ this->currentSize += size;
+ this->_maxSize = std::max(this->_maxSize, this->currentSize);
+
+ if (!isCounting())
+ {
+ size_t availSize = end - begin;
+ ncvAssertReturn(size <= availSize, NCV_ALLOCATOR_INSUFFICIENT_CAPACITY);
+ }
+
+ seg.begin.ptr = begin;
+ seg.begin.memtype = this->_memType;
+ seg.size = size;
+ begin += size;
+
+ return NCV_SUCCESS;
+}
+
+
+NCVStatus NCVMemStackAllocator::dealloc(NCVMemSegment &seg)
+{
+ ncvAssertReturn(isInitialized(), NCV_ALLOCATOR_BAD_ALLOC);
+ ncvAssertReturn(seg.begin.memtype == this->_memType, NCV_ALLOCATOR_BAD_DEALLOC);
+ ncvAssertReturn(seg.begin.ptr != NULL || isCounting(), NCV_ALLOCATOR_BAD_DEALLOC);
+ ncvAssertReturn(seg.begin.ptr == begin - seg.size, NCV_ALLOCATOR_DEALLOC_ORDER);
+
+ currentSize -= seg.size;
+ begin -= seg.size;
+
+ seg.clear();
+
+ ncvAssertReturn(allocBegin <= begin, NCV_ALLOCATOR_BAD_DEALLOC);
+
+ return NCV_SUCCESS;
+}
+
+
+NcvBool NCVMemStackAllocator::isInitialized(void) const
+{
+ return ((this->_alignment & (this->_alignment-1)) == 0) && isCounting() || this->allocBegin != NULL;
+}
+
+
+NcvBool NCVMemStackAllocator::isCounting(void) const
+{
+ return this->_memType == NCVMemoryTypeNone;
+}
+
+
+NCVMemoryType NCVMemStackAllocator::memType(void) const
+{
+ return this->_memType;
+}
+
+
+Ncv32u NCVMemStackAllocator::alignment(void) const
+{
+ return this->_alignment;
+}
+
+
+size_t NCVMemStackAllocator::maxSize(void) const
+{
+ return this->_maxSize;
+}
+
+
+//===================================================================
+//
+// NCVMemNativeAllocator class members implementation
+//
+//===================================================================
+
+
+NCVMemNativeAllocator::NCVMemNativeAllocator(NCVMemoryType memT, Ncv32u alignment)
+ :
+ currentSize(0),
+ _maxSize(0),
+ _memType(memT),
+ _alignment(alignment)
+{
+ ncvAssertPrintReturn(memT != NCVMemoryTypeNone, "NCVMemNativeAllocator ctor:: counting not permitted for this allocator type", );
+}
+
+
+NCVMemNativeAllocator::~NCVMemNativeAllocator()
+{
+ ncvAssertPrintCheck(currentSize == 0, "NCVMemNativeAllocator dtor:: detected memory leak");
+}
+
+
+NCVStatus NCVMemNativeAllocator::alloc(NCVMemSegment &seg, size_t size)
+{
+ seg.clear();
+ ncvAssertReturn(isInitialized(), NCV_ALLOCATOR_BAD_ALLOC);
+
+ switch (this->_memType)
+ {
+ case NCVMemoryTypeDevice:
+ ncvAssertCUDAReturn(cudaMalloc(&seg.begin.ptr, size), NCV_CUDA_ERROR);
+ break;
+ case NCVMemoryTypeHostPinned:
+ ncvAssertCUDAReturn(cudaMallocHost(&seg.begin.ptr, size), NCV_CUDA_ERROR);
+ break;
+ case NCVMemoryTypeHostPageable:
+ seg.begin.ptr = (Ncv8u *)malloc(size);
+ break;
+ default:;
+ }
+
+ this->currentSize += alignUp(size, this->_alignment);
+ this->_maxSize = std::max(this->_maxSize, this->currentSize);
+
+ seg.begin.memtype = this->_memType;
+ seg.size = size;
+
+ return NCV_SUCCESS;
+}
+
+
+NCVStatus NCVMemNativeAllocator::dealloc(NCVMemSegment &seg)
+{
+ ncvAssertReturn(isInitialized(), NCV_ALLOCATOR_BAD_ALLOC);
+ ncvAssertReturn(seg.begin.memtype == this->_memType, NCV_ALLOCATOR_BAD_DEALLOC);
+ ncvAssertReturn(seg.begin.ptr != NULL, NCV_ALLOCATOR_BAD_DEALLOC);
+
+ ncvAssertReturn(currentSize >= alignUp(seg.size, this->_alignment), NCV_ALLOCATOR_BAD_DEALLOC);
+ currentSize -= alignUp(seg.size, this->_alignment);
+
+ switch (this->_memType)
+ {
+ case NCVMemoryTypeDevice:
+ ncvAssertCUDAReturn(cudaFree(seg.begin.ptr), NCV_CUDA_ERROR);
+ break;
+ case NCVMemoryTypeHostPinned:
+ ncvAssertCUDAReturn(cudaFreeHost(seg.begin.ptr), NCV_CUDA_ERROR);
+ break;
+ case NCVMemoryTypeHostPageable:
+ free(seg.begin.ptr);
+ break;
+ default:;
+ }
+
+ seg.clear();
+
+ return NCV_SUCCESS;
+}
+
+
+NcvBool NCVMemNativeAllocator::isInitialized(void) const
+{
+ return (this->_alignment != 0);
+}
+
+
+NcvBool NCVMemNativeAllocator::isCounting(void) const
+{
+ return false;
+}
+
+
+NCVMemoryType NCVMemNativeAllocator::memType(void) const
+{
+ return this->_memType;
+}
+
+
+Ncv32u NCVMemNativeAllocator::alignment(void) const
+{
+ return this->_alignment;
+}
+
+
+size_t NCVMemNativeAllocator::maxSize(void) const
+{
+ return this->_maxSize;
+}
+
+
+//===================================================================
+//
+// Time and timer routines
+//
+//===================================================================
+
+
+typedef struct _NcvTimeMoment NcvTimeMoment;
+
+#if defined(_WIN32) || defined(_WIN64)
+
+ #include <Windows.h>
+
+ typedef struct _NcvTimeMoment
+ {
+ LONGLONG moment, freq;
+ } NcvTimeMoment;
+
+
+ static void _ncvQueryMoment(NcvTimeMoment *t)
+ {
+ QueryPerformanceFrequency((LARGE_INTEGER *)&(t->freq));
+ QueryPerformanceCounter((LARGE_INTEGER *)&(t->moment));
+ }
+
+
+ double _ncvMomentToMicroseconds(NcvTimeMoment *t)
+ {
+ return 1000000.0 * t->moment / t->freq;
+ }
+
+
+ double _ncvMomentsDiffToMicroseconds(NcvTimeMoment *t1, NcvTimeMoment *t2)
+ {
+ return 1000000.0 * 2 * ((t2->moment) - (t1->moment)) / (t1->freq + t2->freq);
+ }
+
+
+ double _ncvMomentsDiffToMilliseconds(NcvTimeMoment *t1, NcvTimeMoment *t2)
+ {
+ return 1000.0 * 2 * ((t2->moment) - (t1->moment)) / (t1->freq + t2->freq);
+ }
+
+#elif defined(__GNUC__)
+
+ #include <sys/time.h>
+
+ typedef struct _NcvTimeMoment
+ {
+ struct timeval tv;
+ struct timezone tz;
+ } NcvTimeMoment;
+
+
+ void _ncvQueryMoment(NcvTimeMoment *t)
+ {
+ gettimeofday(& t->tv, & t->tz);
+ }
+
+
+ double _ncvMomentToMicroseconds(NcvTimeMoment *t)
+ {
+ return 1000000.0 * t->tv.tv_sec + (double)t->tv.tv_usec;
+ }
+
+
+ double _ncvMomentsDiffToMicroseconds(NcvTimeMoment *t1, NcvTimeMoment *t2)
+ {
+ return (((double)t2->tv.tv_sec - (double)t1->tv.tv_sec) * 1000000 + (double)t2->tv.tv_usec - (double)t1->tv.tv_usec);
+ }
+
+ double _ncvMomentsDiffToMilliseconds(NcvTimeMoment *t1, NcvTimeMoment *t2)
+ {
+ return ((double)t2->tv.tv_sec - (double)t1->tv.tv_sec) * 1000;
+ }
+
+#endif //#if defined(_WIN32) || defined(_WIN64)
+
+
+struct _NcvTimer
+{
+ NcvTimeMoment t1, t2;
+};
+
+
+NcvTimer ncvStartTimer(void)
+{
+ struct _NcvTimer *t;
+ t = (struct _NcvTimer *)malloc(sizeof(struct _NcvTimer));
+ _ncvQueryMoment(&t->t1);
+ return t;
+}
+
+
+double ncvEndQueryTimerUs(NcvTimer t)
+{
+ double res;
+ _ncvQueryMoment(&t->t2);
+ res = _ncvMomentsDiffToMicroseconds(&t->t1, &t->t2);
+ free(t);
+ return res;
+}
+
+
+double ncvEndQueryTimerMs(NcvTimer t)
+{
+ double res;
+ _ncvQueryMoment(&t->t2);
+ res = _ncvMomentsDiffToMilliseconds(&t->t1, &t->t2);
+ free(t);
+ return res;
+}
+
+
+//===================================================================
+//
+// Operations with rectangles
+//
+//===================================================================
+
+
+//from OpenCV
+//void groupRectangles(std::vector<NcvRect32u> &hypotheses, int groupThreshold, double eps, std::vector<Ncv32u> *weights);
+
+/*
+NCVStatus ncvGroupRectangles_host(NCVVector<NcvRect32u> &hypotheses,
+ Ncv32u &numHypotheses,
+ Ncv32u minNeighbors,
+ Ncv32f intersectEps,
+ NCVVector<Ncv32u> *hypothesesWeights)
+{
+ ncvAssertReturn(hypotheses.memType() == NCVMemoryTypeHostPageable ||
+ hypotheses.memType() == NCVMemoryTypeHostPinned, NCV_MEM_RESIDENCE_ERROR);
+ if (hypothesesWeights != NULL)
+ {
+ ncvAssertReturn(hypothesesWeights->memType() == NCVMemoryTypeHostPageable ||
+ hypothesesWeights->memType() == NCVMemoryTypeHostPinned, NCV_MEM_RESIDENCE_ERROR);
+ }
+
+ if (numHypotheses == 0)
+ {
+ return NCV_SUCCESS;
+ }
+
+ std::vector<NcvRect32u> rects(numHypotheses);
+ memcpy(&rects[0], hypotheses.ptr(), numHypotheses * sizeof(NcvRect32u));
+
+ std::vector<Ncv32u> weights;
+ if (hypothesesWeights != NULL)
+ {
+ groupRectangles(rects, minNeighbors, intersectEps, &weights);
+ }
+ else
+ {
+ groupRectangles(rects, minNeighbors, intersectEps, NULL);
+ }
+
+ numHypotheses = (Ncv32u)rects.size();
+ if (numHypotheses > 0)
+ {
+ memcpy(hypotheses.ptr(), &rects[0], numHypotheses * sizeof(NcvRect32u));
+ }
+
+ if (hypothesesWeights != NULL)
+ {
+ memcpy(hypothesesWeights->ptr(), &weights[0], numHypotheses * sizeof(Ncv32u));
+ }
+
+ return NCV_SUCCESS;
+}
+*/
+
+template <class T>
+static NCVStatus drawRectsWrapperHost(T *h_dst,
+ Ncv32u dstStride,
+ Ncv32u dstWidth,
+ Ncv32u dstHeight,
+ NcvRect32u *h_rects,
+ Ncv32u numRects,
+ T color)
+{
+ ncvAssertReturn(h_dst != NULL && h_rects != NULL, NCV_NULL_PTR);
+ ncvAssertReturn(dstWidth > 0 && dstHeight > 0, NCV_DIMENSIONS_INVALID);
+ ncvAssertReturn(dstStride >= dstWidth, NCV_INVALID_STEP);
+ ncvAssertReturn(numRects != 0, NCV_SUCCESS);
+ ncvAssertReturn(numRects <= dstWidth * dstHeight, NCV_DIMENSIONS_INVALID);
+
+ for (Ncv32u i=0; i<numRects; i++)
+ {
+ NcvRect32u rect = h_rects[i];
+
+ if (rect.x < dstWidth)
+ {
+ for (Ncv32u i=rect.y; i<rect.y+rect.height && i<dstHeight; i++)
+ {
+ h_dst[i*dstStride+rect.x] = color;
+ }
+ }
+ if (rect.x+rect.width-1 < dstWidth)
+ {
+ for (Ncv32u i=rect.y; i<rect.y+rect.height && i<dstHeight; i++)
+ {
+ h_dst[i*dstStride+rect.x+rect.width-1] = color;
+ }
+ }
+ if (rect.y < dstHeight)
+ {
+ for (Ncv32u j=rect.x; j<rect.x+rect.width && j<dstWidth; j++)
+ {
+ h_dst[rect.y*dstStride+j] = color;
+ }
+ }
+ if (rect.y + rect.height - 1 < dstHeight)
+ {
+ for (Ncv32u j=rect.x; j<rect.x+rect.width && j<dstWidth; j++)
+ {
+ h_dst[(rect.y+rect.height-1)*dstStride+j] = color;
+ }
+ }
+ }
+
+ return NCV_SUCCESS;
+}
+
+
+NCVStatus ncvDrawRects_8u_host(Ncv8u *h_dst,
+ Ncv32u dstStride,
+ Ncv32u dstWidth,
+ Ncv32u dstHeight,
+ NcvRect32u *h_rects,
+ Ncv32u numRects,
+ Ncv8u color)
+{
+ return drawRectsWrapperHost(h_dst, dstStride, dstWidth, dstHeight, h_rects, numRects, color);
+}
+
+
+NCVStatus ncvDrawRects_32u_host(Ncv32u *h_dst,
+ Ncv32u dstStride,
+ Ncv32u dstWidth,
+ Ncv32u dstHeight,
+ NcvRect32u *h_rects,
+ Ncv32u numRects,
+ Ncv32u color)
+{
+ return drawRectsWrapperHost(h_dst, dstStride, dstWidth, dstHeight, h_rects, numRects, color);
+}
+
+
+const Ncv32u NUMTHREADS_DRAWRECTS = 32;
+const Ncv32u NUMTHREADS_DRAWRECTS_LOG2 = 5;
+
+
+template <class T>
+__global__ void drawRects(T *d_dst,
+ Ncv32u dstStride,
+ Ncv32u dstWidth,
+ Ncv32u dstHeight,
+ NcvRect32u *d_rects,
+ Ncv32u numRects,
+ T color)
+{
+ Ncv32u blockId = blockIdx.y * 65535 + blockIdx.x;
+ if (blockId > numRects * 4)
+ {
+ return;
+ }
+
+ NcvRect32u curRect = d_rects[blockId >> 2];
+ NcvBool bVertical = blockId & 0x1;
+ NcvBool bTopLeft = blockId & 0x2;
+
+ Ncv32u pt0x, pt0y;
+ if (bVertical)
+ {
+ Ncv32u numChunks = (curRect.height + NUMTHREADS_DRAWRECTS - 1) >> NUMTHREADS_DRAWRECTS_LOG2;
+
+ pt0x = bTopLeft ? curRect.x : curRect.x + curRect.width - 1;
+ pt0y = curRect.y;
+
+ if (pt0x < dstWidth)
+ {
+ for (Ncv32u chunkId = 0; chunkId < numChunks; chunkId++)
+ {
+ Ncv32u ptY = pt0y + chunkId * NUMTHREADS_DRAWRECTS + threadIdx.x;
+ if (ptY < pt0y + curRect.height && ptY < dstHeight)
+ {
+ d_dst[ptY * dstStride + pt0x] = color;
+ }
+ }
+ }
+ }
+ else
+ {
+ Ncv32u numChunks = (curRect.width + NUMTHREADS_DRAWRECTS - 1) >> NUMTHREADS_DRAWRECTS_LOG2;
+
+ pt0x = curRect.x;
+ pt0y = bTopLeft ? curRect.y : curRect.y + curRect.height - 1;
+
+ if (pt0y < dstHeight)
+ {
+ for (Ncv32u chunkId = 0; chunkId < numChunks; chunkId++)
+ {
+ Ncv32u ptX = pt0x + chunkId * NUMTHREADS_DRAWRECTS + threadIdx.x;
+ if (ptX < pt0x + curRect.width && ptX < dstWidth)
+ {
+ d_dst[pt0y * dstStride + ptX] = color;
+ }
+ }
+ }
+ }
+}
+
+
+template <class T>
+static NCVStatus drawRectsWrapperDevice(T *d_dst,
+ Ncv32u dstStride,
+ Ncv32u dstWidth,
+ Ncv32u dstHeight,
+ NcvRect32u *d_rects,
+ Ncv32u numRects,
+ T color,
+ cudaStream_t cuStream)
+{
+ ncvAssertReturn(d_dst != NULL && d_rects != NULL, NCV_NULL_PTR);
+ ncvAssertReturn(dstWidth > 0 && dstHeight > 0, NCV_DIMENSIONS_INVALID);
+ ncvAssertReturn(dstStride >= dstWidth, NCV_INVALID_STEP);
+ ncvAssertReturn(numRects <= dstWidth * dstHeight, NCV_DIMENSIONS_INVALID);
+
+ if (numRects == 0)
+ {
+ return NCV_SUCCESS;
+ }
+
+ dim3 grid(numRects * 4);
+ dim3 block(NUMTHREADS_DRAWRECTS);
+ if (grid.x > 65535)
+ {
+ grid.y = (grid.x + 65534) / 65535;
+ grid.x = 65535;
+ }
+
+ drawRects<T><<<grid, block>>>(d_dst, dstStride, dstWidth, dstHeight, d_rects, numRects, color);
+
+ ncvAssertCUDALastErrorReturn(NCV_CUDA_ERROR);
+
+ return NCV_SUCCESS;
+}
+
+
+NCVStatus ncvDrawRects_8u_device(Ncv8u *d_dst,
+ Ncv32u dstStride,
+ Ncv32u dstWidth,
+ Ncv32u dstHeight,
+ NcvRect32u *d_rects,
+ Ncv32u numRects,
+ Ncv8u color,
+ cudaStream_t cuStream)
+{
+ return drawRectsWrapperDevice(d_dst, dstStride, dstWidth, dstHeight, d_rects, numRects, color, cuStream);
+}
+
+
+NCVStatus ncvDrawRects_32u_device(Ncv32u *d_dst,
+ Ncv32u dstStride,
+ Ncv32u dstWidth,
+ Ncv32u dstHeight,
+ NcvRect32u *d_rects,
+ Ncv32u numRects,
+ Ncv32u color,
+ cudaStream_t cuStream)
+{
+ return drawRectsWrapperDevice(d_dst, dstStride, dstWidth, dstHeight, d_rects, numRects, color, cuStream);
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (C) 2009-2010, NVIDIA Corporation, all rights reserved.
+ * Third party copyrights are property of their respective owners.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * Ported to PCL by Koen Buys : Attention Work in progress!
+ */
+
+#ifndef PCL_GPU_PEOPLE__NCV_HPP_
+#define PCL_GPU_PEOPLE__NCV_HPP_
+
+#if (defined WIN32 || defined _WIN32 || defined WINCE) && defined CVAPI_EXPORTS
+ #define NCV_EXPORTS __declspec(dllexport)
+#else
+ #define NCV_EXPORTS
+#endif
+
+#ifdef _WIN32
+ #define WIN32_LEAN_AND_MEAN
+#endif
+
+#include <cuda_runtime.h>
+#include <sstream>
+#include <iostream>
+#include <pcl/console/print.h>
+
+//==============================================================================
+//
+// Compile-time assert functionality
+//
+//==============================================================================
+
+/**
+* Compile-time assert namespace
+*/
+namespace NcvCTprep
+{
+ template <bool x>
+ struct CT_ASSERT_FAILURE;
+
+ template <>
+ struct CT_ASSERT_FAILURE<true> {};
+
+ template <int x>
+ struct assertTest{};
+}
+
+#define NCV_CT_PREP_PASTE_AUX(a,b) a##b ///< Concatenation indirection macro
+#define NCV_CT_PREP_PASTE(a,b) NCV_CT_PREP_PASTE_AUX(a, b) ///< Concatenation macro
+
+/**
+* Performs compile-time assertion of a condition on the file scope
+*/
+#define NCV_CT_ASSERT(X) \
+ typedef NcvCTprep::assertTest<sizeof(NcvCTprep::CT_ASSERT_FAILURE< (bool)(X) >)> \
+ NCV_CT_PREP_PASTE(__ct_assert_typedef_, __LINE__)
+
+//==============================================================================
+//
+// Alignment macros
+//
+//==============================================================================
+
+#if !defined(__align__) && !defined(__CUDACC__)
+ #if defined(_WIN32) || defined(_WIN64)
+ #define __align__(n) __declspec(align(n))
+ #elif defined(__unix__)
+ #define __align__(n) __attribute__((__aligned__(n)))
+ #endif
+#endif
+
+//==============================================================================
+//
+// Integral and compound types of guaranteed size
+//
+//==============================================================================
+
+typedef bool NcvBool;
+typedef long long Ncv64s;
+
+#if defined(__APPLE__) && !defined(__CUDACC__)
+ typedef uint64_t Ncv64u;
+#else
+ typedef unsigned long long Ncv64u;
+#endif
+
+typedef int Ncv32s;
+typedef unsigned int Ncv32u;
+typedef short Ncv16s;
+typedef unsigned short Ncv16u;
+typedef char Ncv8s;
+typedef unsigned char Ncv8u;
+typedef float Ncv32f;
+typedef double Ncv64f;
+
+struct NcvRect8u
+{
+ Ncv8u x;
+ Ncv8u y;
+ Ncv8u width;
+ Ncv8u height;
+ __host__ __device__ NcvRect8u() : x(0), y(0), width(0), height(0) {};
+ __host__ __device__ NcvRect8u(Ncv8u x, Ncv8u y, Ncv8u width, Ncv8u height) : x(x), y(y), width(width), height(height) {}
+};
+
+struct NcvRect32s
+{
+ Ncv32s x; ///< x-coordinate of upper left corner.
+ Ncv32s y; ///< y-coordinate of upper left corner.
+ Ncv32s width; ///< Rectangle width.
+ Ncv32s height; ///< Rectangle height.
+ __host__ __device__ NcvRect32s() : x(0), y(0), width(0), height(0) {};
+ __host__ __device__ NcvRect32s(Ncv32s x, Ncv32s y, Ncv32s width, Ncv32s height) : x(x), y(y), width(width), height(height) {}
+};
+
+struct NcvRect32u
+{
+ Ncv32u x; ///< x-coordinate of upper left corner.
+ Ncv32u y; ///< y-coordinate of upper left corner.
+ Ncv32u width; ///< Rectangle width.
+ Ncv32u height; ///< Rectangle height.
+ __host__ __device__ NcvRect32u() : x(0), y(0), width(0), height(0) {};
+ __host__ __device__ NcvRect32u(Ncv32u x, Ncv32u y, Ncv32u width, Ncv32u height) : x(x), y(y), width(width), height(height) {}
+};
+
+struct NcvSize32s
+{
+ Ncv32s width; ///< Rectangle width.
+ Ncv32s height; ///< Rectangle height.
+ __host__ __device__ NcvSize32s() : width(0), height(0) {};
+ __host__ __device__ NcvSize32s(Ncv32s width, Ncv32s height) : width(width), height(height) {}
+};
+
+struct NcvSize32u
+{
+ Ncv32u width; ///< Rectangle width.
+ Ncv32u height; ///< Rectangle height.
+ __host__ __device__ NcvSize32u() : width(0), height(0) {};
+ __host__ __device__ NcvSize32u(Ncv32u width, Ncv32u height) : width(width), height(height) {}
+ __host__ __device__ bool operator == (const NcvSize32u &another) const {return this->width == another.width && this->height == another.height;}
+};
+
+struct NcvPoint2D32s
+{
+ Ncv32s x; ///< Point X.
+ Ncv32s y; ///< Point Y.
+ __host__ __device__ NcvPoint2D32s() : x(0), y(0) {};
+ __host__ __device__ NcvPoint2D32s(Ncv32s x, Ncv32s y) : x(x), y(y) {}
+};
+
+struct NcvPoint2D32u
+{
+ Ncv32u x; ///< Point X.
+ Ncv32u y; ///< Point Y.
+ __host__ __device__ NcvPoint2D32u() : x(0), y(0) {};
+ __host__ __device__ NcvPoint2D32u(Ncv32u x, Ncv32u y) : x(x), y(y) {}
+};
+
+NCV_CT_ASSERT(sizeof(NcvBool) <= 4);
+NCV_CT_ASSERT(sizeof(Ncv64s) == 8);
+NCV_CT_ASSERT(sizeof(Ncv64u) == 8);
+NCV_CT_ASSERT(sizeof(Ncv32s) == 4);
+NCV_CT_ASSERT(sizeof(Ncv32u) == 4);
+NCV_CT_ASSERT(sizeof(Ncv16s) == 2);
+NCV_CT_ASSERT(sizeof(Ncv16u) == 2);
+NCV_CT_ASSERT(sizeof(Ncv8s) == 1);
+NCV_CT_ASSERT(sizeof(Ncv8u) == 1);
+NCV_CT_ASSERT(sizeof(Ncv32f) == 4);
+NCV_CT_ASSERT(sizeof(Ncv64f) == 8);
+NCV_CT_ASSERT(sizeof(NcvRect8u) == sizeof(Ncv32u));
+NCV_CT_ASSERT(sizeof(NcvRect32s) == 4 * sizeof(Ncv32s));
+NCV_CT_ASSERT(sizeof(NcvRect32u) == 4 * sizeof(Ncv32u));
+NCV_CT_ASSERT(sizeof(NcvSize32u) == 2 * sizeof(Ncv32u));
+NCV_CT_ASSERT(sizeof(NcvPoint2D32u) == 2 * sizeof(Ncv32u));
+
+
+//==============================================================================
+//
+// Persistent constants
+//
+//==============================================================================
+
+const Ncv32u K_WARP_SIZE = 32;
+const Ncv32u K_LOG2_WARP_SIZE = 5;
+
+//==============================================================================
+//
+// Error handling
+//
+//==============================================================================
+
+NCV_EXPORTS void ncvDebugOutput(const std::string &msg);
+
+typedef void NCVDebugOutputHandler(const std::string &msg);
+
+NCV_EXPORTS void ncvSetDebugOutputHandler(NCVDebugOutputHandler* func);
+
+#define ncvAssertPrintCheck(pred, msg) \
+ do \
+ { \
+ if (!(pred)) \
+ { \
+ std::ostringstream oss; \
+ oss << "NCV Assertion Failed: " << msg << ", file=" << __FILE__ << ", line=" << __LINE__ << std::endl; \
+ ncvDebugOutput(oss.str()); \
+ } \
+ } while (0)
+
+#define ncvAssertPrintReturn(pred, msg, err) \
+ do \
+ { \
+ ncvAssertPrintCheck(pred, msg); \
+ if (!(pred)) return err; \
+ } while (0)
+
+#define ncvAssertReturn(pred, err) \
+ ncvAssertPrintReturn(pred, "retcode=" << (int)err, err)
+
+#define ncvAssertReturnNcvStat(ncvOp) \
+ do \
+ { \
+ NCVStatus _ncvStat = ncvOp; \
+ ncvAssertPrintReturn(NCV_SUCCESS==_ncvStat, "NcvStat=" << (int)_ncvStat, _ncvStat); \
+ } while (0)
+
+#define ncvAssertCUDAReturn(cudacall, errCode) \
+ do \
+ { \
+ cudaError_t res = cudacall; \
+ ncvAssertPrintReturn(cudaSuccess==res, "cudaError_t=" << res, errCode); \
+ } while (0)
+
+#define ncvAssertCUDALastErrorReturn(errCode) \
+ do \
+ { \
+ cudaError_t res = cudaGetLastError(); \
+ ncvAssertPrintReturn(cudaSuccess==res, "cudaError_t=" << res, errCode); \
+ } while (0)
+
+/**
+* \brief Return-codes for status notification, errors and warnings
+*/
+enum
+{
+ //NCV statuses
+ NCV_SUCCESS,
+ NCV_UNKNOWN_ERROR,
+
+ NCV_CUDA_ERROR,
+ NCV_NPP_ERROR,
+ NCV_FILE_ERROR,
+
+ NCV_NULL_PTR,
+ NCV_INCONSISTENT_INPUT,
+ NCV_TEXTURE_BIND_ERROR,
+ NCV_DIMENSIONS_INVALID,
+
+ NCV_INVALID_ROI,
+ NCV_INVALID_STEP,
+ NCV_INVALID_SCALE,
+
+ NCV_ALLOCATOR_NOT_INITIALIZED,
+ NCV_ALLOCATOR_BAD_ALLOC,
+ NCV_ALLOCATOR_BAD_DEALLOC,
+ NCV_ALLOCATOR_INSUFFICIENT_CAPACITY,
+ NCV_ALLOCATOR_DEALLOC_ORDER,
+ NCV_ALLOCATOR_BAD_REUSE,
+
+ NCV_MEM_COPY_ERROR,
+ NCV_MEM_RESIDENCE_ERROR,
+ NCV_MEM_INSUFFICIENT_CAPACITY,
+
+ NCV_HAAR_INVALID_PIXEL_STEP,
+ NCV_HAAR_TOO_MANY_FEATURES_IN_CLASSIFIER,
+ NCV_HAAR_TOO_MANY_FEATURES_IN_CASCADE,
+ NCV_HAAR_TOO_LARGE_FEATURES,
+ NCV_HAAR_XML_LOADING_EXCEPTION,
+
+ NCV_NOIMPL_HAAR_TILTED_FEATURES,
+ NCV_NOT_IMPLEMENTED,
+
+ NCV_WARNING_HAAR_DETECTIONS_VECTOR_OVERFLOW,
+
+ //NPP statuses
+ NPPST_SUCCESS = NCV_SUCCESS, ///< Successful operation (same as NPP_NO_ERROR)
+ NPPST_ERROR, ///< Unknown error
+ NPPST_CUDA_KERNEL_EXECUTION_ERROR, ///< CUDA kernel execution error
+ NPPST_NULL_POINTER_ERROR, ///< NULL pointer argument error
+ NPPST_TEXTURE_BIND_ERROR, ///< CUDA texture binding error or non-zero offset returned
+ NPPST_MEMCPY_ERROR, ///< CUDA memory copy error
+ NPPST_MEM_ALLOC_ERR, ///< CUDA memory allocation error
+ NPPST_MEMFREE_ERR, ///< CUDA memory deallocation error
+
+ //NPPST statuses
+ NPPST_INVALID_ROI, ///< Invalid region of interest argument
+ NPPST_INVALID_STEP, ///< Invalid image lines step argument (check sign, alignment, relation to image width)
+ NPPST_INVALID_SCALE, ///< Invalid scale parameter passed
+ NPPST_MEM_INSUFFICIENT_BUFFER, ///< Insufficient user-allocated buffer
+ NPPST_MEM_RESIDENCE_ERROR, ///< Memory residence error detected (check if pointers should be device or pinned)
+ NPPST_MEM_INTERNAL_ERROR, ///< Internal memory management error
+
+ NCV_LAST_STATUS ///< Marker to continue error numeration in other files
+};
+
+typedef Ncv32u NCVStatus;
+
+#define NCV_SET_SKIP_COND(x) \
+ bool __ncv_skip_cond = x
+
+#define NCV_RESET_SKIP_COND(x) \
+ __ncv_skip_cond = x
+
+#define NCV_SKIP_COND_BEGIN \
+ if (!__ncv_skip_cond) {
+
+#define NCV_SKIP_COND_END \
+ }
+
+
+//==============================================================================
+//
+// Timer
+//
+//==============================================================================
+
+
+typedef struct _NcvTimer *NcvTimer;
+
+NCV_EXPORTS NcvTimer ncvStartTimer(void);
+
+NCV_EXPORTS double ncvEndQueryTimerUs(NcvTimer t);
+
+NCV_EXPORTS double ncvEndQueryTimerMs(NcvTimer t);
+
+
+//==============================================================================
+//
+// Memory management classes template compound types
+//
+//==============================================================================
+
+
+/**
+* Calculates the aligned top bound value
+*/
+NCV_EXPORTS Ncv32u alignUp(Ncv32u what, Ncv32u alignment);
+
+
+/**
+* NCVMemoryType
+*/
+enum NCVMemoryType
+{
+ NCVMemoryTypeNone,
+ NCVMemoryTypeHostPageable,
+ NCVMemoryTypeHostPinned,
+ NCVMemoryTypeDevice
+};
+
+
+/**
+* NCVMemPtr
+*/
+struct NCV_EXPORTS NCVMemPtr
+{
+ void *ptr;
+ NCVMemoryType memtype;
+ void clear();
+};
+
+
+/**
+* NCVMemSegment
+*/
+struct NCV_EXPORTS NCVMemSegment
+{
+ NCVMemPtr begin;
+ size_t size;
+ void clear();
+};
+
+
+/**
+* INCVMemAllocator (Interface)
+*/
+class NCV_EXPORTS INCVMemAllocator
+{
+public:
+ virtual ~INCVMemAllocator() = 0;
+
+ virtual NCVStatus alloc(NCVMemSegment &seg, size_t size) = 0;
+ virtual NCVStatus dealloc(NCVMemSegment &seg) = 0;
+
+ virtual NcvBool isInitialized(void) const = 0;
+ virtual NcvBool isCounting(void) const = 0;
+
+ virtual NCVMemoryType memType(void) const = 0;
+ virtual Ncv32u alignment(void) const = 0;
+ virtual size_t maxSize(void) const = 0;
+};
+
+inline INCVMemAllocator::~INCVMemAllocator() {}
+
+
+/**
+* NCVMemStackAllocator
+*/
+class NCV_EXPORTS NCVMemStackAllocator : public INCVMemAllocator
+{
+ NCVMemStackAllocator();
+ NCVMemStackAllocator(const NCVMemStackAllocator &);
+
+public:
+
+ explicit NCVMemStackAllocator(Ncv32u alignment);
+ NCVMemStackAllocator(NCVMemoryType memT, size_t capacity, Ncv32u alignment, void *reusePtr=NULL);
+ virtual ~NCVMemStackAllocator();
+
+ virtual NCVStatus alloc(NCVMemSegment &seg, size_t size);
+ virtual NCVStatus dealloc(NCVMemSegment &seg);
+
+ virtual NcvBool isInitialized(void) const;
+ virtual NcvBool isCounting(void) const;
+
+ virtual NCVMemoryType memType(void) const;
+ virtual Ncv32u alignment(void) const;
+ virtual size_t maxSize(void) const;
+
+private:
+
+ NCVMemoryType _memType;
+ Ncv32u _alignment;
+ Ncv8u *allocBegin;
+ Ncv8u *begin;
+ Ncv8u *end;
+ size_t currentSize;
+ size_t _maxSize;
+ NcvBool bReusesMemory;
+};
+
+
+/**
+* NCVMemNativeAllocator
+*/
+class NCV_EXPORTS NCVMemNativeAllocator : public INCVMemAllocator
+{
+public:
+
+ NCVMemNativeAllocator(NCVMemoryType memT, Ncv32u alignment);
+ virtual ~NCVMemNativeAllocator();
+
+ virtual NCVStatus alloc(NCVMemSegment &seg, size_t size);
+ virtual NCVStatus dealloc(NCVMemSegment &seg);
+
+ virtual NcvBool isInitialized(void) const;
+ virtual NcvBool isCounting(void) const;
+
+ virtual NCVMemoryType memType(void) const;
+ virtual Ncv32u alignment(void) const;
+ virtual size_t maxSize(void) const;
+
+private:
+
+ NCVMemNativeAllocator();
+ NCVMemNativeAllocator(const NCVMemNativeAllocator &);
+
+ NCVMemoryType _memType;
+ Ncv32u _alignment;
+ size_t currentSize;
+ size_t _maxSize;
+};
+
+
+/**
+* Copy dispatchers
+*/
+NCV_EXPORTS NCVStatus memSegCopyHelper(void *dst, NCVMemoryType dstType,
+ const void *src, NCVMemoryType srcType,
+ size_t sz, cudaStream_t cuStream);
+
+
+NCV_EXPORTS NCVStatus memSegCopyHelper2D(void *dst, Ncv32u dstPitch, NCVMemoryType dstType,
+ const void *src, Ncv32u srcPitch, NCVMemoryType srcType,
+ Ncv32u widthbytes, Ncv32u height, cudaStream_t cuStream);
+
+
+/**
+* NCVVector (1D)
+*/
+template <class T>
+class NCVVector
+{
+ NCVVector(const NCVVector &);
+
+public:
+
+ NCVVector()
+ {
+ clear();
+ }
+
+ virtual ~NCVVector() {}
+
+ void clear()
+ {
+ _ptr = NULL;
+ _length = 0;
+ _memtype = NCVMemoryTypeNone;
+ }
+
+ NCVStatus copySolid(NCVVector<T> &dst, cudaStream_t cuStream, size_t howMuch=0) const
+ {
+ if (howMuch == 0)
+ {
+ ncvAssertReturn(dst._length == this->_length, NCV_MEM_COPY_ERROR);
+ howMuch = this->_length * sizeof(T);
+ }
+ else
+ {
+ ncvAssertReturn(dst._length * sizeof(T) >= howMuch &&
+ this->_length * sizeof(T) >= howMuch &&
+ howMuch > 0, NCV_MEM_COPY_ERROR);
+ }
+ ncvAssertReturn((this->_ptr != NULL || this->_memtype == NCVMemoryTypeNone) &&
+ (dst._ptr != NULL || dst._memtype == NCVMemoryTypeNone), NCV_NULL_PTR);
+
+ NCVStatus ncvStat = NCV_SUCCESS;
+ if (this->_memtype != NCVMemoryTypeNone)
+ {
+ ncvStat = memSegCopyHelper(dst._ptr, dst._memtype,
+ this->_ptr, this->_memtype,
+ howMuch, cuStream);
+ }
+
+ return ncvStat;
+ }
+
+ T *ptr() const {return this->_ptr;}
+ size_t length() const {return this->_length;}
+ NCVMemoryType memType() const {return this->_memtype;}
+
+protected:
+
+ T *_ptr;
+ size_t _length;
+ NCVMemoryType _memtype;
+};
+
+
+/**
+* NCVVectorAlloc
+*/
+template <class T>
+class NCVVectorAlloc : public NCVVector<T>
+{
+ NCVVectorAlloc();
+ NCVVectorAlloc(const NCVVectorAlloc &);
+ NCVVectorAlloc& operator=(const NCVVectorAlloc<T>&);
+
+public:
+
+ NCVVectorAlloc(INCVMemAllocator &allocator, Ncv32u length)
+ :
+ allocator(allocator)
+ {
+ NCVStatus ncvStat;
+
+ this->clear();
+ this->allocatedMem.clear();
+
+ ncvStat = allocator.alloc(this->allocatedMem, length * sizeof(T));
+ ncvAssertPrintReturn(ncvStat == NCV_SUCCESS, "NCVVectorAlloc ctor:: alloc failed", );
+
+ this->_ptr = (T *)this->allocatedMem.begin.ptr;
+ this->_length = length;
+ this->_memtype = this->allocatedMem.begin.memtype;
+ }
+
+ ~NCVVectorAlloc()
+ {
+ NCVStatus ncvStat;
+
+ ncvStat = allocator.dealloc(this->allocatedMem);
+ ncvAssertPrintCheck(ncvStat == NCV_SUCCESS, "NCVVectorAlloc dtor:: dealloc failed");
+
+ this->clear();
+ }
+
+ NcvBool isMemAllocated() const
+ {
+ return (this->allocatedMem.begin.ptr != NULL) || (this->allocator.isCounting());
+ }
+
+ Ncv32u getAllocatorsAlignment() const
+ {
+ return allocator.alignment();
+ }
+
+ NCVMemSegment getSegment() const
+ {
+ return allocatedMem;
+ }
+
+private:
+ INCVMemAllocator &allocator;
+ NCVMemSegment allocatedMem;
+};
+
+
+/**
+* NCVVectorReuse
+*/
+template <class T>
+class NCVVectorReuse : public NCVVector<T>
+{
+ NCVVectorReuse();
+ NCVVectorReuse(const NCVVectorReuse &);
+
+public:
+
+ explicit NCVVectorReuse(const NCVMemSegment &memSegment)
+ {
+ this->bReused = false;
+ this->clear();
+
+ this->_length = memSegment.size / sizeof(T);
+ this->_ptr = (T *)memSegment.begin.ptr;
+ this->_memtype = memSegment.begin.memtype;
+
+ this->bReused = true;
+ }
+
+ NCVVectorReuse(const NCVMemSegment &memSegment, Ncv32u length)
+ {
+ this->bReused = false;
+ this->clear();
+
+ ncvAssertPrintReturn(length * sizeof(T) <= memSegment.size, \
+ "NCVVectorReuse ctor:: memory binding failed due to size mismatch", );
+
+ this->_length = length;
+ this->_ptr = (T *)memSegment.begin.ptr;
+ this->_memtype = memSegment.begin.memtype;
+
+ this->bReused = true;
+ }
+
+ NcvBool isMemReused() const
+ {
+ return this->bReused;
+ }
+
+private:
+
+ NcvBool bReused;
+};
+
+
+/**
+* NCVMatrix (2D)
+*/
+template <class T>
+class NCVMatrix
+{
+ NCVMatrix(const NCVMatrix &);
+
+public:
+
+ NCVMatrix()
+ {
+ clear();
+ }
+
+ virtual ~NCVMatrix() {}
+
+ void clear()
+ {
+ _ptr = NULL;
+ _pitch = 0;
+ _width = 0;
+ _height = 0;
+ _memtype = NCVMemoryTypeNone;
+ }
+
+ Ncv32u stride() const
+ {
+ return _pitch / sizeof(T);
+ }
+
+ //a side effect of this function is that it copies everything in a single chunk, so the "padding" will be overwritten
+ NCVStatus copySolid(NCVMatrix<T> &dst, cudaStream_t cuStream, size_t howMuch=0) const
+ {
+ if (howMuch == 0)
+ {
+ ncvAssertReturn(dst._pitch == this->_pitch &&
+ dst._height == this->_height, NCV_MEM_COPY_ERROR);
+ howMuch = this->_pitch * this->_height;
+ }
+ else
+ {
+ ncvAssertReturn(dst._pitch * dst._height >= howMuch &&
+ this->_pitch * this->_height >= howMuch &&
+ howMuch > 0, NCV_MEM_COPY_ERROR);
+ }
+ ncvAssertReturn((this->_ptr != NULL || this->_memtype == NCVMemoryTypeNone) &&
+ (dst._ptr != NULL || dst._memtype == NCVMemoryTypeNone), NCV_NULL_PTR);
+
+ NCVStatus ncvStat = NCV_SUCCESS;
+ if (this->_memtype != NCVMemoryTypeNone)
+ {
+ ncvStat = memSegCopyHelper(dst._ptr, dst._memtype,
+ this->_ptr, this->_memtype,
+ howMuch, cuStream);
+ }
+
+ return ncvStat;
+ }
+
+ NCVStatus copy2D(NCVMatrix<T> &dst, NcvSize32u roi, cudaStream_t cuStream) const
+ {
+ ncvAssertReturn(this->width() >= roi.width && this->height() >= roi.height &&
+ dst.width() >= roi.width && dst.height() >= roi.height, NCV_MEM_COPY_ERROR);
+ ncvAssertReturn((this->_ptr != NULL || this->_memtype == NCVMemoryTypeNone) &&
+ (dst._ptr != NULL || dst._memtype == NCVMemoryTypeNone), NCV_NULL_PTR);
+
+ NCVStatus ncvStat = NCV_SUCCESS;
+ if (this->_memtype != NCVMemoryTypeNone)
+ {
+ ncvStat = memSegCopyHelper2D(dst._ptr, dst._pitch, dst._memtype,
+ this->_ptr, this->_pitch, this->_memtype,
+ roi.width * sizeof(T), roi.height, cuStream);
+ }
+
+ return ncvStat;
+ }
+
+ T &at(Ncv32u x, Ncv32u y) const
+ {
+ NcvBool bOutRange = (x >= this->_width || y >= this->_height);
+ ncvAssertPrintCheck(!bOutRange, "Error addressing matrix at [" << x << ", " << y << "]");
+ if (bOutRange)
+ {
+ return *this->_ptr;
+ }
+ return ((T *)((Ncv8u *)this->_ptr + y * this->_pitch))[x];
+ }
+
+ T *ptr() const {return this->_ptr;}
+ Ncv32u width() const {return this->_width;}
+ Ncv32u height() const {return this->_height;}
+ NcvSize32u size() const {return NcvSize32u(this->_width, this->_height);}
+ Ncv32u pitch() const {return this->_pitch;}
+ NCVMemoryType memType() const {return this->_memtype;}
+
+protected:
+
+ T *_ptr;
+ Ncv32u _width;
+ Ncv32u _height;
+ Ncv32u _pitch;
+ NCVMemoryType _memtype;
+};
+
+
+/**
+* NCVMatrixAlloc
+*/
+template <class T>
+class NCVMatrixAlloc : public NCVMatrix<T>
+{
+ NCVMatrixAlloc();
+ NCVMatrixAlloc(const NCVMatrixAlloc &);
+ NCVMatrixAlloc& operator=(const NCVMatrixAlloc &);
+public:
+
+ NCVMatrixAlloc(INCVMemAllocator &allocator, Ncv32u width, Ncv32u height, Ncv32u pitch=0)
+ :
+ allocator(allocator)
+ {
+ NCVStatus ncvStat;
+
+ this->clear();
+ this->allocatedMem.clear();
+
+ Ncv32u widthBytes = width * sizeof(T);
+ Ncv32u pitchBytes = alignUp(widthBytes, allocator.alignment());
+
+ if (pitch != 0)
+ {
+ ncvAssertPrintReturn(pitch >= pitchBytes &&
+ (pitch & (allocator.alignment() - 1)) == 0,
+ "NCVMatrixAlloc ctor:: incorrect pitch passed", );
+ pitchBytes = pitch;
+ }
+
+ Ncv32u requiredAllocSize = pitchBytes * height;
+
+ ncvStat = allocator.alloc(this->allocatedMem, requiredAllocSize);
+ ncvAssertPrintReturn(ncvStat == NCV_SUCCESS, "NCVMatrixAlloc ctor:: alloc failed", );
+
+ this->_ptr = (T *)this->allocatedMem.begin.ptr;
+ this->_width = width;
+ this->_height = height;
+ this->_pitch = pitchBytes;
+ this->_memtype = this->allocatedMem.begin.memtype;
+ }
+
+ ~NCVMatrixAlloc()
+ {
+ NCVStatus ncvStat;
+
+ ncvStat = allocator.dealloc(this->allocatedMem);
+ ncvAssertPrintCheck(ncvStat == NCV_SUCCESS, "NCVMatrixAlloc dtor:: dealloc failed");
+
+ this->clear();
+ }
+
+ NcvBool isMemAllocated() const
+ {
+ return (this->allocatedMem.begin.ptr != NULL) || (this->allocator.isCounting());
+ }
+
+ Ncv32u getAllocatorsAlignment() const
+ {
+ return allocator.alignment();
+ }
+
+ NCVMemSegment getSegment() const
+ {
+ return allocatedMem;
+ }
+
+private:
+
+ INCVMemAllocator &allocator;
+ NCVMemSegment allocatedMem;
+};
+
+
+/**
+* NCVMatrixReuse
+*/
+template <class T>
+class NCVMatrixReuse : public NCVMatrix<T>
+{
+ NCVMatrixReuse();
+ NCVMatrixReuse(const NCVMatrixReuse &);
+
+public:
+
+ NCVMatrixReuse(const NCVMemSegment &memSegment, Ncv32u alignment, Ncv32u width, Ncv32u height, Ncv32u pitch=0, NcvBool bSkipPitchCheck=false)
+ {
+ this->bReused = false;
+ this->clear();
+
+ Ncv32u widthBytes = width * sizeof(T);
+ Ncv32u pitchBytes = alignUp(widthBytes, alignment);
+
+ if (pitch != 0)
+ {
+ if (!bSkipPitchCheck)
+ {
+ ncvAssertPrintReturn(pitch >= pitchBytes &&
+ (pitch & (alignment - 1)) == 0,
+ "NCVMatrixReuse ctor:: incorrect pitch passed", );
+ }
+ else
+ {
+ ncvAssertPrintReturn(pitch >= widthBytes, "NCVMatrixReuse ctor:: incorrect pitch passed", );
+ }
+ pitchBytes = pitch;
+ }
+
+ ncvAssertPrintReturn(pitchBytes * height <= memSegment.size, \
+ "NCVMatrixReuse ctor:: memory binding failed due to size mismatch", );
+
+ this->_width = width;
+ this->_height = height;
+ this->_pitch = pitchBytes;
+ this->_ptr = (T *)memSegment.begin.ptr;
+ this->_memtype = memSegment.begin.memtype;
+
+ this->bReused = true;
+ }
+
+ NCVMatrixReuse(const NCVMatrix<T> &mat, NcvRect32u roi)
+ {
+ this->bReused = false;
+ this->clear();
+
+ ncvAssertPrintReturn(roi.x < mat.width() && roi.y < mat.height() && \
+ roi.x + roi.width <= mat.width() && roi.y + roi.height <= mat.height(),
+ "NCVMatrixReuse ctor:: memory binding failed due to mismatching ROI and source matrix dims", );
+
+ this->_width = roi.width;
+ this->_height = roi.height;
+ this->_pitch = mat.pitch();
+ this->_ptr = &mat.at(roi.x, roi.y);
+ this->_memtype = mat.memType();
+
+ this->bReused = true;
+ }
+
+ NcvBool isMemReused() const
+ {
+ return this->bReused;
+ }
+
+private:
+
+ NcvBool bReused;
+};
+
+/**
+* Operations with rectangles
+*/
+NCV_EXPORTS NCVStatus ncvGroupRectangles_host(NCVVector<NcvRect32u> &hypotheses, Ncv32u &numHypotheses,
+ Ncv32u minNeighbors, Ncv32f intersectEps, NCVVector<Ncv32u> *hypothesesWeights);
+
+NCV_EXPORTS NCVStatus ncvDrawRects_8u_host(Ncv8u *h_dst, Ncv32u dstStride, Ncv32u dstWidth, Ncv32u dstHeight,
+ NcvRect32u *h_rects, Ncv32u numRects, Ncv8u color);
+
+NCV_EXPORTS NCVStatus ncvDrawRects_32u_host(Ncv32u *h_dst, Ncv32u dstStride, Ncv32u dstWidth, Ncv32u dstHeight,
+ NcvRect32u *h_rects, Ncv32u numRects, Ncv32u color);
+
+NCV_EXPORTS NCVStatus ncvDrawRects_8u_device(Ncv8u *d_dst, Ncv32u dstStride, Ncv32u dstWidth, Ncv32u dstHeight,
+ NcvRect32u *d_rects, Ncv32u numRects, Ncv8u color, cudaStream_t cuStream);
+
+NCV_EXPORTS NCVStatus ncvDrawRects_32u_device(Ncv32u *d_dst, Ncv32u dstStride, Ncv32u dstWidth, Ncv32u dstHeight,
+ NcvRect32u *d_rects, Ncv32u numRects, Ncv32u color, cudaStream_t cuStream);
+
+#define CLAMP(x,a,b) ( (x) > (b) ? (b) : ( (x) < (a) ? (a) : (x) ) )
+#define CLAMP_TOP(x, a) (((x) > (a)) ? (a) : (x))
+#define CLAMP_BOTTOM(x, a) (((x) < (a)) ? (a) : (x))
+#define CLAMP_0_255(x) CLAMP(x,0,255)
+
+#define SUB_BEGIN(type, name) struct { __inline type name
+#define SUB_END(name) } name;
+#define SUB_CALL(name) name.name
+
+#define SQR(x) ((x)*(x))
+
+#define ncvSafeMatAlloc(name, type, alloc, width, height, err) \
+ NCVMatrixAlloc<type> name(alloc, width, height); \
+ ncvAssertReturn(name.isMemAllocated(), err);
+
+#endif // PCL_GPU_PEOPLE__NCV_HPP_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (C) 2009-2010, NVIDIA Corporation, all rights reserved.
+ * Third party copyrights are property of their respective owners.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * Ported to PCL by Koen Buys : Attention Work in progress!
+ */
+
+#ifndef _ncv_alg_hpp_
+#define _ncv_alg_hpp_
+
+#include "NCV.hpp"
+
+
+template <class T>
+static void swap(T &p1, T &p2)
+{
+ T tmp = p1;
+ p1 = p2;
+ p2 = tmp;
+}
+
+
+template<typename T>
+static T divUp(T a, T b)
+{
+ return (a + b - 1) / b;
+}
+
+
+template<typename T>
+struct functorAddValues
+{
+ static __device__ __inline__ void assign(volatile T *dst, volatile T *src)
+ {
+ //Works only for integral types. If you see compiler error here, then you have to specify how to copy your object as a set of integral fields.
+ *dst = *src;
+ }
+ static __device__ __inline__ void reduce(volatile T &in1out, const volatile T &in2)
+ {
+ in1out += in2;
+ }
+};
+
+
+template<typename T>
+struct functorMinValues
+{
+ static __device__ __inline__ void assign(volatile T *dst, volatile T *src)
+ {
+ //Works only for integral types. If you see compiler error here, then you have to specify how to copy your object as a set of integral fields.
+ *dst = *src;
+ }
+ static __device__ __inline__ void reduce(volatile T &in1out, const volatile T &in2)
+ {
+ in1out = in1out > in2 ? in2 : in1out;
+ }
+};
+
+
+template<typename T>
+struct functorMaxValues
+{
+ static __device__ __inline__ void assign(volatile T *dst, volatile T *src)
+ {
+ //Works only for integral types. If you see compiler error here, then you have to specify how to copy your object as a set of integral fields.
+ *dst = *src;
+ }
+ static __device__ __inline__ void reduce(volatile T &in1out, const volatile T &in2)
+ {
+ in1out = in1out > in2 ? in1out : in2;
+ }
+};
+
+
+template<typename Tdata, class Tfunc, Ncv32u nThreads>
+static __device__ Tdata subReduce(Tdata threadElem)
+{
+ Tfunc functor;
+
+ __shared__ Tdata _reduceArr[nThreads];
+ volatile Tdata *reduceArr = _reduceArr;
+ functor.assign(reduceArr + threadIdx.x, &threadElem);
+ __syncthreads();
+
+ if (nThreads >= 256 && threadIdx.x < 128)
+ {
+ functor.reduce(reduceArr[threadIdx.x], reduceArr[threadIdx.x + 128]);
+ }
+ __syncthreads();
+
+ if (nThreads >= 128 && threadIdx.x < 64)
+ {
+ functor.reduce(reduceArr[threadIdx.x], reduceArr[threadIdx.x + 64]);
+ }
+ __syncthreads();
+
+ if (threadIdx.x < 32)
+ {
+ if (nThreads >= 64)
+ {
+ functor.reduce(reduceArr[threadIdx.x], reduceArr[threadIdx.x + 32]);
+ }
+ if (nThreads >= 32 && threadIdx.x < 16)
+ {
+ functor.reduce(reduceArr[threadIdx.x], reduceArr[threadIdx.x + 16]);
+ functor.reduce(reduceArr[threadIdx.x], reduceArr[threadIdx.x + 8]);
+ functor.reduce(reduceArr[threadIdx.x], reduceArr[threadIdx.x + 4]);
+ functor.reduce(reduceArr[threadIdx.x], reduceArr[threadIdx.x + 2]);
+ functor.reduce(reduceArr[threadIdx.x], reduceArr[threadIdx.x + 1]);
+ }
+ }
+
+ __syncthreads();
+ Tdata reduceRes;
+ functor.assign(&reduceRes, reduceArr);
+ return reduceRes;
+}
+
+
+#endif //_ncv_alg_hpp_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (C) 2009-2010, NVIDIA Corporation, all rights reserved.
+ * Third party copyrights are property of their respective owners.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * Ported to PCL by Koen Buys : Attention Work in progress!
+ */
+
+#ifndef _ncv_color_conversion_hpp_
+#define _ncv_color_conversion_hpp_
+
+#include "NCVPixelOperations.hpp"
+
+enum NCVColorSpace
+{
+ NCVColorSpaceGray,
+ NCVColorSpaceRGBA,
+};
+
+template<NCVColorSpace CSin, NCVColorSpace CSout, typename Tin, typename Tout> struct __pixColorConv {
+static void _pixColorConv(const Tin &pixIn, Tout &pixOut);
+};
+
+template<typename Tin, typename Tout> struct __pixColorConv<NCVColorSpaceRGBA, NCVColorSpaceGray, Tin, Tout> {
+static void _pixColorConv(const Tin &pixIn, Tout &pixOut)
+{
+ Ncv32f luma = 0.299f * pixIn.x + 0.587f * pixIn.y + 0.114f * pixIn.z;
+ _TDemoteClampNN(luma, pixOut.x);
+}};
+
+template<typename Tin, typename Tout> struct __pixColorConv<NCVColorSpaceGray, NCVColorSpaceRGBA, Tin, Tout> {
+static void _pixColorConv(const Tin &pixIn, Tout &pixOut)
+{
+ _TDemoteClampNN(pixIn.x, pixOut.x);
+ _TDemoteClampNN(pixIn.x, pixOut.y);
+ _TDemoteClampNN(pixIn.x, pixOut.z);
+ pixOut.w = 0;
+}};
+
+template<NCVColorSpace CSin, NCVColorSpace CSout, typename Tin, typename Tout>
+static
+NCVStatus _ncvColorConv_host(const NCVMatrix<Tin> &h_imgIn,
+ const NCVMatrix<Tout> &h_imgOut)
+{
+ ncvAssertReturn(h_imgIn.size() == h_imgOut.size(), NCV_DIMENSIONS_INVALID);
+ ncvAssertReturn(h_imgIn.memType() == h_imgOut.memType() &&
+ (h_imgIn.memType() == NCVMemoryTypeHostPinned || h_imgIn.memType() == NCVMemoryTypeNone), NCV_MEM_RESIDENCE_ERROR);
+ NCV_SET_SKIP_COND(h_imgIn.memType() == NCVMemoryTypeNone);
+ NCV_SKIP_COND_BEGIN
+
+ for (Ncv32u i=0; i<h_imgIn.height(); i++)
+ {
+ for (Ncv32u j=0; j<h_imgIn.width(); j++)
+ {
+ __pixColorConv<CSin, CSout, Tin, Tout>::_pixColorConv(h_imgIn.at(j,i), h_imgOut.at(j,i));
+ }
+ }
+
+ NCV_SKIP_COND_END
+ return NCV_SUCCESS;
+}
+
+#endif //_ncv_color_conversion_hpp_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (C) 2009-2010, NVIDIA Corporation, all rights reserved.
+ * Third party copyrights are property of their respective owners.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * Ported to PCL by Koen Buys : Attention Work in progress!
+ */
+
+////////////////////////////////////////////////////////////////////////////////
+//
+// NVIDIA CUDA implementation of Viola-Jones Object Detection Framework
+//
+// The algorithm and code are explained in the upcoming GPU Computing Gems
+// chapter in detail:
+//
+// Anton Obukhov, "Haar Classifiers for Object Detection with CUDA"
+// PDF URL placeholder
+// email: aobukhov@nvidia.com, devsupport@nvidia.com
+//
+// Credits for help with the code to:
+// Alexey Mendelenko, Cyril Crassin, and Mikhail Smirnov.
+//
+////////////////////////////////////////////////////////////////////////////////
+
+#include <algorithm>
+#include <cstdio>
+
+#include "NCV.hpp"
+#include "NCVAlg.hpp"
+#include "NPP_staging.hpp"
+#include "NCVRuntimeTemplates.hpp"
+#include "NCVHaarObjectDetection.hpp"
+
+
+//==============================================================================
+//
+// BlockScan file
+//
+//==============================================================================
+
+
+NCV_CT_ASSERT(K_WARP_SIZE == 32); //this is required for the manual unroll of the loop in warpScanInclusive
+
+
+//Almost the same as naive scan1Inclusive, but doesn't need __syncthreads()
+//assuming size <= WARP_SIZE and size is power of 2
+__device__ Ncv32u warpScanInclusive(Ncv32u idata, volatile Ncv32u *s_Data)
+{
+ Ncv32u pos = 2 * threadIdx.x - (threadIdx.x & (K_WARP_SIZE - 1));
+ s_Data[pos] = 0;
+ pos += K_WARP_SIZE;
+ s_Data[pos] = idata;
+
+ s_Data[pos] += s_Data[pos - 1];
+ s_Data[pos] += s_Data[pos - 2];
+ s_Data[pos] += s_Data[pos - 4];
+ s_Data[pos] += s_Data[pos - 8];
+ s_Data[pos] += s_Data[pos - 16];
+
+ return s_Data[pos];
+}
+
+__device__ __forceinline__ Ncv32u warpScanExclusive(Ncv32u idata, volatile Ncv32u *s_Data)
+{
+ return warpScanInclusive(idata, s_Data) - idata;
+}
+
+template <Ncv32u tiNumScanThreads>
+__device__ Ncv32u scan1Inclusive(Ncv32u idata, volatile Ncv32u *s_Data)
+{
+ if (tiNumScanThreads > K_WARP_SIZE)
+ {
+ //Bottom-level inclusive warp scan
+ Ncv32u warpResult = warpScanInclusive(idata, s_Data);
+
+ //Save top elements of each warp for exclusive warp scan
+ //sync to wait for warp scans to complete (because s_Data is being overwritten)
+ __syncthreads();
+ if( (threadIdx.x & (K_WARP_SIZE - 1)) == (K_WARP_SIZE - 1) )
+ {
+ s_Data[threadIdx.x >> K_LOG2_WARP_SIZE] = warpResult;
+ }
+
+ //wait for warp scans to complete
+ __syncthreads();
+
+ if( threadIdx.x < (tiNumScanThreads / K_WARP_SIZE) )
+ {
+ //grab top warp elements
+ Ncv32u val = s_Data[threadIdx.x];
+ //calculate exclusive scan and write back to shared memory
+ s_Data[threadIdx.x] = warpScanExclusive(val, s_Data);
+ }
+
+ //return updated warp scans with exclusive scan results
+ __syncthreads();
+ return warpResult + s_Data[threadIdx.x >> K_LOG2_WARP_SIZE];
+ }
+ else
+ {
+ return warpScanInclusive(idata, s_Data);
+ }
+}
+
+
+//==============================================================================
+//
+// HaarClassifierCascade file
+//
+//==============================================================================
+
+
+const Ncv32u MAX_GRID_DIM = 65535;
+
+
+const Ncv32u NUM_THREADS_ANCHORSPARALLEL = 64;
+
+
+#define NUM_THREADS_CLASSIFIERPARALLEL_LOG2 6
+#define NUM_THREADS_CLASSIFIERPARALLEL (1 << NUM_THREADS_CLASSIFIERPARALLEL_LOG2)
+
+
+/** \internal
+* Haar features solid array.
+*/
+texture<uint2, 1, cudaReadModeElementType> texHaarFeatures;
+
+
+/** \internal
+* Haar classifiers flattened trees container.
+* Two parts: first contains root nodes, second - nodes that are referred by root nodes.
+* Drawback: breaks tree locality (might cause more cache misses
+* Advantage: No need to introduce additional 32-bit field to index root nodes offsets
+*/
+texture<uint4, 1, cudaReadModeElementType> texHaarClassifierNodes;
+
+
+texture<Ncv32u, 1, cudaReadModeElementType> texIImage;
+
+
+__device__ HaarStage64 getStage(Ncv32u iStage, HaarStage64 *d_Stages)
+{
+ return d_Stages[iStage];
+}
+
+
+template <NcvBool tbCacheTextureCascade>
+__device__ HaarClassifierNode128 getClassifierNode(Ncv32u iNode, HaarClassifierNode128 *d_ClassifierNodes)
+{
+ HaarClassifierNode128 tmpNode;
+ if (tbCacheTextureCascade)
+ {
+ tmpNode._ui4 = tex1Dfetch(texHaarClassifierNodes, iNode);
+ }
+ else
+ {
+ tmpNode = d_ClassifierNodes[iNode];
+ }
+ return tmpNode;
+}
+
+
+template <NcvBool tbCacheTextureCascade>
+__device__ void getFeature(Ncv32u iFeature, HaarFeature64 *d_Features,
+ Ncv32f *weight,
+ Ncv32u *rectX, Ncv32u *rectY, Ncv32u *rectWidth, Ncv32u *rectHeight)
+{
+ HaarFeature64 feature;
+ if (tbCacheTextureCascade)
+ {
+ feature._ui2 = tex1Dfetch(texHaarFeatures, iFeature);
+ }
+ else
+ {
+ feature = d_Features[iFeature];
+ }
+ feature.getRect(rectX, rectY, rectWidth, rectHeight);
+ *weight = feature.getWeight();
+}
+
+
+template <NcvBool tbCacheTextureIImg>
+__device__ Ncv32u getElemIImg(Ncv32u x, Ncv32u *d_IImg)
+{
+ if (tbCacheTextureIImg)
+ {
+ return tex1Dfetch(texIImage, x);
+ }
+ else
+ {
+ return d_IImg[x];
+ }
+}
+
+
+__device__ Ncv32u d_outMaskPosition;
+
+
+__device__ void compactBlockWriteOutAnchorParallel(Ncv32u threadPassFlag, Ncv32u threadElem, Ncv32u *vectorOut)
+{
+#if __CUDA_ARCH__ >= 110
+
+ __shared__ Ncv32u shmem[NUM_THREADS_ANCHORSPARALLEL * 2];
+ __shared__ Ncv32u numPassed;
+ __shared__ Ncv32u outMaskOffset;
+
+ Ncv32u incScan = scan1Inclusive<NUM_THREADS_ANCHORSPARALLEL>(threadPassFlag, shmem);
+ __syncthreads();
+
+ if (threadIdx.x == NUM_THREADS_ANCHORSPARALLEL-1)
+ {
+ numPassed = incScan;
+ outMaskOffset = atomicAdd(&d_outMaskPosition, incScan);
+ }
+
+ if (threadPassFlag)
+ {
+ Ncv32u excScan = incScan - threadPassFlag;
+ shmem[excScan] = threadElem;
+ }
+
+ __syncthreads();
+
+ if (threadIdx.x < numPassed)
+ {
+ vectorOut[outMaskOffset + threadIdx.x] = shmem[threadIdx.x];
+ }
+#endif
+}
+
+
+template <NcvBool tbInitMaskPositively,
+ NcvBool tbCacheTextureIImg,
+ NcvBool tbCacheTextureCascade,
+ NcvBool tbReadPixelIndexFromVector,
+ NcvBool tbDoAtomicCompaction>
+__global__ void applyHaarClassifierAnchorParallel(Ncv32u *d_IImg, Ncv32u IImgStride,
+ Ncv32f *d_weights, Ncv32u weightsStride,
+ HaarFeature64 *d_Features, HaarClassifierNode128 *d_ClassifierNodes, HaarStage64 *d_Stages,
+ Ncv32u *d_inMask, Ncv32u *d_outMask,
+ Ncv32u mask1Dlen, Ncv32u mask2Dstride,
+ NcvSize32u anchorsRoi, Ncv32u startStageInc, Ncv32u endStageExc, Ncv32f scaleArea)
+{
+ Ncv32u y_offs;
+ Ncv32u x_offs;
+ Ncv32u maskOffset;
+ Ncv32u outMaskVal;
+
+ NcvBool bInactiveThread = false;
+
+ if (tbReadPixelIndexFromVector)
+ {
+ maskOffset = (MAX_GRID_DIM * blockIdx.y + blockIdx.x) * NUM_THREADS_ANCHORSPARALLEL + threadIdx.x;
+
+ if (maskOffset >= mask1Dlen)
+ {
+ if (tbDoAtomicCompaction) bInactiveThread = true; else return;
+ }
+
+ if (!tbDoAtomicCompaction || tbDoAtomicCompaction && !bInactiveThread)
+ {
+ outMaskVal = d_inMask[maskOffset];
+ y_offs = outMaskVal >> 16;
+ x_offs = outMaskVal & 0xFFFF;
+ }
+ }
+ else
+ {
+ y_offs = blockIdx.y;
+ x_offs = blockIdx.x * NUM_THREADS_ANCHORSPARALLEL + threadIdx.x;
+
+ if (x_offs >= mask2Dstride)
+ {
+ if (tbDoAtomicCompaction) bInactiveThread = true; else return;
+ }
+
+ if (!tbDoAtomicCompaction || tbDoAtomicCompaction && !bInactiveThread)
+ {
+ maskOffset = y_offs * mask2Dstride + x_offs;
+
+ if ((x_offs >= anchorsRoi.width) ||
+ (!tbInitMaskPositively &&
+ d_inMask != d_outMask &&
+ d_inMask[maskOffset] == OBJDET_MASK_ELEMENT_INVALID_32U))
+ {
+ if (tbDoAtomicCompaction)
+ {
+ bInactiveThread = true;
+ }
+ else
+ {
+ d_outMask[maskOffset] = OBJDET_MASK_ELEMENT_INVALID_32U;
+ return;
+ }
+ }
+
+ outMaskVal = (y_offs << 16) | x_offs;
+ }
+ }
+
+ NcvBool bPass = true;
+
+ if (!tbDoAtomicCompaction || tbDoAtomicCompaction)
+ {
+ Ncv32f pixelStdDev = 0.0f;
+
+ if (!bInactiveThread)
+ pixelStdDev = d_weights[y_offs * weightsStride + x_offs];
+
+ for (Ncv32u iStage = startStageInc; iStage < endStageExc; iStage++)
+ {
+ Ncv32f curStageSum = 0.0f;
+
+ HaarStage64 curStage = getStage(iStage, d_Stages);
+ Ncv32u numRootNodesInStage = curStage.getNumClassifierRootNodes();
+ Ncv32u curRootNodeOffset = curStage.getStartClassifierRootNodeOffset();
+ Ncv32f stageThreshold = curStage.getStageThreshold();
+
+ while (numRootNodesInStage--)
+ {
+ NcvBool bMoreNodesToTraverse = true;
+ Ncv32u iNode = curRootNodeOffset;
+
+ if (bPass && !bInactiveThread)
+ {
+ while (bMoreNodesToTraverse)
+ {
+ HaarClassifierNode128 curNode = getClassifierNode<tbCacheTextureCascade>(iNode, d_ClassifierNodes);
+ HaarFeatureDescriptor32 featuresDesc = curNode.getFeatureDesc();
+ Ncv32u curNodeFeaturesNum = featuresDesc.getNumFeatures();
+ Ncv32u iFeature = featuresDesc.getFeaturesOffset();
+
+ Ncv32f curNodeVal = 0.0f;
+
+ for (Ncv32u iRect=0; iRect<curNodeFeaturesNum; iRect++)
+ {
+ Ncv32f rectWeight;
+ Ncv32u rectX, rectY, rectWidth, rectHeight;
+ getFeature<tbCacheTextureCascade>
+ (iFeature + iRect, d_Features,
+ &rectWeight, &rectX, &rectY, &rectWidth, &rectHeight);
+
+ Ncv32u iioffsTL = (y_offs + rectY) * IImgStride + (x_offs + rectX);
+ Ncv32u iioffsTR = iioffsTL + rectWidth;
+ Ncv32u iioffsBL = iioffsTL + rectHeight * IImgStride;
+ Ncv32u iioffsBR = iioffsBL + rectWidth;
+
+ Ncv32u rectSum = getElemIImg<tbCacheTextureIImg>(iioffsBR, d_IImg) -
+ getElemIImg<tbCacheTextureIImg>(iioffsBL, d_IImg) +
+ getElemIImg<tbCacheTextureIImg>(iioffsTL, d_IImg) -
+ getElemIImg<tbCacheTextureIImg>(iioffsTR, d_IImg);
+
+ #if defined CPU_FP_COMPLIANCE || defined DISABLE_MAD_SELECTIVELY
+ curNodeVal += __fmul_rn((Ncv32f)rectSum, rectWeight);
+ #else
+ curNodeVal += (Ncv32f)rectSum * rectWeight;
+ #endif
+ }
+
+ HaarClassifierNodeDescriptor32 nodeLeft = curNode.getLeftNodeDesc();
+ HaarClassifierNodeDescriptor32 nodeRight = curNode.getRightNodeDesc();
+ Ncv32f nodeThreshold = curNode.getThreshold();
+
+ HaarClassifierNodeDescriptor32 nextNodeDescriptor;
+ NcvBool nextNodeIsLeaf;
+
+ if (curNodeVal < scaleArea * pixelStdDev * nodeThreshold)
+ {
+ nextNodeDescriptor = nodeLeft;
+ nextNodeIsLeaf = featuresDesc.isLeftNodeLeaf();
+ }
+ else
+ {
+ nextNodeDescriptor = nodeRight;
+ nextNodeIsLeaf = featuresDesc.isRightNodeLeaf();
+ }
+
+ if (nextNodeIsLeaf)
+ {
+ Ncv32f tmpLeafValue = nextNodeDescriptor.getLeafValue();
+ curStageSum += tmpLeafValue;
+ bMoreNodesToTraverse = false;
+ }
+ else
+ {
+ iNode = nextNodeDescriptor.getNextNodeOffset();
+ }
+ }
+ }
+
+ __syncthreads();
+ curRootNodeOffset++;
+ }
+
+ if (curStageSum < stageThreshold)
+ {
+ bPass = false;
+ outMaskVal = OBJDET_MASK_ELEMENT_INVALID_32U;
+ }
+ }
+ }
+
+ __syncthreads();
+
+ if (!tbDoAtomicCompaction)
+ {
+ if (!tbReadPixelIndexFromVector ||
+ (tbReadPixelIndexFromVector && (!bPass || d_inMask != d_outMask)))
+ {
+ d_outMask[maskOffset] = outMaskVal;
+ }
+ }
+ else
+ {
+ compactBlockWriteOutAnchorParallel(bPass && !bInactiveThread,
+ outMaskVal,
+ d_outMask);
+ }
+}
+
+
+template <NcvBool tbCacheTextureIImg,
+ NcvBool tbCacheTextureCascade,
+ NcvBool tbDoAtomicCompaction>
+__global__ void applyHaarClassifierClassifierParallel(Ncv32u *d_IImg, Ncv32u IImgStride,
+ Ncv32f *d_weights, Ncv32u weightsStride,
+ HaarFeature64 *d_Features, HaarClassifierNode128 *d_ClassifierNodes, HaarStage64 *d_Stages,
+ Ncv32u *d_inMask, Ncv32u *d_outMask,
+ Ncv32u mask1Dlen, Ncv32u mask2Dstride,
+ NcvSize32u anchorsRoi, Ncv32u startStageInc, Ncv32u endStageExc, Ncv32f scaleArea)
+{
+ Ncv32u maskOffset = MAX_GRID_DIM * blockIdx.y + blockIdx.x;
+
+ if (maskOffset >= mask1Dlen)
+ {
+ return;
+ }
+
+ Ncv32u outMaskVal = d_inMask[maskOffset];
+ Ncv32u y_offs = outMaskVal >> 16;
+ Ncv32u x_offs = outMaskVal & 0xFFFF;
+
+ Ncv32f pixelStdDev = d_weights[y_offs * weightsStride + x_offs];
+ NcvBool bPass = true;
+
+ for (Ncv32u iStage = startStageInc; iStage<endStageExc; iStage++)
+ {
+ //this variable is subject to reduction
+ Ncv32f curStageSum = 0.0f;
+
+ HaarStage64 curStage = getStage(iStage, d_Stages);
+ Ncv32s numRootNodesInStage = curStage.getNumClassifierRootNodes();
+ Ncv32u curRootNodeOffset = curStage.getStartClassifierRootNodeOffset() + threadIdx.x;
+ Ncv32f stageThreshold = curStage.getStageThreshold();
+
+ Ncv32u numRootChunks = (numRootNodesInStage + NUM_THREADS_CLASSIFIERPARALLEL - 1) >> NUM_THREADS_CLASSIFIERPARALLEL_LOG2;
+
+ for (Ncv32u chunkId=0; chunkId<numRootChunks; chunkId++)
+ {
+ NcvBool bMoreNodesToTraverse = true;
+
+ if (chunkId * NUM_THREADS_CLASSIFIERPARALLEL + threadIdx.x < numRootNodesInStage)
+ {
+ Ncv32u iNode = curRootNodeOffset;
+
+ while (bMoreNodesToTraverse)
+ {
+ HaarClassifierNode128 curNode = getClassifierNode<tbCacheTextureCascade>(iNode, d_ClassifierNodes);
+ HaarFeatureDescriptor32 featuresDesc = curNode.getFeatureDesc();
+ Ncv32u curNodeFeaturesNum = featuresDesc.getNumFeatures();
+ Ncv32u iFeature = featuresDesc.getFeaturesOffset();
+
+ Ncv32f curNodeVal = 0.0f;
+ //TODO: fetch into shmem if size suffices. Shmem can be shared with reduce
+ for (Ncv32u iRect=0; iRect<curNodeFeaturesNum; iRect++)
+ {
+ Ncv32f rectWeight;
+ Ncv32u rectX, rectY, rectWidth, rectHeight;
+ getFeature<tbCacheTextureCascade>
+ (iFeature + iRect, d_Features,
+ &rectWeight, &rectX, &rectY, &rectWidth, &rectHeight);
+
+ Ncv32u iioffsTL = (y_offs + rectY) * IImgStride + (x_offs + rectX);
+ Ncv32u iioffsTR = iioffsTL + rectWidth;
+ Ncv32u iioffsBL = iioffsTL + rectHeight * IImgStride;
+ Ncv32u iioffsBR = iioffsBL + rectWidth;
+
+ Ncv32u rectSum = getElemIImg<tbCacheTextureIImg>(iioffsBR, d_IImg) -
+ getElemIImg<tbCacheTextureIImg>(iioffsBL, d_IImg) +
+ getElemIImg<tbCacheTextureIImg>(iioffsTL, d_IImg) -
+ getElemIImg<tbCacheTextureIImg>(iioffsTR, d_IImg);
+
+#if defined CPU_FP_COMPLIANCE || defined DISABLE_MAD_SELECTIVELY
+ curNodeVal += __fmul_rn((Ncv32f)rectSum, rectWeight);
+#else
+ curNodeVal += (Ncv32f)rectSum * rectWeight;
+#endif
+ }
+
+ HaarClassifierNodeDescriptor32 nodeLeft = curNode.getLeftNodeDesc();
+ HaarClassifierNodeDescriptor32 nodeRight = curNode.getRightNodeDesc();
+ Ncv32f nodeThreshold = curNode.getThreshold();
+
+ HaarClassifierNodeDescriptor32 nextNodeDescriptor;
+ NcvBool nextNodeIsLeaf;
+
+ if (curNodeVal < scaleArea * pixelStdDev * nodeThreshold)
+ {
+ nextNodeDescriptor = nodeLeft;
+ nextNodeIsLeaf = featuresDesc.isLeftNodeLeaf();
+ }
+ else
+ {
+ nextNodeDescriptor = nodeRight;
+ nextNodeIsLeaf = featuresDesc.isRightNodeLeaf();
+ }
+
+ if (nextNodeIsLeaf)
+ {
+ Ncv32f tmpLeafValue = nextNodeDescriptor.getLeafValue();
+ curStageSum += tmpLeafValue;
+ bMoreNodesToTraverse = false;
+ }
+ else
+ {
+ iNode = nextNodeDescriptor.getNextNodeOffset();
+ }
+ }
+ }
+ __syncthreads();
+
+ curRootNodeOffset += NUM_THREADS_CLASSIFIERPARALLEL;
+ }
+
+ Ncv32f finalStageSum = subReduce<Ncv32f, functorAddValues<Ncv32f>, NUM_THREADS_CLASSIFIERPARALLEL>(curStageSum);
+
+ if (finalStageSum < stageThreshold)
+ {
+ bPass = false;
+ outMaskVal = OBJDET_MASK_ELEMENT_INVALID_32U;
+ break;
+ }
+ }
+
+ if (!tbDoAtomicCompaction)
+ {
+ if (!bPass || d_inMask != d_outMask)
+ {
+ if (!threadIdx.x)
+ {
+ d_outMask[maskOffset] = outMaskVal;
+ }
+ }
+ }
+ else
+ {
+#if __CUDA_ARCH__ >= 110
+ if (bPass && !threadIdx.x)
+ {
+ Ncv32u outMaskOffset = atomicAdd(&d_outMaskPosition, 1);
+ d_outMask[outMaskOffset] = outMaskVal;
+ }
+#endif
+ }
+}
+
+
+template <NcvBool tbMaskByInmask,
+ NcvBool tbDoAtomicCompaction>
+__global__ void initializeMaskVector(Ncv32u *d_inMask, Ncv32u *d_outMask,
+ Ncv32u mask1Dlen, Ncv32u mask2Dstride,
+ NcvSize32u anchorsRoi, Ncv32u step)
+{
+ Ncv32u y_offs = blockIdx.y;
+ Ncv32u x_offs = blockIdx.x * NUM_THREADS_ANCHORSPARALLEL + threadIdx.x;
+ Ncv32u outMaskOffset = y_offs * gridDim.x * blockDim.x + x_offs;
+
+ Ncv32u y_offs_upsc = step * y_offs;
+ Ncv32u x_offs_upsc = step * x_offs;
+ Ncv32u inMaskOffset = y_offs_upsc * mask2Dstride + x_offs_upsc;
+
+ Ncv32u outElem = OBJDET_MASK_ELEMENT_INVALID_32U;
+
+ if (x_offs_upsc < anchorsRoi.width &&
+ (!tbMaskByInmask || d_inMask[inMaskOffset] != OBJDET_MASK_ELEMENT_INVALID_32U))
+ {
+ outElem = (y_offs_upsc << 16) | x_offs_upsc;
+ }
+
+ if (!tbDoAtomicCompaction)
+ {
+ d_outMask[outMaskOffset] = outElem;
+ }
+ else
+ {
+ compactBlockWriteOutAnchorParallel(outElem != OBJDET_MASK_ELEMENT_INVALID_32U,
+ outElem,
+ d_outMask);
+ }
+}
+
+
+struct applyHaarClassifierAnchorParallelFunctor
+{
+ dim3 gridConf, blockConf;
+ cudaStream_t cuStream;
+
+ //Kernel arguments are stored as members;
+ Ncv32u *d_IImg;
+ Ncv32u IImgStride;
+ Ncv32f *d_weights;
+ Ncv32u weightsStride;
+ HaarFeature64 *d_Features;
+ HaarClassifierNode128 *d_ClassifierNodes;
+ HaarStage64 *d_Stages;
+ Ncv32u *d_inMask;
+ Ncv32u *d_outMask;
+ Ncv32u mask1Dlen;
+ Ncv32u mask2Dstride;
+ NcvSize32u anchorsRoi;
+ Ncv32u startStageInc;
+ Ncv32u endStageExc;
+ Ncv32f scaleArea;
+
+ //Arguments are passed through the constructor
+ applyHaarClassifierAnchorParallelFunctor(dim3 _gridConf, dim3 _blockConf, cudaStream_t _cuStream,
+ Ncv32u *_d_IImg, Ncv32u _IImgStride,
+ Ncv32f *_d_weights, Ncv32u _weightsStride,
+ HaarFeature64 *_d_Features, HaarClassifierNode128 *_d_ClassifierNodes, HaarStage64 *_d_Stages,
+ Ncv32u *_d_inMask, Ncv32u *_d_outMask,
+ Ncv32u _mask1Dlen, Ncv32u _mask2Dstride,
+ NcvSize32u _anchorsRoi, Ncv32u _startStageInc,
+ Ncv32u _endStageExc, Ncv32f _scaleArea) :
+ gridConf(_gridConf),
+ blockConf(_blockConf),
+ cuStream(_cuStream),
+ d_IImg(_d_IImg),
+ IImgStride(_IImgStride),
+ d_weights(_d_weights),
+ weightsStride(_weightsStride),
+ d_Features(_d_Features),
+ d_ClassifierNodes(_d_ClassifierNodes),
+ d_Stages(_d_Stages),
+ d_inMask(_d_inMask),
+ d_outMask(_d_outMask),
+ mask1Dlen(_mask1Dlen),
+ mask2Dstride(_mask2Dstride),
+ anchorsRoi(_anchorsRoi),
+ startStageInc(_startStageInc),
+ endStageExc(_endStageExc),
+ scaleArea(_scaleArea)
+ {}
+
+ template<class TList>
+ void call(TList tl)
+ {
+ applyHaarClassifierAnchorParallel <
+ Loki::TL::TypeAt<TList, 0>::Result::value,
+ Loki::TL::TypeAt<TList, 1>::Result::value,
+ Loki::TL::TypeAt<TList, 2>::Result::value,
+ Loki::TL::TypeAt<TList, 3>::Result::value,
+ Loki::TL::TypeAt<TList, 4>::Result::value >
+ <<<gridConf, blockConf, 0, cuStream>>>
+ (d_IImg, IImgStride,
+ d_weights, weightsStride,
+ d_Features, d_ClassifierNodes, d_Stages,
+ d_inMask, d_outMask,
+ mask1Dlen, mask2Dstride,
+ anchorsRoi, startStageInc,
+ endStageExc, scaleArea);
+ }
+};
+
+
+void applyHaarClassifierAnchorParallelDynTemplate(NcvBool tbInitMaskPositively,
+ NcvBool tbCacheTextureIImg,
+ NcvBool tbCacheTextureCascade,
+ NcvBool tbReadPixelIndexFromVector,
+ NcvBool tbDoAtomicCompaction,
+
+ dim3 gridConf, dim3 blockConf, cudaStream_t cuStream,
+
+ Ncv32u *d_IImg, Ncv32u IImgStride,
+ Ncv32f *d_weights, Ncv32u weightsStride,
+ HaarFeature64 *d_Features, HaarClassifierNode128 *d_ClassifierNodes, HaarStage64 *d_Stages,
+ Ncv32u *d_inMask, Ncv32u *d_outMask,
+ Ncv32u mask1Dlen, Ncv32u mask2Dstride,
+ NcvSize32u anchorsRoi, Ncv32u startStageInc,
+ Ncv32u endStageExc, Ncv32f scaleArea)
+{
+
+ applyHaarClassifierAnchorParallelFunctor functor(gridConf, blockConf, cuStream,
+ d_IImg, IImgStride,
+ d_weights, weightsStride,
+ d_Features, d_ClassifierNodes, d_Stages,
+ d_inMask, d_outMask,
+ mask1Dlen, mask2Dstride,
+ anchorsRoi, startStageInc,
+ endStageExc, scaleArea);
+
+ //Second parameter is the number of "dynamic" template parameters
+ NCVRuntimeTemplateBool::KernelCaller<Loki::NullType, 5, applyHaarClassifierAnchorParallelFunctor>
+ ::call( &functor,
+ tbInitMaskPositively,
+ tbCacheTextureIImg,
+ tbCacheTextureCascade,
+ tbReadPixelIndexFromVector,
+ tbDoAtomicCompaction);
+}
+
+
+struct applyHaarClassifierClassifierParallelFunctor
+{
+ dim3 gridConf, blockConf;
+ cudaStream_t cuStream;
+
+ //Kernel arguments are stored as members;
+ Ncv32u *d_IImg;
+ Ncv32u IImgStride;
+ Ncv32f *d_weights;
+ Ncv32u weightsStride;
+ HaarFeature64 *d_Features;
+ HaarClassifierNode128 *d_ClassifierNodes;
+ HaarStage64 *d_Stages;
+ Ncv32u *d_inMask;
+ Ncv32u *d_outMask;
+ Ncv32u mask1Dlen;
+ Ncv32u mask2Dstride;
+ NcvSize32u anchorsRoi;
+ Ncv32u startStageInc;
+ Ncv32u endStageExc;
+ Ncv32f scaleArea;
+
+ //Arguments are passed through the constructor
+ applyHaarClassifierClassifierParallelFunctor(dim3 _gridConf, dim3 _blockConf, cudaStream_t _cuStream,
+ Ncv32u *_d_IImg, Ncv32u _IImgStride,
+ Ncv32f *_d_weights, Ncv32u _weightsStride,
+ HaarFeature64 *_d_Features, HaarClassifierNode128 *_d_ClassifierNodes, HaarStage64 *_d_Stages,
+ Ncv32u *_d_inMask, Ncv32u *_d_outMask,
+ Ncv32u _mask1Dlen, Ncv32u _mask2Dstride,
+ NcvSize32u _anchorsRoi, Ncv32u _startStageInc,
+ Ncv32u _endStageExc, Ncv32f _scaleArea) :
+ gridConf(_gridConf),
+ blockConf(_blockConf),
+ cuStream(_cuStream),
+ d_IImg(_d_IImg),
+ IImgStride(_IImgStride),
+ d_weights(_d_weights),
+ weightsStride(_weightsStride),
+ d_Features(_d_Features),
+ d_ClassifierNodes(_d_ClassifierNodes),
+ d_Stages(_d_Stages),
+ d_inMask(_d_inMask),
+ d_outMask(_d_outMask),
+ mask1Dlen(_mask1Dlen),
+ mask2Dstride(_mask2Dstride),
+ anchorsRoi(_anchorsRoi),
+ startStageInc(_startStageInc),
+ endStageExc(_endStageExc),
+ scaleArea(_scaleArea)
+ {}
+
+ template<class TList>
+ void call(TList tl)
+ {
+ applyHaarClassifierClassifierParallel <
+ Loki::TL::TypeAt<TList, 0>::Result::value,
+ Loki::TL::TypeAt<TList, 1>::Result::value,
+ Loki::TL::TypeAt<TList, 2>::Result::value >
+ <<<gridConf, blockConf, 0, cuStream>>>
+ (d_IImg, IImgStride,
+ d_weights, weightsStride,
+ d_Features, d_ClassifierNodes, d_Stages,
+ d_inMask, d_outMask,
+ mask1Dlen, mask2Dstride,
+ anchorsRoi, startStageInc,
+ endStageExc, scaleArea);
+ }
+};
+
+
+void applyHaarClassifierClassifierParallelDynTemplate(NcvBool tbCacheTextureIImg,
+ NcvBool tbCacheTextureCascade,
+ NcvBool tbDoAtomicCompaction,
+
+ dim3 gridConf, dim3 blockConf, cudaStream_t cuStream,
+
+ Ncv32u *d_IImg, Ncv32u IImgStride,
+ Ncv32f *d_weights, Ncv32u weightsStride,
+ HaarFeature64 *d_Features, HaarClassifierNode128 *d_ClassifierNodes, HaarStage64 *d_Stages,
+ Ncv32u *d_inMask, Ncv32u *d_outMask,
+ Ncv32u mask1Dlen, Ncv32u mask2Dstride,
+ NcvSize32u anchorsRoi, Ncv32u startStageInc,
+ Ncv32u endStageExc, Ncv32f scaleArea)
+{
+ applyHaarClassifierClassifierParallelFunctor functor(gridConf, blockConf, cuStream,
+ d_IImg, IImgStride,
+ d_weights, weightsStride,
+ d_Features, d_ClassifierNodes, d_Stages,
+ d_inMask, d_outMask,
+ mask1Dlen, mask2Dstride,
+ anchorsRoi, startStageInc,
+ endStageExc, scaleArea);
+
+ //Second parameter is the number of "dynamic" template parameters
+ NCVRuntimeTemplateBool::KernelCaller<Loki::NullType, 3, applyHaarClassifierClassifierParallelFunctor>
+ ::call( &functor,
+ tbCacheTextureIImg,
+ tbCacheTextureCascade,
+ tbDoAtomicCompaction);
+}
+
+
+struct initializeMaskVectorFunctor
+{
+ dim3 gridConf, blockConf;
+ cudaStream_t cuStream;
+
+ //Kernel arguments are stored as members;
+ Ncv32u *d_inMask;
+ Ncv32u *d_outMask;
+ Ncv32u mask1Dlen;
+ Ncv32u mask2Dstride;
+ NcvSize32u anchorsRoi;
+ Ncv32u step;
+
+ //Arguments are passed through the constructor
+ initializeMaskVectorFunctor(dim3 _gridConf, dim3 _blockConf, cudaStream_t _cuStream,
+ Ncv32u *_d_inMask, Ncv32u *_d_outMask,
+ Ncv32u _mask1Dlen, Ncv32u _mask2Dstride,
+ NcvSize32u _anchorsRoi, Ncv32u _step) :
+ gridConf(_gridConf),
+ blockConf(_blockConf),
+ cuStream(_cuStream),
+ d_inMask(_d_inMask),
+ d_outMask(_d_outMask),
+ mask1Dlen(_mask1Dlen),
+ mask2Dstride(_mask2Dstride),
+ anchorsRoi(_anchorsRoi),
+ step(_step)
+ {}
+
+ template<class TList>
+ void call(TList tl)
+ {
+ initializeMaskVector <
+ Loki::TL::TypeAt<TList, 0>::Result::value,
+ Loki::TL::TypeAt<TList, 1>::Result::value >
+ <<<gridConf, blockConf, 0, cuStream>>>
+ (d_inMask, d_outMask,
+ mask1Dlen, mask2Dstride,
+ anchorsRoi, step);
+ }
+};
+
+
+void initializeMaskVectorDynTemplate(NcvBool tbMaskByInmask,
+ NcvBool tbDoAtomicCompaction,
+
+ dim3 gridConf, dim3 blockConf, cudaStream_t cuStream,
+
+ Ncv32u *d_inMask, Ncv32u *d_outMask,
+ Ncv32u mask1Dlen, Ncv32u mask2Dstride,
+ NcvSize32u anchorsRoi, Ncv32u step)
+{
+ initializeMaskVectorFunctor functor(gridConf, blockConf, cuStream,
+ d_inMask, d_outMask,
+ mask1Dlen, mask2Dstride,
+ anchorsRoi, step);
+
+ //Second parameter is the number of "dynamic" template parameters
+ NCVRuntimeTemplateBool::KernelCaller<Loki::NullType, 2, initializeMaskVectorFunctor>
+ ::call( &functor,
+ tbMaskByInmask,
+ tbDoAtomicCompaction);
+}
+
+
+Ncv32u getStageNumWithNotLessThanNclassifiers(Ncv32u N, HaarClassifierCascadeDescriptor &haar,
+ NCVVector<HaarStage64> &h_HaarStages)
+{
+ Ncv32u i = 0;
+ for (; i<haar.NumStages; i++)
+ {
+ if (h_HaarStages.ptr()[i].getNumClassifierRootNodes() >= N)
+ {
+ break;
+ }
+ }
+ return i;
+}
+
+
+NCVStatus ncvApplyHaarClassifierCascade_device(NCVMatrix<Ncv32u> &d_integralImage,
+ NCVMatrix<Ncv32f> &d_weights,
+ NCVMatrixAlloc<Ncv32u> &d_pixelMask,
+ Ncv32u &numDetections,
+ HaarClassifierCascadeDescriptor &haar,
+ NCVVector<HaarStage64> &h_HaarStages,
+ NCVVector<HaarStage64> &d_HaarStages,
+ NCVVector<HaarClassifierNode128> &d_HaarNodes,
+ NCVVector<HaarFeature64> &d_HaarFeatures,
+ NcvBool bMaskElements,
+ NcvSize32u anchorsRoi,
+ Ncv32u pixelStep,
+ Ncv32f scaleArea,
+ INCVMemAllocator &gpuAllocator,
+ INCVMemAllocator &cpuAllocator,
+ cudaDeviceProp &devProp,
+ cudaStream_t cuStream)
+{
+ ncvAssertReturn(d_integralImage.memType() == d_weights.memType() &&
+ d_integralImage.memType() == d_pixelMask.memType() &&
+ d_integralImage.memType() == gpuAllocator.memType() &&
+ (d_integralImage.memType() == NCVMemoryTypeDevice ||
+ d_integralImage.memType() == NCVMemoryTypeNone), NCV_MEM_RESIDENCE_ERROR);
+ ncvAssertReturn(d_HaarStages.memType() == d_HaarNodes.memType() &&
+ d_HaarStages.memType() == d_HaarFeatures.memType() &&
+ (d_HaarStages.memType() == NCVMemoryTypeDevice ||
+ d_HaarStages.memType() == NCVMemoryTypeNone), NCV_MEM_RESIDENCE_ERROR);
+ ncvAssertReturn(h_HaarStages.memType() != NCVMemoryTypeDevice, NCV_MEM_RESIDENCE_ERROR);
+ ncvAssertReturn(gpuAllocator.isInitialized() && cpuAllocator.isInitialized(), NCV_ALLOCATOR_NOT_INITIALIZED);
+ ncvAssertReturn((d_integralImage.ptr() != NULL && d_weights.ptr() != NULL && d_pixelMask.ptr() != NULL &&
+ h_HaarStages.ptr() != NULL && d_HaarStages.ptr() != NULL && d_HaarNodes.ptr() != NULL &&
+ d_HaarFeatures.ptr() != NULL) || gpuAllocator.isCounting(), NCV_NULL_PTR);
+ ncvAssertReturn(anchorsRoi.width > 0 && anchorsRoi.height > 0 &&
+ d_pixelMask.width() >= anchorsRoi.width && d_pixelMask.height() >= anchorsRoi.height &&
+ d_weights.width() >= anchorsRoi.width && d_weights.height() >= anchorsRoi.height &&
+ d_integralImage.width() >= anchorsRoi.width + haar.ClassifierSize.width &&
+ d_integralImage.height() >= anchorsRoi.height + haar.ClassifierSize.height, NCV_DIMENSIONS_INVALID);
+ ncvAssertReturn(scaleArea > 0, NCV_INVALID_SCALE);
+ ncvAssertReturn(d_HaarStages.length() >= haar.NumStages &&
+ d_HaarNodes.length() >= haar.NumClassifierTotalNodes &&
+ d_HaarFeatures.length() >= haar.NumFeatures &&
+ d_HaarStages.length() == h_HaarStages.length() &&
+ haar.NumClassifierRootNodes <= haar.NumClassifierTotalNodes, NCV_DIMENSIONS_INVALID);
+ ncvAssertReturn(haar.bNeedsTiltedII == false || gpuAllocator.isCounting(), NCV_NOIMPL_HAAR_TILTED_FEATURES);
+ ncvAssertReturn(pixelStep == 1 || pixelStep == 2, NCV_HAAR_INVALID_PIXEL_STEP);
+
+ NCV_SET_SKIP_COND(gpuAllocator.isCounting());
+
+#if defined _SELF_TEST_
+
+ NCVStatus ncvStat;
+
+ NCVMatrixAlloc<Ncv32u> h_integralImage(cpuAllocator, d_integralImage.width, d_integralImage.height, d_integralImage.pitch);
+ ncvAssertReturn(h_integralImage.isMemAllocated(), NCV_ALLOCATOR_BAD_ALLOC);
+ NCVMatrixAlloc<Ncv32f> h_weights(cpuAllocator, d_weights.width, d_weights.height, d_weights.pitch);
+ ncvAssertReturn(h_weights.isMemAllocated(), NCV_ALLOCATOR_BAD_ALLOC);
+ NCVMatrixAlloc<Ncv32u> h_pixelMask(cpuAllocator, d_pixelMask.width, d_pixelMask.height, d_pixelMask.pitch);
+ ncvAssertReturn(h_pixelMask.isMemAllocated(), NCV_ALLOCATOR_BAD_ALLOC);
+ NCVVectorAlloc<HaarClassifierNode128> h_HaarNodes(cpuAllocator, d_HaarNodes.length);
+ ncvAssertReturn(h_HaarNodes.isMemAllocated(), NCV_ALLOCATOR_BAD_ALLOC);
+ NCVVectorAlloc<HaarFeature64> h_HaarFeatures(cpuAllocator, d_HaarFeatures.length);
+ ncvAssertReturn(h_HaarFeatures.isMemAllocated(), NCV_ALLOCATOR_BAD_ALLOC);
+
+ NCVMatrixAlloc<Ncv32u> h_pixelMask_d(cpuAllocator, d_pixelMask.width, d_pixelMask.height, d_pixelMask.pitch);
+ ncvAssertReturn(h_pixelMask_d.isMemAllocated(), NCV_ALLOCATOR_BAD_ALLOC);
+
+ NCV_SKIP_COND_BEGIN
+
+ ncvStat = d_pixelMask.copySolid(h_pixelMask, 0);
+ ncvAssertReturnNcvStat(ncvStat);
+ ncvStat = d_integralImage.copySolid(h_integralImage, 0);
+ ncvAssertReturnNcvStat(ncvStat);
+ ncvStat = d_weights.copySolid(h_weights, 0);
+ ncvAssertReturnNcvStat(ncvStat);
+ ncvStat = d_HaarNodes.copySolid(h_HaarNodes, 0);
+ ncvAssertReturnNcvStat(ncvStat);
+ ncvStat = d_HaarFeatures.copySolid(h_HaarFeatures, 0);
+ ncvAssertReturnNcvStat(ncvStat);
+ ncvAssertCUDAReturn(cudaStreamSynchronize(0), NCV_CUDA_ERROR);
+
+ for (Ncv32u i=0; i<(Ncv32u)anchorsRoi.height; i++)
+ {
+ for (Ncv32u j=0; j<d_pixelMask.stride(); j++)
+ {
+ if ((i%pixelStep==0) && (j%pixelStep==0) && (j<(Ncv32u)anchorsRoi.width))
+ {
+ if (!bMaskElements || h_pixelMask.ptr[i*d_pixelMask.stride()+j] != OBJDET_MASK_ELEMENT_INVALID_32U)
+ {
+ h_pixelMask.ptr[i*d_pixelMask.stride()+j] = (i << 16) | j;
+ }
+ }
+ else
+ {
+ h_pixelMask.ptr[i*d_pixelMask.stride()+j] = OBJDET_MASK_ELEMENT_INVALID_32U;
+ }
+ }
+ }
+
+ NCV_SKIP_COND_END
+
+#endif
+
+ NCVVectorReuse<Ncv32u> d_vecPixelMask(d_pixelMask.getSegment(), anchorsRoi.height * d_pixelMask.stride());
+ ncvAssertReturn(d_vecPixelMask.isMemReused(), NCV_ALLOCATOR_BAD_REUSE);
+
+ NCVVectorAlloc<Ncv32u> d_vecPixelMaskTmp(gpuAllocator, static_cast<Ncv32u>(d_vecPixelMask.length()));
+ ncvAssertReturn(d_vecPixelMaskTmp.isMemAllocated(), NCV_ALLOCATOR_BAD_ALLOC);
+
+ NCVVectorAlloc<Ncv32u> hp_pool32u(cpuAllocator, 2);
+ ncvAssertReturn(hp_pool32u.isMemAllocated(), NCV_ALLOCATOR_BAD_ALLOC);
+ Ncv32u *hp_zero = &hp_pool32u.ptr()[0];
+ Ncv32u *hp_numDet = &hp_pool32u.ptr()[1];
+
+ NCV_SKIP_COND_BEGIN
+ *hp_zero = 0;
+ *hp_numDet = 0;
+ NCV_SKIP_COND_END
+
+ Ncv32f scaleAreaPixels = scaleArea * ((haar.ClassifierSize.width - 2*HAAR_STDDEV_BORDER) *
+ (haar.ClassifierSize.height - 2*HAAR_STDDEV_BORDER));
+
+ NcvBool bTexCacheCascade = devProp.major < 2;
+ NcvBool bTexCacheIImg = true; //this works better even on Fermi so far
+ NcvBool bDoAtomicCompaction = devProp.major >= 2 || (devProp.major == 1 && devProp.minor >= 3);
+
+ NCVVector<Ncv32u> *d_ptrNowData = &d_vecPixelMask;
+ NCVVector<Ncv32u> *d_ptrNowTmp = &d_vecPixelMaskTmp;
+
+ Ncv32u szNppCompactTmpBuf;
+ nppsStCompactGetSize_32u(static_cast<Ncv32u>(d_vecPixelMask.length()), &szNppCompactTmpBuf, devProp);
+ if (bDoAtomicCompaction)
+ {
+ szNppCompactTmpBuf = 0;
+ }
+ NCVVectorAlloc<Ncv8u> d_tmpBufCompact(gpuAllocator, szNppCompactTmpBuf);
+
+ NCV_SKIP_COND_BEGIN
+
+ if (bTexCacheIImg)
+ {
+ cudaChannelFormatDesc cfdTexIImage;
+ cfdTexIImage = cudaCreateChannelDesc<Ncv32u>();
+
+ size_t alignmentOffset;
+ ncvAssertCUDAReturn(cudaBindTexture(&alignmentOffset, texIImage, d_integralImage.ptr(), cfdTexIImage,
+ (anchorsRoi.height + haar.ClassifierSize.height) * d_integralImage.pitch()), NCV_CUDA_ERROR);
+ ncvAssertReturn(alignmentOffset==0, NCV_TEXTURE_BIND_ERROR);
+ }
+
+ if (bTexCacheCascade)
+ {
+ cudaChannelFormatDesc cfdTexHaarFeatures;
+ cudaChannelFormatDesc cfdTexHaarClassifierNodes;
+ cfdTexHaarFeatures = cudaCreateChannelDesc<uint2>();
+ cfdTexHaarClassifierNodes = cudaCreateChannelDesc<uint4>();
+
+ size_t alignmentOffset;
+ ncvAssertCUDAReturn(cudaBindTexture(&alignmentOffset, texHaarFeatures,
+ d_HaarFeatures.ptr(), cfdTexHaarFeatures,sizeof(HaarFeature64) * haar.NumFeatures), NCV_CUDA_ERROR);
+ ncvAssertReturn(alignmentOffset==0, NCV_TEXTURE_BIND_ERROR);
+ ncvAssertCUDAReturn(cudaBindTexture(&alignmentOffset, texHaarClassifierNodes,
+ d_HaarNodes.ptr(), cfdTexHaarClassifierNodes, sizeof(HaarClassifierNode128) * haar.NumClassifierTotalNodes), NCV_CUDA_ERROR);
+ ncvAssertReturn(alignmentOffset==0, NCV_TEXTURE_BIND_ERROR);
+ }
+
+ Ncv32u stageStartAnchorParallel = 0;
+ Ncv32u stageMiddleSwitch = getStageNumWithNotLessThanNclassifiers(NUM_THREADS_CLASSIFIERPARALLEL,
+ haar, h_HaarStages);
+ Ncv32u stageEndClassifierParallel = haar.NumStages;
+ if (stageMiddleSwitch == 0)
+ {
+ stageMiddleSwitch = 1;
+ }
+
+ //create stages subdivision for pixel-parallel processing
+ const Ncv32u compactEveryNstage = bDoAtomicCompaction ? 7 : 1;
+ Ncv32u curStop = stageStartAnchorParallel;
+ std::vector<Ncv32u> pixParallelStageStops;
+ while (curStop < stageMiddleSwitch)
+ {
+ pixParallelStageStops.push_back(curStop);
+ curStop += compactEveryNstage;
+ }
+ if (curStop > compactEveryNstage && curStop - stageMiddleSwitch > compactEveryNstage / 2)
+ {
+ pixParallelStageStops[pixParallelStageStops.size()-1] =
+ (stageMiddleSwitch - (curStop - 2 * compactEveryNstage)) / 2;
+ }
+ pixParallelStageStops.push_back(stageMiddleSwitch);
+ Ncv32u pixParallelStageStopsIndex = 0;
+
+ if (pixelStep != 1 || bMaskElements)
+ {
+ if (bDoAtomicCompaction)
+ {
+ ncvAssertCUDAReturn(cudaMemcpyToSymbolAsync(d_outMaskPosition, hp_zero, sizeof(Ncv32u),
+ 0, cudaMemcpyHostToDevice, cuStream), NCV_CUDA_ERROR);
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+ }
+
+ dim3 gridInit((((anchorsRoi.width + pixelStep - 1) / pixelStep + NUM_THREADS_ANCHORSPARALLEL - 1) / NUM_THREADS_ANCHORSPARALLEL),
+ (anchorsRoi.height + pixelStep - 1) / pixelStep);
+ dim3 blockInit(NUM_THREADS_ANCHORSPARALLEL);
+
+ if (gridInit.x == 0 || gridInit.y == 0)
+ {
+ numDetections = 0;
+ return NCV_SUCCESS;
+ }
+
+ initializeMaskVectorDynTemplate(bMaskElements,
+ bDoAtomicCompaction,
+ gridInit, blockInit, cuStream,
+ d_ptrNowData->ptr(),
+ d_ptrNowTmp->ptr(),
+ static_cast<Ncv32u>(d_vecPixelMask.length()), d_pixelMask.stride(),
+ anchorsRoi, pixelStep);
+ ncvAssertCUDAReturn(cudaGetLastError(), NCV_CUDA_ERROR);
+
+ if (bDoAtomicCompaction)
+ {
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+ ncvAssertCUDAReturn(cudaMemcpyFromSymbolAsync(hp_numDet, d_outMaskPosition, sizeof(Ncv32u),
+ 0, cudaMemcpyDeviceToHost, cuStream), NCV_CUDA_ERROR);
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+ swap(d_ptrNowData, d_ptrNowTmp);
+ }
+ else
+ {
+ NCVStatus nppSt;
+ nppSt = nppsStCompact_32u(d_ptrNowTmp->ptr(), static_cast<Ncv32u>(d_vecPixelMask.length()),
+ d_ptrNowData->ptr(), hp_numDet, OBJDET_MASK_ELEMENT_INVALID_32U,
+ d_tmpBufCompact.ptr(), szNppCompactTmpBuf, devProp);
+ ncvAssertReturn(nppSt == NPPST_SUCCESS, NCV_NPP_ERROR);
+ }
+ numDetections = *hp_numDet;
+ }
+ else
+ {
+ //
+ // 1. Run the first pixel-input pixel-parallel classifier for few stages
+ //
+
+ if (bDoAtomicCompaction)
+ {
+ ncvAssertCUDAReturn(cudaMemcpyToSymbolAsync(d_outMaskPosition, hp_zero, sizeof(Ncv32u),
+ 0, cudaMemcpyHostToDevice, cuStream), NCV_CUDA_ERROR);
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+ }
+
+ dim3 grid1(((d_pixelMask.stride() + NUM_THREADS_ANCHORSPARALLEL - 1) / NUM_THREADS_ANCHORSPARALLEL),
+ anchorsRoi.height);
+ dim3 block1(NUM_THREADS_ANCHORSPARALLEL);
+ applyHaarClassifierAnchorParallelDynTemplate(
+ true, //tbInitMaskPositively
+ bTexCacheIImg, //tbCacheTextureIImg
+ bTexCacheCascade, //tbCacheTextureCascade
+ pixParallelStageStops[pixParallelStageStopsIndex] != 0,//tbReadPixelIndexFromVector
+ bDoAtomicCompaction, //tbDoAtomicCompaction
+ grid1,
+ block1,
+ cuStream,
+ d_integralImage.ptr(), d_integralImage.stride(),
+ d_weights.ptr(), d_weights.stride(),
+ d_HaarFeatures.ptr(), d_HaarNodes.ptr(), d_HaarStages.ptr(),
+ d_ptrNowData->ptr(),
+ bDoAtomicCompaction ? d_ptrNowTmp->ptr() : d_ptrNowData->ptr(),
+ 0,
+ d_pixelMask.stride(),
+ anchorsRoi,
+ pixParallelStageStops[pixParallelStageStopsIndex],
+ pixParallelStageStops[pixParallelStageStopsIndex+1],
+ scaleAreaPixels);
+ ncvAssertCUDAReturn(cudaGetLastError(), NCV_CUDA_ERROR);
+
+ if (bDoAtomicCompaction)
+ {
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+ ncvAssertCUDAReturn(cudaMemcpyFromSymbolAsync(hp_numDet, d_outMaskPosition, sizeof(Ncv32u),
+ 0, cudaMemcpyDeviceToHost, cuStream), NCV_CUDA_ERROR);
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+ }
+ else
+ {
+ NCVStatus nppSt;
+ nppSt = nppsStCompact_32u(d_ptrNowData->ptr(), static_cast<Ncv32u>(d_vecPixelMask.length()),
+ d_ptrNowTmp->ptr(), hp_numDet, OBJDET_MASK_ELEMENT_INVALID_32U,
+ d_tmpBufCompact.ptr(), szNppCompactTmpBuf, devProp);
+ ncvAssertReturnNcvStat(nppSt);
+ }
+
+ swap(d_ptrNowData, d_ptrNowTmp);
+ numDetections = *hp_numDet;
+
+ pixParallelStageStopsIndex++;
+ }
+
+ //
+ // 2. Run pixel-parallel stages
+ //
+
+ for (; pixParallelStageStopsIndex < pixParallelStageStops.size()-1; pixParallelStageStopsIndex++)
+ {
+ if (numDetections == 0)
+ {
+ break;
+ }
+
+ if (bDoAtomicCompaction)
+ {
+ ncvAssertCUDAReturn(cudaMemcpyToSymbolAsync(d_outMaskPosition, hp_zero, sizeof(Ncv32u),
+ 0, cudaMemcpyHostToDevice, cuStream), NCV_CUDA_ERROR);
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+ }
+
+ dim3 grid2((numDetections + NUM_THREADS_ANCHORSPARALLEL - 1) / NUM_THREADS_ANCHORSPARALLEL);
+ if (numDetections > MAX_GRID_DIM)
+ {
+ grid2.x = MAX_GRID_DIM;
+ grid2.y = (numDetections + MAX_GRID_DIM - 1) / MAX_GRID_DIM;
+ }
+ dim3 block2(NUM_THREADS_ANCHORSPARALLEL);
+
+ applyHaarClassifierAnchorParallelDynTemplate(
+ false, //tbInitMaskPositively
+ bTexCacheIImg, //tbCacheTextureIImg
+ bTexCacheCascade, //tbCacheTextureCascade
+ pixParallelStageStops[pixParallelStageStopsIndex] != 0 || pixelStep != 1 || bMaskElements,//tbReadPixelIndexFromVector
+ bDoAtomicCompaction, //tbDoAtomicCompaction
+ grid2,
+ block2,
+ cuStream,
+ d_integralImage.ptr(), d_integralImage.stride(),
+ d_weights.ptr(), d_weights.stride(),
+ d_HaarFeatures.ptr(), d_HaarNodes.ptr(), d_HaarStages.ptr(),
+ d_ptrNowData->ptr(),
+ bDoAtomicCompaction ? d_ptrNowTmp->ptr() : d_ptrNowData->ptr(),
+ numDetections,
+ d_pixelMask.stride(),
+ anchorsRoi,
+ pixParallelStageStops[pixParallelStageStopsIndex],
+ pixParallelStageStops[pixParallelStageStopsIndex+1],
+ scaleAreaPixels);
+ ncvAssertCUDAReturn(cudaGetLastError(), NCV_CUDA_ERROR);
+
+ if (bDoAtomicCompaction)
+ {
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+ ncvAssertCUDAReturn(cudaMemcpyFromSymbolAsync(hp_numDet, d_outMaskPosition, sizeof(Ncv32u),
+ 0, cudaMemcpyDeviceToHost, cuStream), NCV_CUDA_ERROR);
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+ }
+ else
+ {
+ NCVStatus nppSt;
+ nppSt = nppsStCompact_32u(d_ptrNowData->ptr(), numDetections,
+ d_ptrNowTmp->ptr(), hp_numDet, OBJDET_MASK_ELEMENT_INVALID_32U,
+ d_tmpBufCompact.ptr(), szNppCompactTmpBuf, devProp);
+ ncvAssertReturnNcvStat(nppSt);
+ }
+
+ swap(d_ptrNowData, d_ptrNowTmp);
+ numDetections = *hp_numDet;
+ }
+
+ //
+ // 3. Run all left stages in one stage-parallel kernel
+ //
+
+ if (numDetections > 0 && stageMiddleSwitch < stageEndClassifierParallel)
+ {
+ if (bDoAtomicCompaction)
+ {
+ ncvAssertCUDAReturn(cudaMemcpyToSymbolAsync(d_outMaskPosition, hp_zero, sizeof(Ncv32u),
+ 0, cudaMemcpyHostToDevice, cuStream), NCV_CUDA_ERROR);
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+ }
+
+ dim3 grid3(numDetections);
+ if (numDetections > MAX_GRID_DIM)
+ {
+ grid3.x = MAX_GRID_DIM;
+ grid3.y = (numDetections + MAX_GRID_DIM - 1) / MAX_GRID_DIM;
+ }
+ dim3 block3(NUM_THREADS_CLASSIFIERPARALLEL);
+
+ applyHaarClassifierClassifierParallelDynTemplate(
+ bTexCacheIImg, //tbCacheTextureIImg
+ bTexCacheCascade, //tbCacheTextureCascade
+ bDoAtomicCompaction, //tbDoAtomicCompaction
+ grid3,
+ block3,
+ cuStream,
+ d_integralImage.ptr(), d_integralImage.stride(),
+ d_weights.ptr(), d_weights.stride(),
+ d_HaarFeatures.ptr(), d_HaarNodes.ptr(), d_HaarStages.ptr(),
+ d_ptrNowData->ptr(),
+ bDoAtomicCompaction ? d_ptrNowTmp->ptr() : d_ptrNowData->ptr(),
+ numDetections,
+ d_pixelMask.stride(),
+ anchorsRoi,
+ stageMiddleSwitch,
+ stageEndClassifierParallel,
+ scaleAreaPixels);
+ ncvAssertCUDAReturn(cudaGetLastError(), NCV_CUDA_ERROR);
+
+ if (bDoAtomicCompaction)
+ {
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+ ncvAssertCUDAReturn(cudaMemcpyFromSymbolAsync(hp_numDet, d_outMaskPosition, sizeof(Ncv32u),
+ 0, cudaMemcpyDeviceToHost, cuStream), NCV_CUDA_ERROR);
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+ }
+ else
+ {
+ NCVStatus nppSt;
+ nppSt = nppsStCompact_32u(d_ptrNowData->ptr(), numDetections,
+ d_ptrNowTmp->ptr(), hp_numDet, OBJDET_MASK_ELEMENT_INVALID_32U,
+ d_tmpBufCompact.ptr(), szNppCompactTmpBuf, devProp);
+ ncvAssertReturnNcvStat(nppSt);
+ }
+
+ swap(d_ptrNowData, d_ptrNowTmp);
+ numDetections = *hp_numDet;
+ }
+
+ if (d_ptrNowData != &d_vecPixelMask)
+ {
+ d_vecPixelMaskTmp.copySolid(d_vecPixelMask, cuStream);
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+ }
+
+#if defined _SELF_TEST_
+
+ ncvStat = d_pixelMask.copySolid(h_pixelMask_d, 0);
+ ncvAssertReturnNcvStat(ncvStat);
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+
+ if (bDoAtomicCompaction)
+ {
+ std::sort(h_pixelMask_d.ptr, h_pixelMask_d.ptr + numDetections);
+ }
+
+ Ncv32u fpu_oldcw, fpu_cw;
+ _controlfp_s(&fpu_cw, 0, 0);
+ fpu_oldcw = fpu_cw;
+ _controlfp_s(&fpu_cw, _PC_24, _MCW_PC);
+ Ncv32u numDetGold;
+ ncvStat = ncvApplyHaarClassifierCascade_host(h_integralImage, h_weights, h_pixelMask, numDetGold, haar,
+ h_HaarStages, h_HaarNodes, h_HaarFeatures,
+ bMaskElements, anchorsRoi, pixelStep, scaleArea);
+ ncvAssertReturnNcvStat(ncvStat);
+ _controlfp_s(&fpu_cw, fpu_oldcw, _MCW_PC);
+
+ bool bPass = true;
+
+ if (numDetGold != numDetections)
+ {
+ printf("NCVHaarClassifierCascade::applyHaarClassifierCascade numdetections don't match: cpu=%d, gpu=%d\n", numDetGold, numDetections);
+ bPass = false;
+ }
+ else
+ {
+ for (Ncv32u i=0; i<std::max(numDetGold, numDetections) && bPass; i++)
+ {
+ if (h_pixelMask.ptr[i] != h_pixelMask_d.ptr[i])
+ {
+ printf("NCVHaarClassifierCascade::applyHaarClassifierCascade self test failed: i=%d, cpu=%d, gpu=%d\n", i, h_pixelMask.ptr[i], h_pixelMask_d.ptr[i]);
+ bPass = false;
+ }
+ }
+ }
+
+ printf("NCVHaarClassifierCascade::applyHaarClassifierCascade %s\n", bPass?"PASSED":"FAILED");
+#endif
+
+ NCV_SKIP_COND_END
+
+ return NCV_SUCCESS;
+}
+
+
+//==============================================================================
+//
+// HypothesesOperations file
+//
+//==============================================================================
+
+
+const Ncv32u NUM_GROW_THREADS = 128;
+
+
+__device__ __host__ NcvRect32u pixelToRect(Ncv32u pixel, Ncv32u width, Ncv32u height, Ncv32f scale)
+{
+ NcvRect32u res;
+ res.x = (Ncv32u)(scale * (pixel & 0xFFFF));
+ res.y = (Ncv32u)(scale * (pixel >> 16));
+ res.width = (Ncv32u)(scale * width);
+ res.height = (Ncv32u)(scale * height);
+ return res;
+}
+
+
+__global__ void growDetectionsKernel(Ncv32u *pixelMask, Ncv32u numElements,
+ NcvRect32u *hypotheses,
+ Ncv32u rectWidth, Ncv32u rectHeight, Ncv32f curScale)
+{
+ Ncv32u blockId = blockIdx.y * 65535 + blockIdx.x;
+ Ncv32u elemAddr = blockId * NUM_GROW_THREADS + threadIdx.x;
+ if (elemAddr >= numElements)
+ {
+ return;
+ }
+ hypotheses[elemAddr] = pixelToRect(pixelMask[elemAddr], rectWidth, rectHeight, curScale);
+}
+
+
+NCVStatus ncvGrowDetectionsVector_device(NCVVector<Ncv32u> &pixelMask,
+ Ncv32u numPixelMaskDetections,
+ NCVVector<NcvRect32u> &hypotheses,
+ Ncv32u &totalDetections,
+ Ncv32u totalMaxDetections,
+ Ncv32u rectWidth,
+ Ncv32u rectHeight,
+ Ncv32f curScale,
+ cudaStream_t cuStream)
+{
+ ncvAssertReturn(pixelMask.ptr() != NULL && hypotheses.ptr() != NULL, NCV_NULL_PTR);
+ ncvAssertReturn(pixelMask.memType() == hypotheses.memType() &&
+ pixelMask.memType() == NCVMemoryTypeDevice, NCV_MEM_RESIDENCE_ERROR);
+ ncvAssertReturn(rectWidth > 0 && rectHeight > 0 && curScale > 0, NCV_INVALID_ROI);
+ ncvAssertReturn(curScale > 0, NCV_INVALID_SCALE);
+ ncvAssertReturn(totalMaxDetections <= hypotheses.length() &&
+ numPixelMaskDetections <= pixelMask.length() &&
+ totalMaxDetections <= totalMaxDetections, NCV_INCONSISTENT_INPUT);
+
+ NCVStatus ncvStat = NCV_SUCCESS;
+ Ncv32u numDetsToCopy = numPixelMaskDetections;
+
+ if (numDetsToCopy == 0)
+ {
+ return ncvStat;
+ }
+
+ if (totalDetections + numPixelMaskDetections > totalMaxDetections)
+ {
+ ncvStat = NCV_WARNING_HAAR_DETECTIONS_VECTOR_OVERFLOW;
+ numDetsToCopy = totalMaxDetections - totalDetections;
+ }
+
+ dim3 block(NUM_GROW_THREADS);
+ dim3 grid((numDetsToCopy + NUM_GROW_THREADS - 1) / NUM_GROW_THREADS);
+ if (grid.x > 65535)
+ {
+ grid.y = (grid.x + 65534) / 65535;
+ grid.x = 65535;
+ }
+ growDetectionsKernel<<<grid, block, 0, cuStream>>>(pixelMask.ptr(), numDetsToCopy,
+ hypotheses.ptr() + totalDetections,
+ rectWidth, rectHeight, curScale);
+ ncvAssertCUDAReturn(cudaGetLastError(), NCV_CUDA_ERROR);
+
+ totalDetections += numDetsToCopy;
+ return ncvStat;
+}
+
+
+//==============================================================================
+//
+// Pipeline file
+//
+//==============================================================================
+
+
+NCVStatus ncvDetectObjectsMultiScale_device(NCVMatrix<Ncv8u> &d_srcImg,
+ NcvSize32u srcRoi,
+ NCVVector<NcvRect32u> &d_dstRects,
+ Ncv32u &dstNumRects,
+
+ HaarClassifierCascadeDescriptor &haar,
+ NCVVector<HaarStage64> &h_HaarStages,
+ NCVVector<HaarStage64> &d_HaarStages,
+ NCVVector<HaarClassifierNode128> &d_HaarNodes,
+ NCVVector<HaarFeature64> &d_HaarFeatures,
+
+ NcvSize32u minObjSize,
+ Ncv32u minNeighbors, //default 4
+ Ncv32f scaleStep, //default 1.2f
+ Ncv32u pixelStep, //default 1
+ Ncv32u flags, //default NCVPipeObjDet_Default
+
+ INCVMemAllocator &gpuAllocator,
+ INCVMemAllocator &cpuAllocator,
+ cudaDeviceProp &devProp,
+ cudaStream_t cuStream)
+{
+ ncvAssertReturn(d_srcImg.memType() == d_dstRects.memType() &&
+ d_srcImg.memType() == gpuAllocator.memType() &&
+ (d_srcImg.memType() == NCVMemoryTypeDevice ||
+ d_srcImg.memType() == NCVMemoryTypeNone), NCV_MEM_RESIDENCE_ERROR);
+ ncvAssertReturn(d_HaarStages.memType() == d_HaarNodes.memType() &&
+ d_HaarStages.memType() == d_HaarFeatures.memType() &&
+ (d_HaarStages.memType() == NCVMemoryTypeDevice ||
+ d_HaarStages.memType() == NCVMemoryTypeNone), NCV_MEM_RESIDENCE_ERROR);
+ ncvAssertReturn(h_HaarStages.memType() != NCVMemoryTypeDevice, NCV_MEM_RESIDENCE_ERROR);
+ ncvAssertReturn(gpuAllocator.isInitialized() && cpuAllocator.isInitialized(), NCV_ALLOCATOR_NOT_INITIALIZED);
+ ncvAssertReturn((d_srcImg.ptr() != NULL && d_dstRects.ptr() != NULL &&
+ h_HaarStages.ptr() != NULL && d_HaarStages.ptr() != NULL && d_HaarNodes.ptr() != NULL &&
+ d_HaarFeatures.ptr() != NULL) || gpuAllocator.isCounting(), NCV_NULL_PTR);
+ ncvAssertReturn(srcRoi.width > 0 && srcRoi.height > 0 &&
+ d_srcImg.width() >= srcRoi.width && d_srcImg.height() >= srcRoi.height &&
+ srcRoi.width >= minObjSize.width && srcRoi.height >= minObjSize.height &&
+ d_dstRects.length() >= 1, NCV_DIMENSIONS_INVALID);
+ ncvAssertReturn(scaleStep > 1.0f, NCV_INVALID_SCALE);
+ ncvAssertReturn(d_HaarStages.length() >= haar.NumStages &&
+ d_HaarNodes.length() >= haar.NumClassifierTotalNodes &&
+ d_HaarFeatures.length() >= haar.NumFeatures &&
+ d_HaarStages.length() == h_HaarStages.length() &&
+ haar.NumClassifierRootNodes <= haar.NumClassifierTotalNodes, NCV_DIMENSIONS_INVALID);
+ ncvAssertReturn(haar.bNeedsTiltedII == false, NCV_NOIMPL_HAAR_TILTED_FEATURES);
+ ncvAssertReturn(pixelStep == 1 || pixelStep == 2, NCV_HAAR_INVALID_PIXEL_STEP);
+
+ //TODO: set NPP active stream to cuStream
+
+ NCVStatus ncvStat;
+ NCV_SET_SKIP_COND(gpuAllocator.isCounting());
+
+ Ncv32u integralWidth = d_srcImg.width() + 1;
+ Ncv32u integralHeight = d_srcImg.height() + 1;
+
+ NCVMatrixAlloc<Ncv32u> d_integralImage(gpuAllocator, integralWidth, integralHeight);
+ ncvAssertReturn(d_integralImage.isMemAllocated(), NCV_ALLOCATOR_BAD_ALLOC);
+ NCVMatrixAlloc<Ncv64u> d_sqIntegralImage(gpuAllocator, integralWidth, integralHeight);
+ ncvAssertReturn(d_sqIntegralImage.isMemAllocated(), NCV_ALLOCATOR_BAD_ALLOC);
+
+ NCVMatrixAlloc<Ncv32f> d_rectStdDev(gpuAllocator, d_srcImg.width(), d_srcImg.height());
+ ncvAssertReturn(d_rectStdDev.isMemAllocated(), NCV_ALLOCATOR_BAD_ALLOC);
+ NCVMatrixAlloc<Ncv32u> d_pixelMask(gpuAllocator, d_srcImg.width(), d_srcImg.height());
+ ncvAssertReturn(d_pixelMask.isMemAllocated(), NCV_ALLOCATOR_BAD_ALLOC);
+
+ NCVMatrixAlloc<Ncv32u> d_scaledIntegralImage(gpuAllocator, integralWidth, integralHeight);
+ ncvAssertReturn(d_scaledIntegralImage.isMemAllocated(), NCV_ALLOCATOR_BAD_ALLOC);
+ NCVMatrixAlloc<Ncv64u> d_scaledSqIntegralImage(gpuAllocator, integralWidth, integralHeight);
+ ncvAssertReturn(d_scaledSqIntegralImage.isMemAllocated(), NCV_ALLOCATOR_BAD_ALLOC);
+
+ NCVVectorAlloc<NcvRect32u> d_hypothesesIntermediate(gpuAllocator, d_srcImg.width() * d_srcImg.height());
+ ncvAssertReturn(d_hypothesesIntermediate.isMemAllocated(), NCV_ALLOCATOR_BAD_ALLOC);
+ NCVVectorAlloc<NcvRect32u> h_hypothesesIntermediate(cpuAllocator, d_srcImg.width() * d_srcImg.height());
+ ncvAssertReturn(h_hypothesesIntermediate.isMemAllocated(), NCV_ALLOCATOR_BAD_ALLOC);
+
+ NCVStatus nppStat;
+ Ncv32u szTmpBufIntegral, szTmpBufSqIntegral;
+ nppStat = nppiStIntegralGetSize_8u32u(NcvSize32u(d_srcImg.width(), d_srcImg.height()), &szTmpBufIntegral, devProp);
+ ncvAssertReturnNcvStat(nppStat);
+ nppStat = nppiStSqrIntegralGetSize_8u64u(NcvSize32u(d_srcImg.width(), d_srcImg.height()), &szTmpBufSqIntegral, devProp);
+ ncvAssertReturnNcvStat(nppStat);
+ NCVVectorAlloc<Ncv8u> d_tmpIIbuf(gpuAllocator, std::max(szTmpBufIntegral, szTmpBufSqIntegral));
+ ncvAssertReturn(d_tmpIIbuf.isMemAllocated(), NCV_ALLOCATOR_BAD_ALLOC);
+
+ NCV_SKIP_COND_BEGIN
+
+ nppStat = nppiStIntegral_8u32u_C1R(d_srcImg.ptr(), d_srcImg.pitch(),
+ d_integralImage.ptr(), d_integralImage.pitch(),
+ NcvSize32u(d_srcImg.width(), d_srcImg.height()),
+ d_tmpIIbuf.ptr(), szTmpBufIntegral, devProp);
+ ncvAssertReturnNcvStat(nppStat);
+
+ nppStat = nppiStSqrIntegral_8u64u_C1R(d_srcImg.ptr(), d_srcImg.pitch(),
+ d_sqIntegralImage.ptr(), d_sqIntegralImage.pitch(),
+ NcvSize32u(d_srcImg.width(), d_srcImg.height()),
+ d_tmpIIbuf.ptr(), szTmpBufSqIntegral, devProp);
+ ncvAssertReturnNcvStat(nppStat);
+
+ NCV_SKIP_COND_END
+
+ dstNumRects = 0;
+
+ Ncv32u lastCheckedScale = 0;
+ NcvBool bReverseTraverseScale = ((flags & NCVPipeObjDet_FindLargestObject) != 0);
+ std::vector<Ncv32u> scalesVector;
+
+ NcvBool bFoundLargestFace = false;
+
+ for (Ncv32f scaleIter = 1.0f; ; scaleIter *= scaleStep)
+ {
+ Ncv32u scale = (Ncv32u)scaleIter;
+ if (lastCheckedScale == scale)
+ {
+ continue;
+ }
+ lastCheckedScale = scale;
+
+ if (haar.ClassifierSize.width * (Ncv32s)scale < minObjSize.width ||
+ haar.ClassifierSize.height * (Ncv32s)scale < minObjSize.height)
+ {
+ continue;
+ }
+
+ NcvSize32s srcRoi, srcIIRoi, scaledIIRoi, searchRoi;
+
+ srcRoi.width = d_srcImg.width();
+ srcRoi.height = d_srcImg.height();
+
+ srcIIRoi.width = srcRoi.width + 1;
+ srcIIRoi.height = srcRoi.height + 1;
+
+ scaledIIRoi.width = srcIIRoi.width / scale;
+ scaledIIRoi.height = srcIIRoi.height / scale;
+
+ searchRoi.width = scaledIIRoi.width - haar.ClassifierSize.width;
+ searchRoi.height = scaledIIRoi.height - haar.ClassifierSize.height;
+
+ if (searchRoi.width <= 0 || searchRoi.height <= 0)
+ {
+ break;
+ }
+
+ scalesVector.push_back(scale);
+
+ if (gpuAllocator.isCounting())
+ {
+ break;
+ }
+ }
+
+ if (bReverseTraverseScale)
+ {
+ std::reverse(scalesVector.begin(), scalesVector.end());
+ }
+
+ //TODO: handle _fair_scale_ flag
+ for (Ncv32u i=0; i<scalesVector.size(); i++)
+ {
+ Ncv32u scale = scalesVector[i];
+
+ NcvSize32u srcRoi, scaledIIRoi, searchRoi;
+ NcvSize32u srcIIRoi;
+ srcRoi.width = d_srcImg.width();
+ srcRoi.height = d_srcImg.height();
+ srcIIRoi.width = srcRoi.width + 1;
+ srcIIRoi.height = srcRoi.height + 1;
+ scaledIIRoi.width = srcIIRoi.width / scale;
+ scaledIIRoi.height = srcIIRoi.height / scale;
+ searchRoi.width = scaledIIRoi.width - haar.ClassifierSize.width;
+ searchRoi.height = scaledIIRoi.height - haar.ClassifierSize.height;
+
+ NCV_SKIP_COND_BEGIN
+
+ nppStat = nppiStDecimate_32u_C1R(
+ d_integralImage.ptr(), d_integralImage.pitch(),
+ d_scaledIntegralImage.ptr(), d_scaledIntegralImage.pitch(),
+ srcIIRoi, scale, true);
+ ncvAssertReturnNcvStat(nppStat);
+
+ nppStat = nppiStDecimate_64u_C1R(
+ d_sqIntegralImage.ptr(), d_sqIntegralImage.pitch(),
+ d_scaledSqIntegralImage.ptr(), d_scaledSqIntegralImage.pitch(),
+ srcIIRoi, scale, true);
+ ncvAssertReturnNcvStat(nppStat);
+
+ const NcvRect32u rect(
+ HAAR_STDDEV_BORDER,
+ HAAR_STDDEV_BORDER,
+ haar.ClassifierSize.width - 2*HAAR_STDDEV_BORDER,
+ haar.ClassifierSize.height - 2*HAAR_STDDEV_BORDER);
+ nppStat = nppiStRectStdDev_32f_C1R(
+ d_scaledIntegralImage.ptr(), d_scaledIntegralImage.pitch(),
+ d_scaledSqIntegralImage.ptr(), d_scaledSqIntegralImage.pitch(),
+ d_rectStdDev.ptr(), d_rectStdDev.pitch(),
+ NcvSize32u(searchRoi.width, searchRoi.height), rect,
+ (Ncv32f)scale*scale, true);
+ ncvAssertReturnNcvStat(nppStat);
+
+ NCV_SKIP_COND_END
+
+ Ncv32u detectionsOnThisScale;
+ ncvStat = ncvApplyHaarClassifierCascade_device(
+ d_scaledIntegralImage, d_rectStdDev, d_pixelMask,
+ detectionsOnThisScale,
+ haar, h_HaarStages, d_HaarStages, d_HaarNodes, d_HaarFeatures, false,
+ searchRoi, pixelStep, (Ncv32f)scale*scale,
+ gpuAllocator, cpuAllocator, devProp, cuStream);
+ ncvAssertReturnNcvStat(nppStat);
+
+ NCV_SKIP_COND_BEGIN
+
+ NCVVectorReuse<Ncv32u> d_vecPixelMask(d_pixelMask.getSegment());
+ ncvStat = ncvGrowDetectionsVector_device(
+ d_vecPixelMask,
+ detectionsOnThisScale,
+ d_hypothesesIntermediate,
+ dstNumRects,
+ static_cast<Ncv32u>(d_hypothesesIntermediate.length()),
+ haar.ClassifierSize.width,
+ haar.ClassifierSize.height,
+ (Ncv32f)scale,
+ cuStream);
+ ncvAssertReturn(ncvStat == NCV_SUCCESS, ncvStat);
+
+ if (flags & NCVPipeObjDet_FindLargestObject)
+ {
+ if (dstNumRects == 0)
+ {
+ continue;
+ }
+
+ if (dstNumRects != 0)
+ {
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+ ncvStat = d_hypothesesIntermediate.copySolid(h_hypothesesIntermediate, cuStream,
+ dstNumRects * sizeof(NcvRect32u));
+ ncvAssertReturnNcvStat(ncvStat);
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+ }
+
+ Ncv32u numStrongHypothesesNow = dstNumRects;
+ // TODO Fix this to be back operational
+ /*
+ ncvStat = ncvGroupRectangles_host(
+ h_hypothesesIntermediate,
+ numStrongHypothesesNow,
+ minNeighbors,
+ RECT_SIMILARITY_PROPORTION,
+ NULL);
+ ncvAssertReturnNcvStat(ncvStat);
+ */
+ if (numStrongHypothesesNow > 0)
+ {
+ NcvRect32u maxRect = h_hypothesesIntermediate.ptr()[0];
+ for (Ncv32u j=1; j<numStrongHypothesesNow; j++)
+ {
+ if (maxRect.width < h_hypothesesIntermediate.ptr()[j].width)
+ {
+ maxRect = h_hypothesesIntermediate.ptr()[j];
+ }
+ }
+
+ h_hypothesesIntermediate.ptr()[0] = maxRect;
+ dstNumRects = 1;
+
+ ncvStat = h_hypothesesIntermediate.copySolid(d_dstRects, cuStream, sizeof(NcvRect32u));
+ ncvAssertReturnNcvStat(ncvStat);
+
+ bFoundLargestFace = true;
+
+ break;
+ }
+ }
+
+ NCV_SKIP_COND_END
+
+ if (gpuAllocator.isCounting())
+ {
+ break;
+ }
+ }
+
+ NCVStatus ncvRetCode = NCV_SUCCESS;
+
+ NCV_SKIP_COND_BEGIN
+
+ if (flags & NCVPipeObjDet_FindLargestObject)
+ {
+ if (!bFoundLargestFace)
+ {
+ dstNumRects = 0;
+ }
+ }
+ else
+ {
+ //TODO: move hypotheses filtration to GPU pipeline (the only CPU-resident element of the pipeline left)
+ if (dstNumRects != 0)
+ {
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+ ncvStat = d_hypothesesIntermediate.copySolid(h_hypothesesIntermediate, cuStream,
+ dstNumRects * sizeof(NcvRect32u));
+ ncvAssertReturnNcvStat(ncvStat);
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+ }
+ // Todo fix this to be back operational
+ /*
+ ncvStat = ncvGroupRectangles_host(
+ h_hypothesesIntermediate,
+ dstNumRects,
+ minNeighbors,
+ RECT_SIMILARITY_PROPORTION,
+ NULL);
+ ncvAssertReturnNcvStat(ncvStat);
+ */
+ if (dstNumRects > d_dstRects.length())
+ {
+ ncvRetCode = NCV_WARNING_HAAR_DETECTIONS_VECTOR_OVERFLOW;
+ dstNumRects = static_cast<Ncv32u>(d_dstRects.length());
+ }
+
+ if (dstNumRects != 0)
+ {
+ ncvStat = h_hypothesesIntermediate.copySolid(d_dstRects, cuStream,
+ dstNumRects * sizeof(NcvRect32u));
+ ncvAssertReturnNcvStat(ncvStat);
+ }
+ }
+
+ if (flags & NCVPipeObjDet_VisualizeInPlace)
+ {
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+ ncvDrawRects_8u_device(d_srcImg.ptr(), d_srcImg.stride(),
+ d_srcImg.width(), d_srcImg.height(),
+ d_dstRects.ptr(), dstNumRects, 255, cuStream);
+ }
+
+ NCV_SKIP_COND_END
+
+ return ncvRetCode;
+}
+
+
+//==============================================================================
+//
+// Purely Host code: classifier IO, mock-ups
+//
+//==============================================================================
+
+
+#ifdef _SELF_TEST_
+#include <float.h>
+#endif
+
+#define NVBIN_HAAR_SIZERESERVED 16
+#define NVBIN_HAAR_VERSION 0x1
+
+NCVStatus ncvApplyHaarClassifierCascade_host(NCVMatrix<Ncv32u> &h_integralImage,
+ NCVMatrix<Ncv32f> &h_weights,
+ NCVMatrixAlloc<Ncv32u> &h_pixelMask,
+ Ncv32u &numDetections,
+ HaarClassifierCascadeDescriptor &haar,
+ NCVVector<HaarStage64> &h_HaarStages,
+ NCVVector<HaarClassifierNode128> &h_HaarNodes,
+ NCVVector<HaarFeature64> &h_HaarFeatures,
+ NcvBool bMaskElements,
+ NcvSize32u anchorsRoi,
+ Ncv32u pixelStep,
+ Ncv32f scaleArea)
+{
+ ncvAssertReturn(h_integralImage.memType() == h_weights.memType() &&
+ h_integralImage.memType() == h_pixelMask.memType() &&
+ (h_integralImage.memType() == NCVMemoryTypeHostPageable ||
+ h_integralImage.memType() == NCVMemoryTypeHostPinned), NCV_MEM_RESIDENCE_ERROR);
+ ncvAssertReturn(h_HaarStages.memType() == h_HaarNodes.memType() &&
+ h_HaarStages.memType() == h_HaarFeatures.memType() &&
+ (h_HaarStages.memType() == NCVMemoryTypeHostPageable ||
+ h_HaarStages.memType() == NCVMemoryTypeHostPinned), NCV_MEM_RESIDENCE_ERROR);
+ ncvAssertReturn(h_integralImage.ptr() != NULL && h_weights.ptr() != NULL && h_pixelMask.ptr() != NULL &&
+ h_HaarStages.ptr() != NULL && h_HaarNodes.ptr() != NULL && h_HaarFeatures.ptr() != NULL, NCV_NULL_PTR);
+ ncvAssertReturn(anchorsRoi.width > 0 && anchorsRoi.height > 0 &&
+ h_pixelMask.width() >= anchorsRoi.width && h_pixelMask.height() >= anchorsRoi.height &&
+ h_weights.width() >= anchorsRoi.width && h_weights.height() >= anchorsRoi.height &&
+ h_integralImage.width() >= anchorsRoi.width + haar.ClassifierSize.width &&
+ h_integralImage.height() >= anchorsRoi.height + haar.ClassifierSize.height, NCV_DIMENSIONS_INVALID);
+ ncvAssertReturn(scaleArea > 0, NCV_INVALID_SCALE);
+ ncvAssertReturn(h_HaarStages.length() >= haar.NumStages &&
+ h_HaarNodes.length() >= haar.NumClassifierTotalNodes &&
+ h_HaarFeatures.length() >= haar.NumFeatures &&
+ h_HaarStages.length() == h_HaarStages.length() &&
+ haar.NumClassifierRootNodes <= haar.NumClassifierTotalNodes, NCV_DIMENSIONS_INVALID);
+ ncvAssertReturn(haar.bNeedsTiltedII == false, NCV_NOIMPL_HAAR_TILTED_FEATURES);
+ ncvAssertReturn(pixelStep == 1 || pixelStep == 2, NCV_HAAR_INVALID_PIXEL_STEP);
+
+ Ncv32f scaleAreaPixels = scaleArea * ((haar.ClassifierSize.width - 2*HAAR_STDDEV_BORDER) *
+ (haar.ClassifierSize.height - 2*HAAR_STDDEV_BORDER));
+
+ for (Ncv32u i=0; i<anchorsRoi.height; i++)
+ {
+ for (Ncv32u j=0; j<h_pixelMask.stride(); j++)
+ {
+ if (i % pixelStep != 0 || j % pixelStep != 0 || j >= anchorsRoi.width)
+ {
+ h_pixelMask.ptr()[i * h_pixelMask.stride() + j] = OBJDET_MASK_ELEMENT_INVALID_32U;
+ }
+ else
+ {
+ for (Ncv32u iStage = 0; iStage < haar.NumStages; iStage++)
+ {
+ Ncv32f curStageSum = 0.0f;
+ Ncv32u numRootNodesInStage = h_HaarStages.ptr()[iStage].getNumClassifierRootNodes();
+ Ncv32u curRootNodeOffset = h_HaarStages.ptr()[iStage].getStartClassifierRootNodeOffset();
+
+ if (iStage == 0)
+ {
+ if (bMaskElements && h_pixelMask.ptr()[i * h_pixelMask.stride() + j] == OBJDET_MASK_ELEMENT_INVALID_32U)
+ {
+ break;
+ }
+ else
+ {
+ h_pixelMask.ptr()[i * h_pixelMask.stride() + j] = ((i << 16) | j);
+ }
+ }
+ else if (h_pixelMask.ptr()[i * h_pixelMask.stride() + j] == OBJDET_MASK_ELEMENT_INVALID_32U)
+ {
+ break;
+ }
+
+ while (numRootNodesInStage--)
+ {
+ NcvBool bMoreNodesToTraverse = true;
+ Ncv32u curNodeOffset = curRootNodeOffset;
+
+ while (bMoreNodesToTraverse)
+ {
+ HaarClassifierNode128 curNode = h_HaarNodes.ptr()[curNodeOffset];
+ HaarFeatureDescriptor32 curFeatDesc = curNode.getFeatureDesc();
+ Ncv32u curNodeFeaturesNum = curFeatDesc.getNumFeatures();
+ Ncv32u curNodeFeaturesOffs = curFeatDesc.getFeaturesOffset();
+
+ Ncv32f curNodeVal = 0.f;
+ for (Ncv32u iRect=0; iRect<curNodeFeaturesNum; iRect++)
+ {
+ HaarFeature64 feature = h_HaarFeatures.ptr()[curNodeFeaturesOffs + iRect];
+ Ncv32u rectX, rectY, rectWidth, rectHeight;
+ feature.getRect(&rectX, &rectY, &rectWidth, &rectHeight);
+ Ncv32f rectWeight = feature.getWeight();
+ Ncv32u iioffsTL = (i + rectY) * h_integralImage.stride() + (j + rectX);
+ Ncv32u iioffsTR = iioffsTL + rectWidth;
+ Ncv32u iioffsBL = iioffsTL + rectHeight * h_integralImage.stride();
+ Ncv32u iioffsBR = iioffsBL + rectWidth;
+
+ Ncv32u iivalTL = h_integralImage.ptr()[iioffsTL];
+ Ncv32u iivalTR = h_integralImage.ptr()[iioffsTR];
+ Ncv32u iivalBL = h_integralImage.ptr()[iioffsBL];
+ Ncv32u iivalBR = h_integralImage.ptr()[iioffsBR];
+ Ncv32u rectSum = iivalBR - iivalBL + iivalTL - iivalTR;
+ curNodeVal += (Ncv32f)rectSum * rectWeight;
+ }
+
+ HaarClassifierNodeDescriptor32 nodeLeft = curNode.getLeftNodeDesc();
+ HaarClassifierNodeDescriptor32 nodeRight = curNode.getRightNodeDesc();
+ Ncv32f nodeThreshold = curNode.getThreshold();
+
+ HaarClassifierNodeDescriptor32 nextNodeDescriptor;
+ NcvBool nextNodeIsLeaf;
+
+ if (curNodeVal < scaleAreaPixels * h_weights.ptr()[i * h_weights.stride() + j] * nodeThreshold)
+ {
+ nextNodeDescriptor = nodeLeft;
+ nextNodeIsLeaf = curFeatDesc.isLeftNodeLeaf();
+ }
+ else
+ {
+ nextNodeDescriptor = nodeRight;
+ nextNodeIsLeaf = curFeatDesc.isRightNodeLeaf();
+ }
+
+ if (nextNodeIsLeaf)
+ {
+ Ncv32f tmpLeafValue = nextNodeDescriptor.getLeafValueHost();
+ curStageSum += tmpLeafValue;
+ bMoreNodesToTraverse = false;
+ }
+ else
+ {
+ curNodeOffset = nextNodeDescriptor.getNextNodeOffset();
+ }
+ }
+
+ curRootNodeOffset++;
+ }
+
+ Ncv32f tmpStageThreshold = h_HaarStages.ptr()[iStage].getStageThreshold();
+ if (curStageSum < tmpStageThreshold)
+ {
+ //drop
+ h_pixelMask.ptr()[i * h_pixelMask.stride() + j] = OBJDET_MASK_ELEMENT_INVALID_32U;
+ break;
+ }
+ }
+ }
+ }
+ }
+
+ std::sort(h_pixelMask.ptr(), h_pixelMask.ptr() + anchorsRoi.height * h_pixelMask.stride());
+ Ncv32u i = 0;
+ for (; i<anchorsRoi.height * h_pixelMask.stride(); i++)
+ {
+ if (h_pixelMask.ptr()[i] == OBJDET_MASK_ELEMENT_INVALID_32U)
+ {
+ break;
+ }
+ }
+ numDetections = i;
+
+ return NCV_SUCCESS;
+}
+
+NCVStatus ncvGrowDetectionsVector_host(NCVVector<Ncv32u> &pixelMask,
+ Ncv32u numPixelMaskDetections,
+ NCVVector<NcvRect32u> &hypotheses,
+ Ncv32u &totalDetections,
+ Ncv32u totalMaxDetections,
+ Ncv32u rectWidth,
+ Ncv32u rectHeight,
+ Ncv32f curScale)
+{
+ ncvAssertReturn(pixelMask.ptr() != NULL && hypotheses.ptr() != NULL, NCV_NULL_PTR);
+ ncvAssertReturn(pixelMask.memType() == hypotheses.memType() &&
+ pixelMask.memType() != NCVMemoryTypeDevice, NCV_MEM_RESIDENCE_ERROR);
+ ncvAssertReturn(rectWidth > 0 && rectHeight > 0 && curScale > 0, NCV_INVALID_ROI);
+ ncvAssertReturn(curScale > 0, NCV_INVALID_SCALE);
+ ncvAssertReturn(totalMaxDetections <= hypotheses.length() &&
+ numPixelMaskDetections <= pixelMask.length() &&
+ totalMaxDetections <= totalMaxDetections, NCV_INCONSISTENT_INPUT);
+
+ NCVStatus ncvStat = NCV_SUCCESS;
+ Ncv32u numDetsToCopy = numPixelMaskDetections;
+
+ if (numDetsToCopy == 0)
+ {
+ return ncvStat;
+ }
+
+ if (totalDetections + numPixelMaskDetections > totalMaxDetections)
+ {
+ ncvStat = NCV_WARNING_HAAR_DETECTIONS_VECTOR_OVERFLOW;
+ numDetsToCopy = totalMaxDetections - totalDetections;
+ }
+
+ for (Ncv32u i=0; i<numDetsToCopy; i++)
+ {
+ hypotheses.ptr()[totalDetections + i] = pixelToRect(pixelMask.ptr()[i], rectWidth, rectHeight, curScale);
+ }
+
+ totalDetections += numDetsToCopy;
+ return ncvStat;
+}
+
+NCVStatus ncvHaarStoreNVBIN_host(const std::string &filename,
+ HaarClassifierCascadeDescriptor haar,
+ NCVVector<HaarStage64> &h_HaarStages,
+ NCVVector<HaarClassifierNode128> &h_HaarNodes,
+ NCVVector<HaarFeature64> &h_HaarFeatures)
+{
+ ncvAssertReturn(h_HaarStages.length() >= haar.NumStages, NCV_INCONSISTENT_INPUT);
+ ncvAssertReturn(h_HaarNodes.length() >= haar.NumClassifierTotalNodes, NCV_INCONSISTENT_INPUT);
+ ncvAssertReturn(h_HaarFeatures.length() >= haar.NumFeatures, NCV_INCONSISTENT_INPUT);
+ ncvAssertReturn(h_HaarStages.memType() == NCVMemoryTypeHostPinned &&
+ h_HaarNodes.memType() == NCVMemoryTypeHostPinned &&
+ h_HaarFeatures.memType() == NCVMemoryTypeHostPinned, NCV_MEM_RESIDENCE_ERROR);
+
+ Ncv32u szStages = haar.NumStages * sizeof(HaarStage64);
+ Ncv32u szClassifiers = haar.NumClassifierTotalNodes * sizeof(HaarClassifierNode128);
+ Ncv32u szFeatures = haar.NumFeatures * sizeof(HaarFeature64);
+
+ Ncv32u dataOffset = 0;
+ std::vector<unsigned char> fdata;
+ fdata.resize(szStages+szClassifiers+szFeatures+1024, 0);
+
+ //header
+ *(Ncv32u *)(&fdata[0]+dataOffset) = NVBIN_HAAR_VERSION;
+
+ //data
+ dataOffset = NVBIN_HAAR_SIZERESERVED;
+ *(Ncv32u *)(&fdata[0]+dataOffset) = haar.NumStages;
+ dataOffset += sizeof(Ncv32u);
+ *(Ncv32u *)(&fdata[0]+dataOffset) = haar.NumClassifierRootNodes;
+ dataOffset += sizeof(Ncv32u);
+ *(Ncv32u *)(&fdata[0]+dataOffset) = haar.NumClassifierTotalNodes;
+ dataOffset += sizeof(Ncv32u);
+ *(Ncv32u *)(&fdata[0]+dataOffset) = haar.NumFeatures;
+ dataOffset += sizeof(Ncv32u);
+ *(NcvSize32u *)(&fdata[0]+dataOffset) = haar.ClassifierSize;
+ dataOffset += sizeof(NcvSize32u);
+ *(NcvBool *)(&fdata[0]+dataOffset) = haar.bNeedsTiltedII;
+ dataOffset += sizeof(NcvBool);
+ *(NcvBool *)(&fdata[0]+dataOffset) = haar.bHasStumpsOnly;
+ dataOffset += sizeof(NcvBool);
+
+ memcpy(&fdata[0]+dataOffset, h_HaarStages.ptr(), szStages);
+ dataOffset += szStages;
+ memcpy(&fdata[0]+dataOffset, h_HaarNodes.ptr(), szClassifiers);
+ dataOffset += szClassifiers;
+ memcpy(&fdata[0]+dataOffset, h_HaarFeatures.ptr(), szFeatures);
+ dataOffset += szFeatures;
+ Ncv32u fsize = dataOffset;
+
+ //TODO: CRC32 here
+
+ //update header
+ dataOffset = sizeof(Ncv32u);
+ *(Ncv32u *)(&fdata[0]+dataOffset) = fsize;
+
+ FILE *fp = fopen(filename.c_str(), "wb");
+ ncvAssertReturn(fp != NULL, NCV_FILE_ERROR);
+ fwrite(&fdata[0], fsize, 1, fp);
+ fclose(fp);
+ return NCV_SUCCESS;
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (C) 2009-2010, NVIDIA Corporation, all rights reserved.
+ * Third party copyrights are property of their respective owners.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * Ported to PCL by Koen Buys : Attention Work in progress!
+ */
+
+////////////////////////////////////////////////////////////////////////////////
+//
+// NVIDIA CUDA implementation of Viola-Jones Object Detection Framework
+//
+// The algorithm and code are explained in the upcoming GPU Computing Gems
+// chapter in detail:
+//
+// Anton Obukhov, "Haar Classifiers for Object Detection with CUDA"
+// PDF URL placeholder
+// email: aobukhov@nvidia.com, devsupport@nvidia.com
+//
+// Credits for help with the code to:
+// Alexey Mendelenko, Cyril Crassin, and Mikhail Smirnov.
+//
+////////////////////////////////////////////////////////////////////////////////
+
+#ifndef PCL_GPU_PEOPLE_NCVHAAROBJECTDETECTION_HPP_
+#define PCL_GPU_PEOPLE_NCVHAAROBJECTDETECTION_HPP_
+
+#include <string>
+#include "NCV.hpp"
+
+//==============================================================================
+//
+// Guaranteed size cross-platform classifier structures
+//
+//==============================================================================
+
+struct HaarFeature64
+{
+ uint2 _ui2;
+
+#define HaarFeature64_CreateCheck_MaxRectField 0xFF
+
+ __host__ NCVStatus setRect(Ncv32u rectX, Ncv32u rectY, Ncv32u rectWidth, Ncv32u rectHeight, Ncv32u /*clsWidth*/, Ncv32u /*clsHeight*/)
+ {
+ ncvAssertReturn(rectWidth <= HaarFeature64_CreateCheck_MaxRectField && rectHeight <= HaarFeature64_CreateCheck_MaxRectField, NCV_HAAR_TOO_LARGE_FEATURES);
+ ((NcvRect8u*)&(this->_ui2.x))->x = (Ncv8u)rectX;
+ ((NcvRect8u*)&(this->_ui2.x))->y = (Ncv8u)rectY;
+ ((NcvRect8u*)&(this->_ui2.x))->width = (Ncv8u)rectWidth;
+ ((NcvRect8u*)&(this->_ui2.x))->height = (Ncv8u)rectHeight;
+ return NCV_SUCCESS;
+ }
+
+ __host__ NCVStatus setWeight(Ncv32f weight)
+ {
+ ((Ncv32f*)&(this->_ui2.y))[0] = weight;
+ return NCV_SUCCESS;
+ }
+
+ __device__ __host__ void getRect(Ncv32u *rectX, Ncv32u *rectY, Ncv32u *rectWidth, Ncv32u *rectHeight)
+ {
+ NcvRect8u tmpRect = *(NcvRect8u*)(&this->_ui2.x);
+ *rectX = tmpRect.x;
+ *rectY = tmpRect.y;
+ *rectWidth = tmpRect.width;
+ *rectHeight = tmpRect.height;
+ }
+
+ __device__ __host__ Ncv32f getWeight(void)
+ {
+ return *(Ncv32f*)(&this->_ui2.y);
+ }
+};
+
+struct HaarFeatureDescriptor32
+{
+ private:
+
+#define HaarFeatureDescriptor32_Interpret_MaskFlagTilted 0x80000000
+#define HaarFeatureDescriptor32_Interpret_MaskFlagLeftNodeLeaf 0x40000000
+#define HaarFeatureDescriptor32_Interpret_MaskFlagRightNodeLeaf 0x20000000
+#define HaarFeatureDescriptor32_CreateCheck_MaxNumFeatures 0x1F
+#define HaarFeatureDescriptor32_NumFeatures_Shift 24
+#define HaarFeatureDescriptor32_CreateCheck_MaxFeatureOffset 0x00FFFFFF
+
+ Ncv32u desc;
+
+ public:
+
+ __host__ NCVStatus create(NcvBool bTilted, NcvBool bLeftLeaf, NcvBool bRightLeaf,
+ Ncv32u numFeatures, Ncv32u offsetFeatures)
+ {
+ if (numFeatures > HaarFeatureDescriptor32_CreateCheck_MaxNumFeatures)
+ {
+ return NCV_HAAR_TOO_MANY_FEATURES_IN_CLASSIFIER;
+ }
+ if (offsetFeatures > HaarFeatureDescriptor32_CreateCheck_MaxFeatureOffset)
+ {
+ return NCV_HAAR_TOO_MANY_FEATURES_IN_CASCADE;
+ }
+ this->desc = 0;
+ this->desc |= (bTilted ? HaarFeatureDescriptor32_Interpret_MaskFlagTilted : 0);
+ this->desc |= (bLeftLeaf ? HaarFeatureDescriptor32_Interpret_MaskFlagLeftNodeLeaf : 0);
+ this->desc |= (bRightLeaf ? HaarFeatureDescriptor32_Interpret_MaskFlagRightNodeLeaf : 0);
+ this->desc |= (numFeatures << HaarFeatureDescriptor32_NumFeatures_Shift);
+ this->desc |= offsetFeatures;
+ return NCV_SUCCESS;
+ }
+
+ __device__ __host__ NcvBool isTilted(void)
+ {
+ return (this->desc & HaarFeatureDescriptor32_Interpret_MaskFlagTilted) != 0;
+ }
+
+ __device__ __host__ NcvBool isLeftNodeLeaf(void)
+ {
+ return (this->desc & HaarFeatureDescriptor32_Interpret_MaskFlagLeftNodeLeaf) != 0;
+ }
+
+ __device__ __host__ NcvBool isRightNodeLeaf(void)
+ {
+ return (this->desc & HaarFeatureDescriptor32_Interpret_MaskFlagRightNodeLeaf) != 0;
+ }
+
+ __device__ __host__ Ncv32u getNumFeatures(void)
+ {
+ return (this->desc >> HaarFeatureDescriptor32_NumFeatures_Shift) & HaarFeatureDescriptor32_CreateCheck_MaxNumFeatures;
+ }
+
+ __device__ __host__ Ncv32u getFeaturesOffset(void)
+ {
+ return this->desc & HaarFeatureDescriptor32_CreateCheck_MaxFeatureOffset;
+ }
+};
+
+struct HaarClassifierNodeDescriptor32
+{
+ uint1 _ui1;
+
+ __host__ NCVStatus create(Ncv32f leafValue)
+ {
+ *(Ncv32f *)&this->_ui1 = leafValue;
+ return (NCV_SUCCESS);
+ }
+
+ __host__ NCVStatus create(Ncv32u offsetHaarClassifierNode)
+ {
+ this->_ui1.x = offsetHaarClassifierNode;
+ return (NCV_SUCCESS);
+ }
+
+ __host__ Ncv32f getLeafValueHost(void)
+ {
+ return (*(Ncv32f *)&this->_ui1.x);
+ }
+
+ __host__ bool isLeaf() // TODO: check this hack don't know if is correct
+ {
+ if( _ui1.x == 0)
+ return (false);
+ else
+ return (true);
+ }
+
+#ifdef __CUDACC__
+ __device__ Ncv32f getLeafValue(void)
+ {
+ return (__int_as_float(this->_ui1.x));
+ }
+#endif
+
+ __device__ __host__ Ncv32u getNextNodeOffset(void)
+ {
+ return (this->_ui1.x);
+ }
+};
+
+struct HaarClassifierNode128
+{
+ uint4 _ui4;
+
+ __host__ NCVStatus setFeatureDesc(HaarFeatureDescriptor32 f)
+ {
+ this->_ui4.x = *(Ncv32u *)&f;
+ return NCV_SUCCESS;
+ }
+
+ __host__ NCVStatus setThreshold(Ncv32f t)
+ {
+ this->_ui4.y = *(Ncv32u *)&t;
+ return NCV_SUCCESS;
+ }
+
+ __host__ NCVStatus setLeftNodeDesc(HaarClassifierNodeDescriptor32 nl)
+ {
+ this->_ui4.z = *(Ncv32u *)&nl;
+ return NCV_SUCCESS;
+ }
+
+ __host__ NCVStatus setRightNodeDesc(HaarClassifierNodeDescriptor32 nr)
+ {
+ this->_ui4.w = *(Ncv32u *)&nr;
+ return NCV_SUCCESS;
+ }
+
+ __host__ __device__ HaarFeatureDescriptor32 getFeatureDesc(void)
+ {
+ return *(HaarFeatureDescriptor32 *)&this->_ui4.x;
+ }
+
+ __host__ __device__ Ncv32f getThreshold(void)
+ {
+ return *(Ncv32f*)&this->_ui4.y;
+ }
+
+ __host__ __device__ HaarClassifierNodeDescriptor32 getLeftNodeDesc(void)
+ {
+ return *(HaarClassifierNodeDescriptor32 *)&this->_ui4.z;
+ }
+
+ __host__ __device__ HaarClassifierNodeDescriptor32 getRightNodeDesc(void)
+ {
+ return *(HaarClassifierNodeDescriptor32 *)&this->_ui4.w;
+ }
+};
+
+struct HaarStage64
+{
+#define HaarStage64_Interpret_MaskRootNodes 0x0000FFFF
+#define HaarStage64_Interpret_MaskRootNodeOffset 0xFFFF0000
+#define HaarStage64_Interpret_ShiftRootNodeOffset 16
+
+ uint2 _ui2;
+
+ __host__ NCVStatus setStageThreshold(Ncv32f t)
+ {
+ this->_ui2.x = *(Ncv32u *)&t;
+ return NCV_SUCCESS;
+ }
+
+ __host__ NCVStatus setStartClassifierRootNodeOffset(Ncv32u val)
+ {
+ if (val > (HaarStage64_Interpret_MaskRootNodeOffset >> HaarStage64_Interpret_ShiftRootNodeOffset))
+ {
+ return NCV_HAAR_XML_LOADING_EXCEPTION;
+ }
+ this->_ui2.y = (val << HaarStage64_Interpret_ShiftRootNodeOffset) | (this->_ui2.y & HaarStage64_Interpret_MaskRootNodes);
+ return NCV_SUCCESS;
+ }
+
+ __host__ NCVStatus setNumClassifierRootNodes(Ncv32u val)
+ {
+ if (val > HaarStage64_Interpret_MaskRootNodes)
+ {
+ return NCV_HAAR_XML_LOADING_EXCEPTION;
+ }
+ this->_ui2.y = val | (this->_ui2.y & HaarStage64_Interpret_MaskRootNodeOffset);
+ return NCV_SUCCESS;
+ }
+
+ __host__ __device__ Ncv32f getStageThreshold(void)
+ {
+ return *(Ncv32f*)&this->_ui2.x;
+ }
+
+ __host__ __device__ Ncv32u getStartClassifierRootNodeOffset(void)
+ {
+ return (this->_ui2.y >> HaarStage64_Interpret_ShiftRootNodeOffset);
+ }
+
+ __host__ __device__ Ncv32u getNumClassifierRootNodes(void)
+ {
+ return (this->_ui2.y & HaarStage64_Interpret_MaskRootNodes);
+ }
+};
+
+NCV_CT_ASSERT(sizeof(HaarFeature64) == 8);
+NCV_CT_ASSERT(sizeof(HaarFeatureDescriptor32) == 4);
+NCV_CT_ASSERT(sizeof(HaarClassifierNodeDescriptor32) == 4);
+NCV_CT_ASSERT(sizeof(HaarClassifierNode128) == 16);
+NCV_CT_ASSERT(sizeof(HaarStage64) == 8);
+
+/**
+ * \brief Classifier cascade descriptor
+ */
+struct HaarClassifierCascadeDescriptor
+{
+ Ncv32u NumStages;
+ Ncv32u NumClassifierRootNodes;
+ Ncv32u NumClassifierTotalNodes;
+ Ncv32u NumFeatures;
+ NcvSize32u ClassifierSize;
+ NcvBool bNeedsTiltedII;
+ NcvBool bHasStumpsOnly;
+};
+
+//==============================================================================
+//
+// Functional interface
+//
+//==============================================================================
+
+enum
+{
+ NCVPipeObjDet_Default = 0x000,
+ NCVPipeObjDet_UseFairImageScaling = 0x001,
+ NCVPipeObjDet_FindLargestObject = 0x002,
+ NCVPipeObjDet_VisualizeInPlace = 0x004,
+};
+
+NCV_EXPORTS NCVStatus ncvDetectObjectsMultiScale_device(NCVMatrix<Ncv8u> &d_srcImg,
+ NcvSize32u srcRoi,
+ NCVVector<NcvRect32u> &d_dstRects,
+ Ncv32u &dstNumRects,
+
+ HaarClassifierCascadeDescriptor &haar,
+ NCVVector<HaarStage64> &h_HaarStages,
+ NCVVector<HaarStage64> &d_HaarStages,
+ NCVVector<HaarClassifierNode128> &d_HaarNodes,
+ NCVVector<HaarFeature64> &d_HaarFeatures,
+
+ NcvSize32u minObjSize,
+ Ncv32u minNeighbors, //default 4
+ Ncv32f scaleStep, //default 1.2f
+ Ncv32u pixelStep, //default 1
+ Ncv32u flags, //default NCVPipeObjDet_Default
+
+ INCVMemAllocator &gpuAllocator,
+ INCVMemAllocator &cpuAllocator,
+ cudaDeviceProp &devProp,
+ cudaStream_t cuStream);
+
+#define OBJDET_MASK_ELEMENT_INVALID_32U 0xFFFFFFFF
+#define HAAR_STDDEV_BORDER 1
+
+NCV_EXPORTS NCVStatus ncvApplyHaarClassifierCascade_device(NCVMatrix<Ncv32u> &d_integralImage,
+ NCVMatrix<Ncv32f> &d_weights,
+ NCVMatrixAlloc<Ncv32u> &d_pixelMask,
+ Ncv32u &numDetections,
+ HaarClassifierCascadeDescriptor &haar,
+ NCVVector<HaarStage64> &h_HaarStages,
+ NCVVector<HaarStage64> &d_HaarStages,
+ NCVVector<HaarClassifierNode128> &d_HaarNodes,
+ NCVVector<HaarFeature64> &d_HaarFeatures,
+ NcvBool bMaskElements,
+ NcvSize32u anchorsRoi,
+ Ncv32u pixelStep,
+ Ncv32f scaleArea,
+ INCVMemAllocator &gpuAllocator,
+ INCVMemAllocator &cpuAllocator,
+ cudaDeviceProp &devProp,
+ cudaStream_t cuStream);
+
+NCV_EXPORTS NCVStatus ncvApplyHaarClassifierCascade_host(NCVMatrix<Ncv32u> &h_integralImage,
+ NCVMatrix<Ncv32f> &h_weights,
+ NCVMatrixAlloc<Ncv32u> &h_pixelMask,
+ Ncv32u &numDetections,
+ HaarClassifierCascadeDescriptor &haar,
+ NCVVector<HaarStage64> &h_HaarStages,
+ NCVVector<HaarClassifierNode128> &h_HaarNodes,
+ NCVVector<HaarFeature64> &h_HaarFeatures,
+ NcvBool bMaskElements,
+ NcvSize32u anchorsRoi,
+ Ncv32u pixelStep,
+ Ncv32f scaleArea);
+
+#define RECT_SIMILARITY_PROPORTION 0.2f
+
+NCV_EXPORTS NCVStatus ncvGrowDetectionsVector_device(NCVVector<Ncv32u> &pixelMask,
+ Ncv32u numPixelMaskDetections,
+ NCVVector<NcvRect32u> &hypotheses,
+ Ncv32u &totalDetections,
+ Ncv32u totalMaxDetections,
+ Ncv32u rectWidth,
+ Ncv32u rectHeight,
+ Ncv32f curScale,
+ cudaStream_t cuStream);
+
+NCV_EXPORTS NCVStatus ncvGrowDetectionsVector_host(NCVVector<Ncv32u> &pixelMask,
+ Ncv32u numPixelMaskDetections,
+ NCVVector<NcvRect32u> &hypotheses,
+ Ncv32u &totalDetections,
+ Ncv32u totalMaxDetections,
+ Ncv32u rectWidth,
+ Ncv32u rectHeight,
+ Ncv32f curScale);
+
+NCV_EXPORTS NCVStatus ncvHaarGetClassifierSize(const std::string &filename, Ncv32u &numStages,
+ Ncv32u &numNodes, Ncv32u &numFeatures);
+
+NCV_EXPORTS NCVStatus ncvHaarLoadFromFile_host(const std::string &filename,
+ HaarClassifierCascadeDescriptor &haar,
+ NCVVector<HaarStage64> &h_HaarStages,
+ NCVVector<HaarClassifierNode128> &h_HaarNodes,
+ NCVVector<HaarFeature64> &h_HaarFeatures);
+
+NCV_EXPORTS NCVStatus ncvHaarStoreNVBIN_host(const std::string &filename,
+ HaarClassifierCascadeDescriptor haar,
+ NCVVector<HaarStage64> &h_HaarStages,
+ NCVVector<HaarClassifierNode128> &h_HaarNodes,
+ NCVVector<HaarFeature64> &h_HaarFeatures);
+
+#endif // PCL_GPU_PEOPLE_NCVHAAROBJECTDETECTION_HPP_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (C) 2009-2010, NVIDIA Corporation, all rights reserved.
+ * Third party copyrights are property of their respective owners.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * Ported to PCL by Koen Buys : Attention Work in progress!
+ */
+
+#ifndef _ncv_pixel_operations_hpp_
+#define _ncv_pixel_operations_hpp_
+
+#include <limits.h>
+#include <float.h>
+#include "NCV.hpp"
+
+template<typename TBase> inline __host__ __device__ TBase _pixMaxVal();
+template<> inline __host__ __device__ Ncv8u _pixMaxVal<Ncv8u>() {return UCHAR_MAX;}
+template<> inline __host__ __device__ Ncv16u _pixMaxVal<Ncv16u>() {return USHRT_MAX;}
+template<> inline __host__ __device__ Ncv32u _pixMaxVal<Ncv32u>() {return UINT_MAX;}
+template<> inline __host__ __device__ Ncv8s _pixMaxVal<Ncv8s>() {return SCHAR_MAX;}
+template<> inline __host__ __device__ Ncv16s _pixMaxVal<Ncv16s>() {return SHRT_MAX;}
+template<> inline __host__ __device__ Ncv32s _pixMaxVal<Ncv32s>() {return INT_MAX;}
+template<> inline __host__ __device__ Ncv32f _pixMaxVal<Ncv32f>() {return FLT_MAX;}
+template<> inline __host__ __device__ Ncv64f _pixMaxVal<Ncv64f>() {return DBL_MAX;}
+
+template<typename TBase> inline __host__ __device__ TBase _pixMinVal();
+template<> inline __host__ __device__ Ncv8u _pixMinVal<Ncv8u>() {return 0;}
+template<> inline __host__ __device__ Ncv16u _pixMinVal<Ncv16u>() {return 0;}
+template<> inline __host__ __device__ Ncv32u _pixMinVal<Ncv32u>() {return 0;}
+template<> inline __host__ __device__ Ncv8s _pixMinVal<Ncv8s>() {return SCHAR_MIN;}
+template<> inline __host__ __device__ Ncv16s _pixMinVal<Ncv16s>() {return SHRT_MIN;}
+template<> inline __host__ __device__ Ncv32s _pixMinVal<Ncv32s>() {return INT_MIN;}
+template<> inline __host__ __device__ Ncv32f _pixMinVal<Ncv32f>() {return FLT_MIN;}
+template<> inline __host__ __device__ Ncv64f _pixMinVal<Ncv64f>() {return DBL_MIN;}
+
+template<typename Tvec> struct TConvVec2Base;
+template<> struct TConvVec2Base<uchar1> {typedef Ncv8u TBase;};
+template<> struct TConvVec2Base<uchar3> {typedef Ncv8u TBase;};
+template<> struct TConvVec2Base<uchar4> {typedef Ncv8u TBase;};
+template<> struct TConvVec2Base<ushort1> {typedef Ncv16u TBase;};
+template<> struct TConvVec2Base<ushort3> {typedef Ncv16u TBase;};
+template<> struct TConvVec2Base<ushort4> {typedef Ncv16u TBase;};
+template<> struct TConvVec2Base<uint1> {typedef Ncv32u TBase;};
+template<> struct TConvVec2Base<uint3> {typedef Ncv32u TBase;};
+template<> struct TConvVec2Base<uint4> {typedef Ncv32u TBase;};
+template<> struct TConvVec2Base<float1> {typedef Ncv32f TBase;};
+template<> struct TConvVec2Base<float3> {typedef Ncv32f TBase;};
+template<> struct TConvVec2Base<float4> {typedef Ncv32f TBase;};
+template<> struct TConvVec2Base<double1> {typedef Ncv64f TBase;};
+template<> struct TConvVec2Base<double3> {typedef Ncv64f TBase;};
+template<> struct TConvVec2Base<double4> {typedef Ncv64f TBase;};
+
+#define NC(T) (sizeof(T) / sizeof(TConvVec2Base<T>::TBase))
+
+template<typename TBase, Ncv32u NC> struct TConvBase2Vec;
+template<> struct TConvBase2Vec<Ncv8u, 1> {typedef uchar1 TVec;};
+template<> struct TConvBase2Vec<Ncv8u, 3> {typedef uchar3 TVec;};
+template<> struct TConvBase2Vec<Ncv8u, 4> {typedef uchar4 TVec;};
+template<> struct TConvBase2Vec<Ncv16u, 1> {typedef ushort1 TVec;};
+template<> struct TConvBase2Vec<Ncv16u, 3> {typedef ushort3 TVec;};
+template<> struct TConvBase2Vec<Ncv16u, 4> {typedef ushort4 TVec;};
+template<> struct TConvBase2Vec<Ncv32u, 1> {typedef uint1 TVec;};
+template<> struct TConvBase2Vec<Ncv32u, 3> {typedef uint3 TVec;};
+template<> struct TConvBase2Vec<Ncv32u, 4> {typedef uint4 TVec;};
+template<> struct TConvBase2Vec<Ncv32f, 1> {typedef float1 TVec;};
+template<> struct TConvBase2Vec<Ncv32f, 3> {typedef float3 TVec;};
+template<> struct TConvBase2Vec<Ncv32f, 4> {typedef float4 TVec;};
+template<> struct TConvBase2Vec<Ncv64f, 1> {typedef double1 TVec;};
+template<> struct TConvBase2Vec<Ncv64f, 3> {typedef double3 TVec;};
+template<> struct TConvBase2Vec<Ncv64f, 4> {typedef double4 TVec;};
+
+//TODO: consider using CUDA intrinsics to avoid branching
+template<typename Tin> static inline __host__ __device__ void _TDemoteClampZ(Tin &a, Ncv8u &out) {out = (Ncv8u)CLAMP_0_255(a);};
+template<typename Tin> static inline __host__ __device__ void _TDemoteClampZ(Tin &a, Ncv16u &out) {out = (Ncv16u)CLAMP(a, 0, USHRT_MAX);}
+template<typename Tin> static inline __host__ __device__ void _TDemoteClampZ(Tin &a, Ncv32u &out) {out = (Ncv32u)CLAMP(a, 0, UINT_MAX);}
+template<typename Tin> static inline __host__ __device__ void _TDemoteClampZ(Tin &a, Ncv32f &out) {out = (Ncv32f)a;}
+
+//TODO: consider using CUDA intrinsics to avoid branching
+template<typename Tin> static inline __host__ __device__ void _TDemoteClampNN(Tin &a, Ncv8u &out) {out = (Ncv8u)CLAMP_0_255(a+0.5f);}
+template<typename Tin> static inline __host__ __device__ void _TDemoteClampNN(Tin &a, Ncv16u &out) {out = (Ncv16u)CLAMP(a+0.5f, 0, USHRT_MAX);}
+template<typename Tin> static inline __host__ __device__ void _TDemoteClampNN(Tin &a, Ncv32u &out) {out = (Ncv32u)CLAMP(a+0.5f, 0, UINT_MAX);}
+template<typename Tin> static inline __host__ __device__ void _TDemoteClampNN(Tin &a, Ncv32f &out) {out = (Ncv32f)a;}
+
+template<typename Tout> inline Tout _pixMakeZero();
+template<> inline __host__ __device__ uchar1 _pixMakeZero<uchar1>() {return make_uchar1(0);}
+template<> inline __host__ __device__ uchar3 _pixMakeZero<uchar3>() {return make_uchar3(0,0,0);}
+template<> inline __host__ __device__ uchar4 _pixMakeZero<uchar4>() {return make_uchar4(0,0,0,0);}
+template<> inline __host__ __device__ ushort1 _pixMakeZero<ushort1>() {return make_ushort1(0);}
+template<> inline __host__ __device__ ushort3 _pixMakeZero<ushort3>() {return make_ushort3(0,0,0);}
+template<> inline __host__ __device__ ushort4 _pixMakeZero<ushort4>() {return make_ushort4(0,0,0,0);}
+template<> inline __host__ __device__ uint1 _pixMakeZero<uint1>() {return make_uint1(0);}
+template<> inline __host__ __device__ uint3 _pixMakeZero<uint3>() {return make_uint3(0,0,0);}
+template<> inline __host__ __device__ uint4 _pixMakeZero<uint4>() {return make_uint4(0,0,0,0);}
+template<> inline __host__ __device__ float1 _pixMakeZero<float1>() {return make_float1(0.f);}
+template<> inline __host__ __device__ float3 _pixMakeZero<float3>() {return make_float3(0.f,0.f,0.f);}
+template<> inline __host__ __device__ float4 _pixMakeZero<float4>() {return make_float4(0.f,0.f,0.f,0.f);}
+template<> inline __host__ __device__ double1 _pixMakeZero<double1>() {return make_double1(0.);}
+template<> inline __host__ __device__ double3 _pixMakeZero<double3>() {return make_double3(0.,0.,0.);}
+template<> inline __host__ __device__ double4 _pixMakeZero<double4>() {return make_double4(0.,0.,0.,0.);}
+
+static inline __host__ __device__ uchar1 _pixMake(Ncv8u x) {return make_uchar1(x);}
+static inline __host__ __device__ uchar3 _pixMake(Ncv8u x, Ncv8u y, Ncv8u z) {return make_uchar3(x,y,z);}
+static inline __host__ __device__ uchar4 _pixMake(Ncv8u x, Ncv8u y, Ncv8u z, Ncv8u w) {return make_uchar4(x,y,z,w);}
+static inline __host__ __device__ ushort1 _pixMake(Ncv16u x) {return make_ushort1(x);}
+static inline __host__ __device__ ushort3 _pixMake(Ncv16u x, Ncv16u y, Ncv16u z) {return make_ushort3(x,y,z);}
+static inline __host__ __device__ ushort4 _pixMake(Ncv16u x, Ncv16u y, Ncv16u z, Ncv16u w) {return make_ushort4(x,y,z,w);}
+static inline __host__ __device__ uint1 _pixMake(Ncv32u x) {return make_uint1(x);}
+static inline __host__ __device__ uint3 _pixMake(Ncv32u x, Ncv32u y, Ncv32u z) {return make_uint3(x,y,z);}
+static inline __host__ __device__ uint4 _pixMake(Ncv32u x, Ncv32u y, Ncv32u z, Ncv32u w) {return make_uint4(x,y,z,w);}
+static inline __host__ __device__ float1 _pixMake(Ncv32f x) {return make_float1(x);}
+static inline __host__ __device__ float3 _pixMake(Ncv32f x, Ncv32f y, Ncv32f z) {return make_float3(x,y,z);}
+static inline __host__ __device__ float4 _pixMake(Ncv32f x, Ncv32f y, Ncv32f z, Ncv32f w) {return make_float4(x,y,z,w);}
+static inline __host__ __device__ double1 _pixMake(Ncv64f x) {return make_double1(x);}
+static inline __host__ __device__ double3 _pixMake(Ncv64f x, Ncv64f y, Ncv64f z) {return make_double3(x,y,z);}
+static inline __host__ __device__ double4 _pixMake(Ncv64f x, Ncv64f y, Ncv64f z, Ncv64f w) {return make_double4(x,y,z,w);}
+
+
+template<typename Tin, typename Tout, Ncv32u CN> struct __pixDemoteClampZ_CN {static __host__ __device__ Tout _pixDemoteClampZ_CN(Tin &pix);};
+
+template<typename Tin, typename Tout> struct __pixDemoteClampZ_CN<Tin, Tout, 1> {
+static __host__ __device__ Tout _pixDemoteClampZ_CN(Tin &pix)
+{
+ Tout out;
+ _TDemoteClampZ(pix.x, out.x);
+ return out;
+}};
+
+template<typename Tin, typename Tout> struct __pixDemoteClampZ_CN<Tin, Tout, 3> {
+static __host__ __device__ Tout _pixDemoteClampZ_CN(Tin &pix)
+{
+ Tout out;
+ _TDemoteClampZ(pix.x, out.x);
+ _TDemoteClampZ(pix.y, out.y);
+ _TDemoteClampZ(pix.z, out.z);
+ return out;
+}};
+
+template<typename Tin, typename Tout> struct __pixDemoteClampZ_CN<Tin, Tout, 4> {
+static __host__ __device__ Tout _pixDemoteClampZ_CN(Tin &pix)
+{
+ Tout out;
+ _TDemoteClampZ(pix.x, out.x);
+ _TDemoteClampZ(pix.y, out.y);
+ _TDemoteClampZ(pix.z, out.z);
+ _TDemoteClampZ(pix.w, out.w);
+ return out;
+}};
+
+template<typename Tin, typename Tout> static inline __host__ __device__ Tout _pixDemoteClampZ(Tin &pix)
+{
+ return __pixDemoteClampZ_CN<Tin, Tout, NC(Tin)>::_pixDemoteClampZ_CN(pix);
+}
+
+
+template<typename Tin, typename Tout, Ncv32u CN> struct __pixDemoteClampNN_CN {static __host__ __device__ Tout _pixDemoteClampNN_CN(Tin &pix);};
+
+template<typename Tin, typename Tout> struct __pixDemoteClampNN_CN<Tin, Tout, 1> {
+static __host__ __device__ Tout _pixDemoteClampNN_CN(Tin &pix)
+{
+ Tout out;
+ _TDemoteClampNN(pix.x, out.x);
+ return out;
+}};
+
+template<typename Tin, typename Tout> struct __pixDemoteClampNN_CN<Tin, Tout, 3> {
+static __host__ __device__ Tout _pixDemoteClampNN_CN(Tin &pix)
+{
+ Tout out;
+ _TDemoteClampNN(pix.x, out.x);
+ _TDemoteClampNN(pix.y, out.y);
+ _TDemoteClampNN(pix.z, out.z);
+ return out;
+}};
+
+template<typename Tin, typename Tout> struct __pixDemoteClampNN_CN<Tin, Tout, 4> {
+static __host__ __device__ Tout _pixDemoteClampNN_CN(Tin &pix)
+{
+ Tout out;
+ _TDemoteClampNN(pix.x, out.x);
+ _TDemoteClampNN(pix.y, out.y);
+ _TDemoteClampNN(pix.z, out.z);
+ _TDemoteClampNN(pix.w, out.w);
+ return out;
+}};
+
+template<typename Tin, typename Tout> static inline __host__ __device__ Tout _pixDemoteClampNN(Tin &pix)
+{
+ return __pixDemoteClampNN_CN<Tin, Tout, NC(Tin)>::_pixDemoteClampNN_CN(pix);
+}
+
+
+template<typename Tin, typename Tout, typename Tw, Ncv32u CN> struct __pixScale_CN {static __host__ __device__ Tout _pixScale_CN(Tin &pix, Tw w);};
+
+template<typename Tin, typename Tout, typename Tw> struct __pixScale_CN<Tin, Tout, Tw, 1> {
+static __host__ __device__ Tout _pixScale_CN(Tin &pix, Tw w)
+{
+ Tout out;
+ typedef typename TConvVec2Base<Tout>::TBase TBout;
+ out.x = (TBout)(pix.x * w);
+ return out;
+}};
+
+template<typename Tin, typename Tout, typename Tw> struct __pixScale_CN<Tin, Tout, Tw, 3> {
+static __host__ __device__ Tout _pixScale_CN(Tin &pix, Tw w)
+{
+ Tout out;
+ typedef typename TConvVec2Base<Tout>::TBase TBout;
+ out.x = (TBout)(pix.x * w);
+ out.y = (TBout)(pix.y * w);
+ out.z = (TBout)(pix.z * w);
+ return out;
+}};
+
+template<typename Tin, typename Tout, typename Tw> struct __pixScale_CN<Tin, Tout, Tw, 4> {
+static __host__ __device__ Tout _pixScale_CN(Tin &pix, Tw w)
+{
+ Tout out;
+ typedef typename TConvVec2Base<Tout>::TBase TBout;
+ out.x = (TBout)(pix.x * w);
+ out.y = (TBout)(pix.y * w);
+ out.z = (TBout)(pix.z * w);
+ out.w = (TBout)(pix.w * w);
+ return out;
+}};
+
+template<typename Tin, typename Tout, typename Tw> static __host__ __device__ Tout _pixScale(Tin &pix, Tw w)
+{
+ return __pixScale_CN<Tin, Tout, Tw, NC(Tin)>::_pixScale_CN(pix, w);
+}
+
+
+template<typename Tin, typename Tout, Ncv32u CN> struct __pixAdd_CN {static __host__ __device__ Tout _pixAdd_CN(Tout &pix1, Tin &pix2);};
+
+template<typename Tin, typename Tout> struct __pixAdd_CN<Tin, Tout, 1> {
+static __host__ __device__ Tout _pixAdd_CN(Tout &pix1, Tin &pix2)
+{
+ Tout out;
+ out.x = pix1.x + pix2.x;
+ return out;
+}};
+
+template<typename Tin, typename Tout> struct __pixAdd_CN<Tin, Tout, 3> {
+static __host__ __device__ Tout _pixAdd_CN(Tout &pix1, Tin &pix2)
+{
+ Tout out;
+ out.x = pix1.x + pix2.x;
+ out.y = pix1.y + pix2.y;
+ out.z = pix1.z + pix2.z;
+ return out;
+}};
+
+template<typename Tin, typename Tout> struct __pixAdd_CN<Tin, Tout, 4> {
+static __host__ __device__ Tout _pixAdd_CN(Tout &pix1, Tin &pix2)
+{
+ Tout out;
+ out.x = pix1.x + pix2.x;
+ out.y = pix1.y + pix2.y;
+ out.z = pix1.z + pix2.z;
+ out.w = pix1.w + pix2.w;
+ return out;
+}};
+
+template<typename Tin, typename Tout> static __host__ __device__ Tout _pixAdd(Tout &pix1, Tin &pix2)
+{
+ return __pixAdd_CN<Tin, Tout, NC(Tin)>::_pixAdd_CN(pix1, pix2);
+}
+
+
+template<typename Tin, typename Tout, Ncv32u CN> struct __pixDist_CN {static __host__ __device__ Tout _pixDist_CN(Tin &pix1, Tin &pix2);};
+
+template<typename Tin, typename Tout> struct __pixDist_CN<Tin, Tout, 1> {
+static __host__ __device__ Tout _pixDist_CN(Tin &pix1, Tin &pix2)
+{
+ return Tout(SQR(pix1.x - pix2.x));
+}};
+
+template<typename Tin, typename Tout> struct __pixDist_CN<Tin, Tout, 3> {
+static __host__ __device__ Tout _pixDist_CN(Tin &pix1, Tin &pix2)
+{
+ return Tout(SQR(pix1.x - pix2.x) + SQR(pix1.y - pix2.y) + SQR(pix1.z - pix2.z));
+}};
+
+template<typename Tin, typename Tout> struct __pixDist_CN<Tin, Tout, 4> {
+static __host__ __device__ Tout _pixDist_CN(Tin &pix1, Tin &pix2)
+{
+ return Tout(SQR(pix1.x - pix2.x) + SQR(pix1.y - pix2.y) + SQR(pix1.z - pix2.z) + SQR(pix1.w - pix2.w));
+}};
+
+template<typename Tin, typename Tout> static __host__ __device__ Tout _pixDist(Tin &pix1, Tin &pix2)
+{
+ return __pixDist_CN<Tin, Tout, NC(Tin)>::_pixDist_CN(pix1, pix2);
+}
+
+
+template <typename T> struct TAccPixWeighted;
+template<> struct TAccPixWeighted<uchar1> {typedef double1 type;};
+template<> struct TAccPixWeighted<uchar3> {typedef double3 type;};
+template<> struct TAccPixWeighted<uchar4> {typedef double4 type;};
+template<> struct TAccPixWeighted<ushort1> {typedef double1 type;};
+template<> struct TAccPixWeighted<ushort3> {typedef double3 type;};
+template<> struct TAccPixWeighted<ushort4> {typedef double4 type;};
+template<> struct TAccPixWeighted<float1> {typedef double1 type;};
+template<> struct TAccPixWeighted<float3> {typedef double3 type;};
+template<> struct TAccPixWeighted<float4> {typedef double4 type;};
+
+template<typename Tfrom> struct TAccPixDist {};
+template<> struct TAccPixDist<uchar1> {typedef Ncv32u type;};
+template<> struct TAccPixDist<uchar3> {typedef Ncv32u type;};
+template<> struct TAccPixDist<uchar4> {typedef Ncv32u type;};
+template<> struct TAccPixDist<ushort1> {typedef Ncv32u type;};
+template<> struct TAccPixDist<ushort3> {typedef Ncv32u type;};
+template<> struct TAccPixDist<ushort4> {typedef Ncv32u type;};
+template<> struct TAccPixDist<float1> {typedef Ncv32f type;};
+template<> struct TAccPixDist<float3> {typedef Ncv32f type;};
+template<> struct TAccPixDist<float4> {typedef Ncv32f type;};
+
+#endif //_ncv_pixel_operations_hpp_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (C) 2009-2010, NVIDIA Corporation, all rights reserved.
+ * Third party copyrights are property of their respective owners.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * Ported to PCL by Koen Buys : Attention Work in progress!
+ */
+
+
+#include <cuda_runtime.h>
+#include <stdio.h>
+#include "NCV.hpp"
+#include "NCVAlg.hpp"
+#include "NCVPyramid.hpp"
+#include "NCVPixelOperations.hpp"
+//#include "opencv2/gpu/device/common.hpp"
+
+template<typename T, Ncv32u CN> struct __average4_CN {static __host__ __device__ T _average4_CN(const T &p00, const T &p01, const T &p10, const T &p11);};
+
+template<typename T> struct __average4_CN<T, 1> {
+static __host__ __device__ T _average4_CN(const T &p00, const T &p01, const T &p10, const T &p11)
+{
+ T out;
+ out.x = ((Ncv32s)p00.x + p01.x + p10.x + p11.x + 2) / 4;
+ return out;
+}};
+
+template<> struct __average4_CN<float1, 1> {
+static __host__ __device__ float1 _average4_CN(const float1 &p00, const float1 &p01, const float1 &p10, const float1 &p11)
+{
+ float1 out;
+ out.x = (p00.x + p01.x + p10.x + p11.x) / 4;
+ return out;
+}};
+
+template<> struct __average4_CN<double1, 1> {
+static __host__ __device__ double1 _average4_CN(const double1 &p00, const double1 &p01, const double1 &p10, const double1 &p11)
+{
+ double1 out;
+ out.x = (p00.x + p01.x + p10.x + p11.x) / 4;
+ return out;
+}};
+
+template<typename T> struct __average4_CN<T, 3> {
+static __host__ __device__ T _average4_CN(const T &p00, const T &p01, const T &p10, const T &p11)
+{
+ T out;
+ out.x = ((Ncv32s)p00.x + p01.x + p10.x + p11.x + 2) / 4;
+ out.y = ((Ncv32s)p00.y + p01.y + p10.y + p11.y + 2) / 4;
+ out.z = ((Ncv32s)p00.z + p01.z + p10.z + p11.z + 2) / 4;
+ return out;
+}};
+
+template<> struct __average4_CN<float3, 3> {
+static __host__ __device__ float3 _average4_CN(const float3 &p00, const float3 &p01, const float3 &p10, const float3 &p11)
+{
+ float3 out;
+ out.x = (p00.x + p01.x + p10.x + p11.x) / 4;
+ out.y = (p00.y + p01.y + p10.y + p11.y) / 4;
+ out.z = (p00.z + p01.z + p10.z + p11.z) / 4;
+ return out;
+}};
+
+template<> struct __average4_CN<double3, 3> {
+static __host__ __device__ double3 _average4_CN(const double3 &p00, const double3 &p01, const double3 &p10, const double3 &p11)
+{
+ double3 out;
+ out.x = (p00.x + p01.x + p10.x + p11.x) / 4;
+ out.y = (p00.y + p01.y + p10.y + p11.y) / 4;
+ out.z = (p00.z + p01.z + p10.z + p11.z) / 4;
+ return out;
+}};
+
+template<typename T> struct __average4_CN<T, 4> {
+static __host__ __device__ T _average4_CN(const T &p00, const T &p01, const T &p10, const T &p11)
+{
+ T out;
+ out.x = ((Ncv32s)p00.x + p01.x + p10.x + p11.x + 2) / 4;
+ out.y = ((Ncv32s)p00.y + p01.y + p10.y + p11.y + 2) / 4;
+ out.z = ((Ncv32s)p00.z + p01.z + p10.z + p11.z + 2) / 4;
+ out.w = ((Ncv32s)p00.w + p01.w + p10.w + p11.w + 2) / 4;
+ return out;
+}};
+
+template<> struct __average4_CN<float4, 4> {
+static __host__ __device__ float4 _average4_CN(const float4 &p00, const float4 &p01, const float4 &p10, const float4 &p11)
+{
+ float4 out;
+ out.x = (p00.x + p01.x + p10.x + p11.x) / 4;
+ out.y = (p00.y + p01.y + p10.y + p11.y) / 4;
+ out.z = (p00.z + p01.z + p10.z + p11.z) / 4;
+ out.w = (p00.w + p01.w + p10.w + p11.w) / 4;
+ return out;
+}};
+
+template<> struct __average4_CN<double4, 4> {
+static __host__ __device__ double4 _average4_CN(const double4 &p00, const double4 &p01, const double4 &p10, const double4 &p11)
+{
+ double4 out;
+ out.x = (p00.x + p01.x + p10.x + p11.x) / 4;
+ out.y = (p00.y + p01.y + p10.y + p11.y) / 4;
+ out.z = (p00.z + p01.z + p10.z + p11.z) / 4;
+ out.w = (p00.w + p01.w + p10.w + p11.w) / 4;
+ return out;
+}};
+
+template<typename T> static __host__ __device__ T _average4(const T &p00, const T &p01, const T &p10, const T &p11)
+{
+ return __average4_CN<T, NC(T)>::_average4_CN(p00, p01, p10, p11);
+}
+
+template<typename Tin, typename Tout, Ncv32u CN> struct __lerp_CN {static __host__ __device__ Tout _lerp_CN(const Tin &a, const Tin &b, Ncv32f d);};
+
+template<typename Tin, typename Tout> struct __lerp_CN<Tin, Tout, 1> {
+static __host__ __device__ Tout _lerp_CN(const Tin &a, const Tin &b, Ncv32f d)
+{
+ typedef typename TConvVec2Base<Tout>::TBase TB;
+ return _pixMake(TB(b.x * d + a.x * (1 - d)));
+}};
+
+template<typename Tin, typename Tout> struct __lerp_CN<Tin, Tout, 3> {
+static __host__ __device__ Tout _lerp_CN(const Tin &a, const Tin &b, Ncv32f d)
+{
+ typedef typename TConvVec2Base<Tout>::TBase TB;
+ return _pixMake(TB(b.x * d + a.x * (1 - d)),
+ TB(b.y * d + a.y * (1 - d)),
+ TB(b.z * d + a.z * (1 - d)));
+}};
+
+template<typename Tin, typename Tout> struct __lerp_CN<Tin, Tout, 4> {
+static __host__ __device__ Tout _lerp_CN(const Tin &a, const Tin &b, Ncv32f d)
+{
+ typedef typename TConvVec2Base<Tout>::TBase TB;
+ return _pixMake(TB(b.x * d + a.x * (1 - d)),
+ TB(b.y * d + a.y * (1 - d)),
+ TB(b.z * d + a.z * (1 - d)),
+ TB(b.w * d + a.w * (1 - d)));
+}};
+
+template<typename Tin, typename Tout> static __host__ __device__ Tout _lerp(const Tin &a, const Tin &b, Ncv32f d)
+{
+ return __lerp_CN<Tin, Tout, NC(Tin)>::_lerp_CN(a, b, d);
+}
+
+template<typename T>
+__global__ void kernelDownsampleX2(T *d_src,
+ Ncv32u srcPitch,
+ T *d_dst,
+ Ncv32u dstPitch,
+ NcvSize32u dstRoi)
+{
+ Ncv32u i = blockIdx.y * blockDim.y + threadIdx.y;
+ Ncv32u j = blockIdx.x * blockDim.x + threadIdx.x;
+
+ if (i < dstRoi.height && j < dstRoi.width)
+ {
+ T *d_src_line1 = (T *)((Ncv8u *)d_src + (2 * i + 0) * srcPitch);
+ T *d_src_line2 = (T *)((Ncv8u *)d_src + (2 * i + 1) * srcPitch);
+ T *d_dst_line = (T *)((Ncv8u *)d_dst + i * dstPitch);
+
+ T p00 = d_src_line1[2*j+0];
+ T p01 = d_src_line1[2*j+1];
+ T p10 = d_src_line2[2*j+0];
+ T p11 = d_src_line2[2*j+1];
+
+ d_dst_line[j] = _average4(p00, p01, p10, p11);
+ }
+}
+
+/*
+namespace cv { namespace gpu { namespace device
+{
+ namespace pyramid
+ {
+ template <typename T> void kernelDownsampleX2_gpu(DevMem2Db src, DevMem2Db dst, cudaStream_t stream)
+ {
+ dim3 bDim(16, 8);
+ dim3 gDim(divUp(src.cols, bDim.x), divUp(src.rows, bDim.y));
+
+ kernelDownsampleX2<<<gDim, bDim, 0, stream>>>((T*)src.data, src.step, (T*)dst.data, dst.step, NcvSize32u(dst.cols, dst.rows));
+
+ cudaSafeCall( cudaGetLastError() );
+
+ if (stream == 0)
+ cudaSafeCall( cudaDeviceSynchronize() );
+ }
+
+ template void kernelDownsampleX2_gpu<uchar1>(DevMem2Db src, DevMem2Db dst, cudaStream_t stream);
+ template void kernelDownsampleX2_gpu<uchar3>(DevMem2Db src, DevMem2Db dst, cudaStream_t stream);
+ template void kernelDownsampleX2_gpu<uchar4>(DevMem2Db src, DevMem2Db dst, cudaStream_t stream);
+
+ template void kernelDownsampleX2_gpu<ushort1>(DevMem2Db src, DevMem2Db dst, cudaStream_t stream);
+ template void kernelDownsampleX2_gpu<ushort3>(DevMem2Db src, DevMem2Db dst, cudaStream_t stream);
+ template void kernelDownsampleX2_gpu<ushort4>(DevMem2Db src, DevMem2Db dst, cudaStream_t stream);
+
+ template void kernelDownsampleX2_gpu<float1>(DevMem2Db src, DevMem2Db dst, cudaStream_t stream);
+ template void kernelDownsampleX2_gpu<float3>(DevMem2Db src, DevMem2Db dst, cudaStream_t stream);
+ template void kernelDownsampleX2_gpu<float4>(DevMem2Db src, DevMem2Db dst, cudaStream_t stream);
+ }
+}}} */
+
+template<typename T>
+__global__ void kernelInterpolateFrom1(T *d_srcTop,
+ Ncv32u srcTopPitch,
+ NcvSize32u szTopRoi,
+ T *d_dst,
+ Ncv32u dstPitch,
+ NcvSize32u dstRoi)
+{
+ Ncv32u i = blockIdx.y * blockDim.y + threadIdx.y;
+ Ncv32u j = blockIdx.x * blockDim.x + threadIdx.x;
+
+ if (i < dstRoi.height && j < dstRoi.width)
+ {
+ Ncv32f ptTopX = 1.0f * (szTopRoi.width - 1) * j / (dstRoi.width - 1);
+ Ncv32f ptTopY = 1.0f * (szTopRoi.height - 1) * i / (dstRoi.height - 1);
+ Ncv32u xl = (Ncv32u)ptTopX;
+ Ncv32u xh = xl+1;
+ Ncv32f dx = ptTopX - xl;
+ Ncv32u yl = (Ncv32u)ptTopY;
+ Ncv32u yh = yl+1;
+ Ncv32f dy = ptTopY - yl;
+
+ T *d_src_line1 = (T *)((Ncv8u *)d_srcTop + yl * srcTopPitch);
+ T *d_src_line2 = (T *)((Ncv8u *)d_srcTop + yh * srcTopPitch);
+ T *d_dst_line = (T *)((Ncv8u *)d_dst + i * dstPitch);
+
+ T p00, p01, p10, p11;
+ p00 = d_src_line1[xl];
+ p01 = xh < szTopRoi.width ? d_src_line1[xh] : p00;
+ p10 = yh < szTopRoi.height ? d_src_line2[xl] : p00;
+ p11 = (xh < szTopRoi.width && yh < szTopRoi.height) ? d_src_line2[xh] : p00;
+ typedef typename TConvBase2Vec<Ncv32f, NC(T)>::TVec TVFlt;
+ TVFlt m_00_01 = _lerp<T, TVFlt>(p00, p01, dx);
+ TVFlt m_10_11 = _lerp<T, TVFlt>(p10, p11, dx);
+ TVFlt mixture = _lerp<TVFlt, TVFlt>(m_00_01, m_10_11, dy);
+ T outPix = _pixDemoteClampZ<TVFlt, T>(mixture);
+
+ d_dst_line[j] = outPix;
+ }
+}
+
+/*
+namespace cv { namespace gpu { namespace device
+{
+ namespace pyramid
+ {
+ template <typename T> void kernelInterpolateFrom1_gpu(DevMem2Db src, DevMem2Db dst, cudaStream_t stream)
+ {
+ dim3 bDim(16, 8);
+ dim3 gDim(divUp(dst.cols, bDim.x), divUp(dst.rows, bDim.y));
+
+ kernelInterpolateFrom1<<<gDim, bDim, 0, stream>>>((T*) src.data, src.step, NcvSize32u(src.cols, src.rows),
+ (T*) dst.data, dst.step, NcvSize32u(dst.cols, dst.rows));
+
+ cudaSafeCall( cudaGetLastError() );
+
+ if (stream == 0)
+ cudaSafeCall( cudaDeviceSynchronize() );
+ }
+
+ template void kernelInterpolateFrom1_gpu<uchar1>(DevMem2Db src, DevMem2Db dst, cudaStream_t stream);
+ template void kernelInterpolateFrom1_gpu<uchar3>(DevMem2Db src, DevMem2Db dst, cudaStream_t stream);
+ template void kernelInterpolateFrom1_gpu<uchar4>(DevMem2Db src, DevMem2Db dst, cudaStream_t stream);
+
+ template void kernelInterpolateFrom1_gpu<ushort1>(DevMem2Db src, DevMem2Db dst, cudaStream_t stream);
+ template void kernelInterpolateFrom1_gpu<ushort3>(DevMem2Db src, DevMem2Db dst, cudaStream_t stream);
+ template void kernelInterpolateFrom1_gpu<ushort4>(DevMem2Db src, DevMem2Db dst, cudaStream_t stream);
+
+ template void kernelInterpolateFrom1_gpu<float1>(DevMem2Db src, DevMem2Db dst, cudaStream_t stream);
+ template void kernelInterpolateFrom1_gpu<float3>(DevMem2Db src, DevMem2Db dst, cudaStream_t stream);
+ template void kernelInterpolateFrom1_gpu<float4>(DevMem2Db src, DevMem2Db dst, cudaStream_t stream);
+ }
+}}} */
+
+
+#if 0 //def _WIN32
+
+template<typename T>
+static T _interpLinear(const T &a, const T &b, Ncv32f d)
+{
+ typedef typename TConvBase2Vec<Ncv32f, NC(T)>::TVec TVFlt;
+ TVFlt tmp = _lerp<T, TVFlt>(a, b, d);
+ return _pixDemoteClampZ<TVFlt, T>(tmp);
+}
+
+template<typename T>
+static T _interpBilinear(const NCVMatrix<T> &refLayer, Ncv32f x, Ncv32f y)
+{
+ Ncv32u xl = (Ncv32u)x;
+ Ncv32u xh = xl+1;
+ Ncv32f dx = x - xl;
+ Ncv32u yl = (Ncv32u)y;
+ Ncv32u yh = yl+1;
+ Ncv32f dy = y - yl;
+ T p00, p01, p10, p11;
+ p00 = refLayer.at(xl, yl);
+ p01 = xh < refLayer.width() ? refLayer.at(xh, yl) : p00;
+ p10 = yh < refLayer.height() ? refLayer.at(xl, yh) : p00;
+ p11 = (xh < refLayer.width() && yh < refLayer.height()) ? refLayer.at(xh, yh) : p00;
+ typedef typename TConvBase2Vec<Ncv32f, NC(T)>::TVec TVFlt;
+ TVFlt m_00_01 = _lerp<T, TVFlt>(p00, p01, dx);
+ TVFlt m_10_11 = _lerp<T, TVFlt>(p10, p11, dx);
+ TVFlt mixture = _lerp<TVFlt, TVFlt>(m_00_01, m_10_11, dy);
+ return _pixDemoteClampZ<TVFlt, T>(mixture);
+}
+
+template <class T>
+NCVImagePyramid<T>::NCVImagePyramid(const NCVMatrix<T> &img,
+ Ncv8u numLayers,
+ INCVMemAllocator &alloc,
+ cudaStream_t cuStream)
+{
+ this->_isInitialized = false;
+ ncvAssertPrintReturn(img.memType() == alloc.memType(), "NCVImagePyramid::ctor error", );
+
+ this->layer0 = &img;
+ NcvSize32u szLastLayer(img.width(), img.height());
+ this->nLayers = 1;
+
+ NCV_SET_SKIP_COND(alloc.isCounting());
+ NcvBool bDeviceCode = alloc.memType() == NCVMemoryTypeDevice;
+
+ if (numLayers == 0)
+ {
+ numLayers = 255; //it will cut-off when any of the dimensions goes 1
+ }
+
+#ifdef SELF_CHECK_GPU
+ NCVMemNativeAllocator allocCPU(NCVMemoryTypeHostPinned, 512);
+#endif
+
+ for (Ncv32u i=0; i<(Ncv32u)numLayers-1; i++)
+ {
+ NcvSize32u szCurLayer(szLastLayer.width / 2, szLastLayer.height / 2);
+ if (szCurLayer.width == 0 || szCurLayer.height == 0)
+ {
+ break;
+ }
+
+ this->pyramid.push_back(new NCVMatrixAlloc<T>(alloc, szCurLayer.width, szCurLayer.height));
+ ncvAssertPrintReturn(((NCVMatrixAlloc<T> *)(this->pyramid[i]))->isMemAllocated(), "NCVImagePyramid::ctor error", );
+ this->nLayers++;
+
+ //fill in the layer
+ NCV_SKIP_COND_BEGIN
+
+ const NCVMatrix<T> *prevLayer = i == 0 ? this->layer0 : this->pyramid[i-1];
+ NCVMatrix<T> *curLayer = this->pyramid[i];
+
+ if (bDeviceCode)
+ {
+ dim3 bDim(16, 8);
+ dim3 gDim(divUp(szCurLayer.width, bDim.x), divUp(szCurLayer.height, bDim.y));
+ kernelDownsampleX2<<<gDim, bDim, 0, cuStream>>>(prevLayer->ptr(),
+ prevLayer->pitch(),
+ curLayer->ptr(),
+ curLayer->pitch(),
+ szCurLayer);
+ ncvAssertPrintReturn(cudaSuccess == cudaGetLastError(), "NCVImagePyramid::ctor error", );
+
+#ifdef SELF_CHECK_GPU
+ NCVMatrixAlloc<T> h_prevLayer(allocCPU, prevLayer->width(), prevLayer->height());
+ ncvAssertPrintReturn(h_prevLayer.isMemAllocated(), "Validation failure in NCVImagePyramid::ctor", );
+ NCVMatrixAlloc<T> h_curLayer(allocCPU, curLayer->width(), curLayer->height());
+ ncvAssertPrintReturn(h_curLayer.isMemAllocated(), "Validation failure in NCVImagePyramid::ctor", );
+ ncvAssertPrintReturn(NCV_SUCCESS == prevLayer->copy2D(h_prevLayer, prevLayer->size(), cuStream), "Validation failure in NCVImagePyramid::ctor", );
+ ncvAssertPrintReturn(NCV_SUCCESS == curLayer->copy2D(h_curLayer, curLayer->size(), cuStream), "Validation failure in NCVImagePyramid::ctor", );
+ ncvAssertPrintReturn(cudaSuccess == cudaStreamSynchronize(cuStream), "Validation failure in NCVImagePyramid::ctor", );
+ for (Ncv32u i=0; i<szCurLayer.height; i++)
+ {
+ for (Ncv32u j=0; j<szCurLayer.width; j++)
+ {
+ T p00 = h_prevLayer.at(2*j+0, 2*i+0);
+ T p01 = h_prevLayer.at(2*j+1, 2*i+0);
+ T p10 = h_prevLayer.at(2*j+0, 2*i+1);
+ T p11 = h_prevLayer.at(2*j+1, 2*i+1);
+ T outGold = _average4(p00, p01, p10, p11);
+ T outGPU = h_curLayer.at(j, i);
+ ncvAssertPrintReturn(0 == memcmp(&outGold, &outGPU, sizeof(T)), "Validation failure in NCVImagePyramid::ctor with kernelDownsampleX2", );
+ }
+ }
+#endif
+ }
+ else
+ {
+ for (Ncv32u i=0; i<szCurLayer.height; i++)
+ {
+ for (Ncv32u j=0; j<szCurLayer.width; j++)
+ {
+ T p00 = prevLayer->at(2*j+0, 2*i+0);
+ T p01 = prevLayer->at(2*j+1, 2*i+0);
+ T p10 = prevLayer->at(2*j+0, 2*i+1);
+ T p11 = prevLayer->at(2*j+1, 2*i+1);
+ curLayer->at(j, i) = _average4(p00, p01, p10, p11);
+ }
+ }
+ }
+
+ NCV_SKIP_COND_END
+
+ szLastLayer = szCurLayer;
+ }
+
+ this->_isInitialized = true;
+}
+
+template <class T>
+NCVImagePyramid<T>::~NCVImagePyramid()
+{
+}
+
+template <class T>
+NcvBool NCVImagePyramid<T>::isInitialized() const
+{
+ return this->_isInitialized;
+}
+
+template <class T>
+NCVStatus NCVImagePyramid<T>::getLayer(NCVMatrix<T> &outImg,
+ NcvSize32u outRoi,
+ NcvBool bTrilinear,
+ cudaStream_t cuStream) const
+{
+ ncvAssertReturn(this->isInitialized(), NCV_UNKNOWN_ERROR);
+ ncvAssertReturn(outImg.memType() == this->layer0->memType(), NCV_MEM_RESIDENCE_ERROR);
+ ncvAssertReturn(outRoi.width <= this->layer0->width() && outRoi.height <= this->layer0->height() &&
+ outRoi.width > 0 && outRoi.height > 0, NCV_DIMENSIONS_INVALID);
+
+ if (outRoi.width == this->layer0->width() && outRoi.height == this->layer0->height())
+ {
+ ncvAssertReturnNcvStat(this->layer0->copy2D(outImg, NcvSize32u(this->layer0->width(), this->layer0->height()), cuStream));
+ return NCV_SUCCESS;
+ }
+
+ Ncv32f lastScale = 1.0f;
+ Ncv32f curScale;
+ const NCVMatrix<T> *lastLayer = this->layer0;
+ const NCVMatrix<T> *curLayer = NULL;
+ NcvBool bUse2Refs = false;
+
+ for (Ncv32u i=0; i<this->nLayers-1; i++)
+ {
+ curScale = lastScale * 0.5f;
+ curLayer = this->pyramid[i];
+
+ if (outRoi.width == curLayer->width() && outRoi.height == curLayer->height())
+ {
+ ncvAssertReturnNcvStat(this->pyramid[i]->copy2D(outImg, NcvSize32u(this->pyramid[i]->width(), this->pyramid[i]->height()), cuStream));
+ return NCV_SUCCESS;
+ }
+
+ if (outRoi.width >= curLayer->width() && outRoi.height >= curLayer->height())
+ {
+ if (outRoi.width < lastLayer->width() && outRoi.height < lastLayer->height())
+ {
+ bUse2Refs = true;
+ }
+ break;
+ }
+
+ lastScale = curScale;
+ lastLayer = curLayer;
+ }
+
+ bUse2Refs = bUse2Refs && bTrilinear;
+
+ NCV_SET_SKIP_COND(outImg.memType() == NCVMemoryTypeNone);
+ NcvBool bDeviceCode = this->layer0->memType() == NCVMemoryTypeDevice;
+
+#ifdef SELF_CHECK_GPU
+ NCVMemNativeAllocator allocCPU(NCVMemoryTypeHostPinned, 512);
+#endif
+
+ NCV_SKIP_COND_BEGIN
+
+ if (bDeviceCode)
+ {
+ ncvAssertReturn(bUse2Refs == false, NCV_NOT_IMPLEMENTED);
+
+ dim3 bDim(16, 8);
+ dim3 gDim(divUp(outRoi.width, bDim.x), divUp(outRoi.height, bDim.y));
+ kernelInterpolateFrom1<<<gDim, bDim, 0, cuStream>>>(lastLayer->ptr(),
+ lastLayer->pitch(),
+ lastLayer->size(),
+ outImg.ptr(),
+ outImg.pitch(),
+ outRoi);
+ ncvAssertCUDAReturn(cudaGetLastError(), NCV_CUDA_ERROR);
+
+#ifdef SELF_CHECK_GPU
+ ncvSafeMatAlloc(h_lastLayer, T, allocCPU, lastLayer->width(), lastLayer->height(), NCV_ALLOCATOR_BAD_ALLOC);
+ ncvSafeMatAlloc(h_outImg, T, allocCPU, outImg.width(), outImg.height(), NCV_ALLOCATOR_BAD_ALLOC);
+ ncvAssertReturnNcvStat(lastLayer->copy2D(h_lastLayer, lastLayer->size(), cuStream));
+ ncvAssertReturnNcvStat(outImg.copy2D(h_outImg, outRoi, cuStream));
+ ncvAssertCUDAReturn(cudaStreamSynchronize(cuStream), NCV_CUDA_ERROR);
+
+ for (Ncv32u i=0; i<outRoi.height; i++)
+ {
+ for (Ncv32u j=0; j<outRoi.width; j++)
+ {
+ NcvSize32u szTopLayer(lastLayer->width(), lastLayer->height());
+ Ncv32f ptTopX = 1.0f * (szTopLayer.width - 1) * j / (outRoi.width - 1);
+ Ncv32f ptTopY = 1.0f * (szTopLayer.height - 1) * i / (outRoi.height - 1);
+ T outGold = _interpBilinear(h_lastLayer, ptTopX, ptTopY);
+ ncvAssertPrintReturn(0 == memcmp(&outGold, &h_outImg.at(j,i), sizeof(T)), "Validation failure in NCVImagePyramid::ctor with kernelInterpolateFrom1", NCV_UNKNOWN_ERROR);
+ }
+ }
+#endif
+ }
+ else
+ {
+ for (Ncv32u i=0; i<outRoi.height; i++)
+ {
+ for (Ncv32u j=0; j<outRoi.width; j++)
+ {
+ //top layer pixel (always exists)
+ NcvSize32u szTopLayer(lastLayer->width(), lastLayer->height());
+ Ncv32f ptTopX = 1.0f * (szTopLayer.width - 1) * j / (outRoi.width - 1);
+ Ncv32f ptTopY = 1.0f * (szTopLayer.height - 1) * i / (outRoi.height - 1);
+ T topPix = _interpBilinear(*lastLayer, ptTopX, ptTopY);
+ T trilinearPix = topPix;
+
+ if (bUse2Refs)
+ {
+ //bottom layer pixel (exists only if the requested scale is greater than the smallest layer scale)
+ NcvSize32u szBottomLayer(curLayer->width(), curLayer->height());
+ Ncv32f ptBottomX = 1.0f * (szBottomLayer.width - 1) * j / (outRoi.width - 1);
+ Ncv32f ptBottomY = 1.0f * (szBottomLayer.height - 1) * i / (outRoi.height - 1);
+ T bottomPix = _interpBilinear(*curLayer, ptBottomX, ptBottomY);
+
+ Ncv32f scale = (1.0f * outRoi.width / layer0->width() + 1.0f * outRoi.height / layer0->height()) / 2;
+ Ncv32f dl = (scale - curScale) / (lastScale - curScale);
+ dl = CLAMP(dl, 0.0f, 1.0f);
+ trilinearPix = _interpLinear(bottomPix, topPix, dl);
+ }
+
+ outImg.at(j, i) = trilinearPix;
+ }
+ }
+ }
+
+ NCV_SKIP_COND_END
+
+ return NCV_SUCCESS;
+}
+
+template class NCVImagePyramid<uchar1>;
+template class NCVImagePyramid<uchar3>;
+template class NCVImagePyramid<uchar4>;
+template class NCVImagePyramid<ushort1>;
+template class NCVImagePyramid<ushort3>;
+template class NCVImagePyramid<ushort4>;
+template class NCVImagePyramid<uint1>;
+template class NCVImagePyramid<uint3>;
+template class NCVImagePyramid<uint4>;
+template class NCVImagePyramid<float1>;
+template class NCVImagePyramid<float3>;
+template class NCVImagePyramid<float4>;
+
+#endif //_WIN32
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (C) 2009-2010, NVIDIA Corporation, all rights reserved.
+ * Third party copyrights are property of their respective owners.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * Ported to PCL by Koen Buys : Attention Work in progress!
+ */
+
+
+#ifndef _ncvpyramid_hpp_
+#define _ncvpyramid_hpp_
+
+#include <memory>
+#include <vector>
+#include "NCV.hpp"
+
+#if 0 //def _WIN32
+
+template <class T>
+class NCV_EXPORTS NCVMatrixStack
+{
+public:
+ NCVMatrixStack() {this->_arr.clear();}
+ ~NCVMatrixStack()
+ {
+ const Ncv32u nElem = this->_arr.size();
+ for (Ncv32u i=0; i<nElem; i++)
+ {
+ pop_back();
+ }
+ }
+ void push_back(NCVMatrix<T> *elem) {this->_arr.push_back(std::tr1::shared_ptr< NCVMatrix<T> >(elem));}
+ void pop_back() {this->_arr.pop_back();}
+ NCVMatrix<T> * operator [] (int i) const {return this->_arr[i].get();}
+private:
+ std::vector< std::tr1::shared_ptr< NCVMatrix<T> > > _arr;
+};
+
+
+template <class T>
+class NCV_EXPORTS NCVImagePyramid
+{
+public:
+
+ NCVImagePyramid(const NCVMatrix<T> &img,
+ Ncv8u nLayers,
+ INCVMemAllocator &alloc,
+ cudaStream_t cuStream);
+ ~NCVImagePyramid();
+ NcvBool isInitialized() const;
+ NCVStatus getLayer(NCVMatrix<T> &outImg,
+ NcvSize32u outRoi,
+ NcvBool bTrilinear,
+ cudaStream_t cuStream) const;
+
+private:
+
+ NcvBool _isInitialized;
+ const NCVMatrix<T> *layer0;
+ NCVMatrixStack<T> pyramid;
+ Ncv32u nLayers;
+};
+
+#endif //_WIN32
+
+#endif //_ncvpyramid_hpp_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (C) 2009-2010, NVIDIA Corporation, all rights reserved.
+ * Third party copyrights are property of their respective owners.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * Ported to PCL by Koen Buys : Attention Work in progress!
+ */
+
+#ifndef _ncvruntimetemplates_hpp_
+#define _ncvruntimetemplates_hpp_
+#if _MSC_VER >= 1200
+#pragma warning( disable: 4800 )
+#endif
+
+
+#include <stdarg.h>
+#include <vector>
+
+
+////////////////////////////////////////////////////////////////////////////////
+// The Loki Library
+// Copyright (c) 2001 by Andrei Alexandrescu
+// This code accompanies the book:
+// Alexandrescu, Andrei. "Modern C++ Design: Generic Programming and Design
+// Patterns Applied". Copyright (c) 2001. Addison-Wesley.
+// Permission to use, copy, modify, distribute and sell this software for any
+// purpose is hereby granted without fee, provided that the above copyright
+// notice appear in all copies and that both that copyright notice and this
+// permission notice appear in supporting documentation.
+// The author or Addison-Welsey Longman make no representations about the
+// suitability of this software for any purpose. It is provided "as is"
+// without express or implied warranty.
+// http://loki-lib.sourceforge.net/index.php?n=Main.License
+////////////////////////////////////////////////////////////////////////////////
+
+namespace Loki
+{
+ //==============================================================================
+ // class NullType
+ // Used as a placeholder for "no type here"
+ // Useful as an end marker in typelists
+ //==============================================================================
+
+ class NullType {};
+
+ //==============================================================================
+ // class template Typelist
+ // The building block of typelists of any length
+ // Use it through the LOKI_TYPELIST_NN macros
+ // Defines nested types:
+ // Head (first element, a non-typelist type by convention)
+ // Tail (second element, can be another typelist)
+ //==============================================================================
+
+ template <class T, class U>
+ struct Typelist
+ {
+ typedef T Head;
+ typedef U Tail;
+ };
+
+ //==============================================================================
+ // class template Int2Type
+ // Converts each integral constant into a unique type
+ // Invocation: Int2Type<v> where v is a compile-time constant integral
+ // Defines 'value', an enum that evaluates to v
+ //==============================================================================
+
+ template <int v>
+ struct Int2Type
+ {
+ enum { value = v };
+ };
+
+ namespace TL
+ {
+ //==============================================================================
+ // class template TypeAt
+ // Finds the type at a given index in a typelist
+ // Invocation (TList is a typelist and index is a compile-time integral
+ // constant):
+ // TypeAt<TList, index>::Result
+ // returns the type in position 'index' in TList
+ // If you pass an out-of-bounds index, the result is a compile-time error
+ //==============================================================================
+
+ template <class TList, unsigned int index> struct TypeAt;
+
+ template <class Head, class Tail>
+ struct TypeAt<Typelist<Head, Tail>, 0>
+ {
+ typedef Head Result;
+ };
+
+ template <class Head, class Tail, unsigned int i>
+ struct TypeAt<Typelist<Head, Tail>, i>
+ {
+ typedef typename TypeAt<Tail, i - 1>::Result Result;
+ };
+ }
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Runtime boolean template instance dispatcher
+// Cyril Crassin <cyril.crassin@icare3d.org>
+// NVIDIA, 2010
+////////////////////////////////////////////////////////////////////////////////
+
+namespace NCVRuntimeTemplateBool
+{
+ //This struct is used to transform a list of parameters into template arguments
+ //The idea is to build a typelist containing the arguments
+ //and to pass this typelist to a user defined functor
+ template<typename TList, int NumArguments, class Func>
+ struct KernelCaller
+ {
+ //Convenience function used by the user
+ //Takes a variable argument list, transforms it into a list
+ static void call(Func *functor, ...)
+ {
+ //Vector used to collect arguments
+ std::vector<int> templateParamList;
+
+ //Variable argument list manipulation
+ va_list listPointer;
+ va_start(listPointer, functor);
+ //Collect parameters into the list
+ for(int i=0; i<NumArguments; i++)
+ {
+ int val = va_arg(listPointer, int);
+ templateParamList.push_back(val);
+ }
+ va_end(listPointer);
+
+ //Call the actual typelist building function
+ call(*functor, templateParamList);
+ }
+
+ //Actual function called recursively to build a typelist based
+ //on a list of values
+ static void call( Func &functor, std::vector<int> &templateParamList)
+ {
+ //Get current parameter value in the list
+ NcvBool val = templateParamList[templateParamList.size() - 1];
+ templateParamList.pop_back();
+
+ //Select the compile time value to add into the typelist
+ //depending on the runtime variable and make recursive call.
+ //Both versions are really instantiated
+ if (val)
+ {
+ KernelCaller<
+ Loki::Typelist<typename Loki::Int2Type<1>, TList >,
+ NumArguments-1, Func >
+ ::call(functor, templateParamList);
+ }
+ else
+ {
+ KernelCaller<
+ Loki::Typelist<typename Loki::Int2Type<0>, TList >,
+ NumArguments-1, Func >
+ ::call(functor, templateParamList);
+ }
+ }
+ };
+
+ //Specialization for 0 value left in the list
+ //-> actual kernel functor call
+ template<class TList, class Func>
+ struct KernelCaller<TList, 0, Func>
+ {
+ static void call(Func &functor)
+ {
+ //Call to the functor's kernel call method
+ functor.call(TList()); //TList instantiated to get the method template parameter resolved
+ }
+
+ static void call(Func &functor, std::vector<int> &templateParams)
+ {
+ functor.call(TList());
+ }
+ };
+}
+
+#endif //_ncvruntimetemplates_hpp_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (C) 2009-2010, NVIDIA Corporation, all rights reserved.
+ * Third party copyrights are property of their respective owners.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * Ported to PCL by Koen Buys : Attention Work in progress!
+ */
+
+
+#include <vector>
+#include <cuda_runtime.h>
+#include "NPP_staging.hpp"
+
+
+texture<Ncv8u, 1, cudaReadModeElementType> tex8u;
+texture<Ncv32u, 1, cudaReadModeElementType> tex32u;
+texture<uint2, 1, cudaReadModeElementType> tex64u;
+
+
+//==============================================================================
+//
+// CUDA streams handling
+//
+//==============================================================================
+
+
+static cudaStream_t nppStream = 0;
+
+
+cudaStream_t nppStGetActiveCUDAstream(void)
+{
+ return nppStream;
+}
+
+
+
+cudaStream_t nppStSetActiveCUDAstream(cudaStream_t cudaStream)
+{
+ cudaStream_t tmp = nppStream;
+ nppStream = cudaStream;
+ return tmp;
+}
+
+
+//==============================================================================
+//
+// BlockScan.cuh
+//
+//==============================================================================
+
+
+NCV_CT_ASSERT(K_WARP_SIZE == 32); //this is required for the manual unroll of the loop in warpScanInclusive
+
+
+//Almost the same as naive scan1Inclusive, but doesn't need __syncthreads()
+//assuming size <= WARP_SIZE and size is power of 2
+template <class T>
+inline __device__ T warpScanInclusive(T idata, volatile T *s_Data)
+{
+ Ncv32u pos = 2 * threadIdx.x - (threadIdx.x & (K_WARP_SIZE - 1));
+ s_Data[pos] = 0;
+ pos += K_WARP_SIZE;
+ s_Data[pos] = idata;
+
+ //for(Ncv32u offset = 1; offset < K_WARP_SIZE; offset <<= 1)
+ //{
+ // s_Data[pos] += s_Data[pos - offset];
+ //}
+
+ s_Data[pos] += s_Data[pos - 1];
+ s_Data[pos] += s_Data[pos - 2];
+ s_Data[pos] += s_Data[pos - 4];
+ s_Data[pos] += s_Data[pos - 8];
+ s_Data[pos] += s_Data[pos - 16];
+
+ return s_Data[pos];
+}
+
+
+template <class T>
+inline __device__ T warpScanExclusive(T idata, volatile T *s_Data)
+{
+ return warpScanInclusive(idata, s_Data) - idata;
+}
+
+
+template <class T, Ncv32u tiNumScanThreads>
+inline __device__ T blockScanInclusive(T idata, volatile T *s_Data)
+{
+ if (tiNumScanThreads > K_WARP_SIZE)
+ {
+ //Bottom-level inclusive warp scan
+ T warpResult = warpScanInclusive(idata, s_Data);
+
+ //Save top elements of each warp for exclusive warp scan
+ //sync to wait for warp scans to complete (because s_Data is being overwritten)
+ __syncthreads();
+ if( (threadIdx.x & (K_WARP_SIZE - 1)) == (K_WARP_SIZE - 1) )
+ {
+ s_Data[threadIdx.x >> K_LOG2_WARP_SIZE] = warpResult;
+ }
+
+ //wait for warp scans to complete
+ __syncthreads();
+
+ if( threadIdx.x < (tiNumScanThreads / K_WARP_SIZE) )
+ {
+ //grab top warp elements
+ T val = s_Data[threadIdx.x];
+ //calculate exclusive scan and write back to shared memory
+ s_Data[threadIdx.x] = warpScanExclusive(val, s_Data);
+ }
+
+ //return updated warp scans with exclusive scan results
+ __syncthreads();
+ return warpResult + s_Data[threadIdx.x >> K_LOG2_WARP_SIZE];
+ }
+ else
+ {
+ return warpScanInclusive(idata, s_Data);
+ }
+}
+
+
+//==============================================================================
+//
+// IntegralImage.cu
+//
+//==============================================================================
+
+
+const Ncv32u NUM_SCAN_THREADS = 256;
+const Ncv32u LOG2_NUM_SCAN_THREADS = 8;
+
+
+template<class T_in, class T_out>
+struct _scanElemOp
+{
+ template<bool tbDoSqr>
+ static inline __host__ __device__ T_out scanElemOp(T_in elem)
+ {
+ return scanElemOp( elem, Int2Type<(int)tbDoSqr>() );
+ }
+
+private:
+
+ template <int v> struct Int2Type { enum { value = v }; };
+
+ static inline __host__ __device__ T_out scanElemOp(T_in elem, Int2Type<0>)
+ {
+ return (T_out)elem;
+ }
+
+ static inline __host__ __device__ T_out scanElemOp(T_in elem, Int2Type<1>)
+ {
+ return (T_out)(elem*elem);
+ }
+};
+
+
+template<class T>
+inline __device__ T readElem(T *d_src, Ncv32u texOffs, Ncv32u srcStride, Ncv32u curElemOffs);
+
+
+template<>
+inline __device__ Ncv8u readElem<Ncv8u>(Ncv8u *d_src, Ncv32u texOffs, Ncv32u srcStride, Ncv32u curElemOffs)
+{
+ return tex1Dfetch(tex8u, texOffs + srcStride * blockIdx.x + curElemOffs);
+}
+
+
+template<>
+inline __device__ Ncv32u readElem<Ncv32u>(Ncv32u *d_src, Ncv32u texOffs, Ncv32u srcStride, Ncv32u curElemOffs)
+{
+ return d_src[curElemOffs];
+}
+
+
+template<>
+inline __device__ Ncv32f readElem<Ncv32f>(Ncv32f *d_src, Ncv32u texOffs, Ncv32u srcStride, Ncv32u curElemOffs)
+{
+ return d_src[curElemOffs];
+}
+
+
+/**
+* \brief Segmented scan kernel
+*
+* Calculates per-row prefix scans of the input image.
+* Out-of-bounds safe: reads 'size' elements, writes 'size+1' elements
+*
+* \tparam T_in Type of input image elements
+* \tparam T_out Type of output image elements
+* \tparam T_op Defines an operation to be performed on the input image pixels
+*
+* \param d_src [IN] Source image pointer
+* \param srcWidth [IN] Source image width
+* \param srcStride [IN] Source image stride
+* \param d_II [OUT] Output image pointer
+* \param IIstride [IN] Output image stride
+*
+* \return None
+*/
+template <class T_in, class T_out, bool tbDoSqr>
+__global__ void scanRows(T_in *d_src, Ncv32u texOffs, Ncv32u srcWidth, Ncv32u srcStride,
+ T_out *d_II, Ncv32u IIstride)
+{
+ //advance pointers to the current line
+ if (sizeof(T_in) != 1)
+ {
+ d_src += srcStride * blockIdx.x;
+ }
+ //for initial image 8bit source we use texref tex8u
+ d_II += IIstride * blockIdx.x;
+
+ Ncv32u numBuckets = (srcWidth + NUM_SCAN_THREADS - 1) >> LOG2_NUM_SCAN_THREADS;
+ Ncv32u offsetX = 0;
+
+ __shared__ T_out shmem[NUM_SCAN_THREADS * 2];
+ __shared__ T_out carryElem;
+ carryElem = 0;
+ __syncthreads();
+
+ while (numBuckets--)
+ {
+ Ncv32u curElemOffs = offsetX + threadIdx.x;
+ T_out curScanElem;
+
+ T_in curElem;
+ T_out curElemMod;
+
+ if (curElemOffs < srcWidth)
+ {
+ //load elements
+ curElem = readElem<T_in>(d_src, texOffs, srcStride, curElemOffs);
+ }
+ curElemMod = _scanElemOp<T_in, T_out>::scanElemOp<tbDoSqr>(curElem);
+
+ //inclusive scan
+ curScanElem = blockScanInclusive<T_out, NUM_SCAN_THREADS>(curElemMod, shmem);
+
+ if (curElemOffs <= srcWidth)
+ {
+ //make scan exclusive and write the bucket to the output buffer
+ d_II[curElemOffs] = carryElem + curScanElem - curElemMod;
+ offsetX += NUM_SCAN_THREADS;
+ }
+
+ //remember last element for subsequent buckets adjustment
+ __syncthreads();
+ if (threadIdx.x == NUM_SCAN_THREADS-1)
+ {
+ carryElem += curScanElem;
+ }
+ __syncthreads();
+ }
+
+ if (offsetX == srcWidth && !threadIdx.x)
+ {
+ d_II[offsetX] = carryElem;
+ }
+}
+
+
+template <bool tbDoSqr, class T_in, class T_out>
+NCVStatus scanRowsWrapperDevice(T_in *d_src, Ncv32u srcStride,
+ T_out *d_dst, Ncv32u dstStride, NcvSize32u roi)
+{
+ cudaChannelFormatDesc cfdTex;
+ size_t alignmentOffset = 0;
+ if (sizeof(T_in) == 1)
+ {
+ cfdTex = cudaCreateChannelDesc<Ncv8u>();
+ ncvAssertCUDAReturn(cudaBindTexture(&alignmentOffset, tex8u, d_src, cfdTex, roi.height * srcStride), NPPST_TEXTURE_BIND_ERROR);
+ if (alignmentOffset > 0)
+ {
+ ncvAssertCUDAReturn(cudaUnbindTexture(tex8u), NCV_CUDA_ERROR);
+ ncvAssertCUDAReturn(cudaBindTexture(&alignmentOffset, tex8u, d_src, cfdTex, alignmentOffset + roi.height * srcStride), NPPST_TEXTURE_BIND_ERROR);
+ }
+ }
+ scanRows
+ <T_in, T_out, tbDoSqr>
+ <<<roi.height, NUM_SCAN_THREADS, 0, nppStGetActiveCUDAstream()>>>
+ (d_src, (Ncv32u)alignmentOffset, roi.width, srcStride, d_dst, dstStride);
+
+ ncvAssertCUDALastErrorReturn(NPPST_CUDA_KERNEL_EXECUTION_ERROR);
+
+ return NPPST_SUCCESS;
+}
+
+
+static Ncv32u getPaddedDimension(Ncv32u dim, Ncv32u elemTypeSize, Ncv32u allocatorAlignment)
+{
+ Ncv32u alignMask = allocatorAlignment-1;
+ Ncv32u inverseAlignMask = ~alignMask;
+ Ncv32u dimBytes = dim * elemTypeSize;
+ Ncv32u pitch = (dimBytes + alignMask) & inverseAlignMask;
+ Ncv32u PaddedDim = pitch / elemTypeSize;
+ return PaddedDim;
+}
+
+
+template <class T_in, class T_out>
+NCVStatus ncvIntegralImage_device(T_in *d_src, Ncv32u srcStep,
+ T_out *d_dst, Ncv32u dstStep, NcvSize32u roi,
+ INCVMemAllocator &gpuAllocator)
+{
+ ncvAssertReturn(sizeof(T_out) == sizeof(Ncv32u), NPPST_MEM_INTERNAL_ERROR);
+ ncvAssertReturn(gpuAllocator.memType() == NCVMemoryTypeDevice ||
+ gpuAllocator.memType() == NCVMemoryTypeNone, NPPST_MEM_RESIDENCE_ERROR);
+ ncvAssertReturn(gpuAllocator.isInitialized(), NPPST_MEM_INTERNAL_ERROR);
+ ncvAssertReturn((d_src != NULL && d_dst != NULL) || gpuAllocator.isCounting(), NPPST_NULL_POINTER_ERROR);
+ ncvAssertReturn(roi.width > 0 && roi.height > 0, NPPST_INVALID_ROI);
+ ncvAssertReturn(srcStep >= roi.width * sizeof(T_in) &&
+ dstStep >= (roi.width + 1) * sizeof(T_out) &&
+ srcStep % sizeof(T_in) == 0 &&
+ dstStep % sizeof(T_out) == 0, NPPST_INVALID_STEP);
+ srcStep /= sizeof(T_in);
+ dstStep /= sizeof(T_out);
+
+ Ncv32u WidthII = roi.width + 1;
+ Ncv32u HeightII = roi.height + 1;
+ Ncv32u PaddedWidthII32 = getPaddedDimension(WidthII, sizeof(Ncv32u), gpuAllocator.alignment());
+ Ncv32u PaddedHeightII32 = getPaddedDimension(HeightII, sizeof(Ncv32u), gpuAllocator.alignment());
+
+ NCVMatrixAlloc<T_out> Tmp32_1(gpuAllocator, PaddedWidthII32, PaddedHeightII32);
+ ncvAssertReturn(gpuAllocator.isCounting() || Tmp32_1.isMemAllocated(), NPPST_MEM_INTERNAL_ERROR);
+ NCVMatrixAlloc<T_out> Tmp32_2(gpuAllocator, PaddedHeightII32, PaddedWidthII32);
+ ncvAssertReturn(gpuAllocator.isCounting() || Tmp32_2.isMemAllocated(), NPPST_MEM_INTERNAL_ERROR);
+ ncvAssertReturn(Tmp32_1.pitch() * Tmp32_1.height() == Tmp32_2.pitch() * Tmp32_2.height(), NPPST_MEM_INTERNAL_ERROR);
+
+ NCVStatus ncvStat;
+ NCV_SET_SKIP_COND(gpuAllocator.isCounting());
+
+ NCV_SKIP_COND_BEGIN
+
+ ncvStat = scanRowsWrapperDevice
+ <false>
+ (d_src, srcStep, Tmp32_1.ptr(), PaddedWidthII32, roi);
+ ncvAssertReturnNcvStat(ncvStat);
+
+ ncvStat = nppiStTranspose_32u_C1R((Ncv32u *)Tmp32_1.ptr(), PaddedWidthII32*sizeof(Ncv32u),
+ (Ncv32u *)Tmp32_2.ptr(), PaddedHeightII32*sizeof(Ncv32u), NcvSize32u(WidthII, roi.height));
+ ncvAssertReturnNcvStat(ncvStat);
+
+ ncvStat = scanRowsWrapperDevice
+ <false>
+ (Tmp32_2.ptr(), PaddedHeightII32, Tmp32_1.ptr(), PaddedHeightII32, NcvSize32u(roi.height, WidthII));
+ ncvAssertReturnNcvStat(ncvStat);
+
+ ncvStat = nppiStTranspose_32u_C1R((Ncv32u *)Tmp32_1.ptr(), PaddedHeightII32*sizeof(Ncv32u),
+ (Ncv32u *)d_dst, dstStep*sizeof(Ncv32u), NcvSize32u(HeightII, WidthII));
+ ncvAssertReturnNcvStat(ncvStat);
+
+ NCV_SKIP_COND_END
+
+ return NPPST_SUCCESS;
+}
+
+
+NCVStatus ncvSquaredIntegralImage_device(Ncv8u *d_src, Ncv32u srcStep,
+ Ncv64u *d_dst, Ncv32u dstStep, NcvSize32u roi,
+ INCVMemAllocator &gpuAllocator)
+{
+ ncvAssertReturn(gpuAllocator.isInitialized(), NPPST_MEM_INTERNAL_ERROR);
+ ncvAssertReturn(gpuAllocator.memType() == NCVMemoryTypeDevice ||
+ gpuAllocator.memType() == NCVMemoryTypeNone, NPPST_MEM_RESIDENCE_ERROR);
+ ncvAssertReturn((d_src != NULL && d_dst != NULL) || gpuAllocator.isCounting(), NPPST_NULL_POINTER_ERROR);
+ ncvAssertReturn(roi.width > 0 && roi.height > 0, NPPST_INVALID_ROI);
+ ncvAssertReturn(srcStep >= roi.width &&
+ dstStep >= (roi.width + 1) * sizeof(Ncv64u) &&
+ dstStep % sizeof(Ncv64u) == 0, NPPST_INVALID_STEP);
+ dstStep /= sizeof(Ncv64u);
+
+ Ncv32u WidthII = roi.width + 1;
+ Ncv32u HeightII = roi.height + 1;
+ Ncv32u PaddedWidthII32 = getPaddedDimension(WidthII, sizeof(Ncv32u), gpuAllocator.alignment());
+ Ncv32u PaddedHeightII32 = getPaddedDimension(HeightII, sizeof(Ncv32u), gpuAllocator.alignment());
+ Ncv32u PaddedWidthII64 = getPaddedDimension(WidthII, sizeof(Ncv64u), gpuAllocator.alignment());
+ Ncv32u PaddedHeightII64 = getPaddedDimension(HeightII, sizeof(Ncv64u), gpuAllocator.alignment());
+ Ncv32u PaddedWidthMax = PaddedWidthII32 > PaddedWidthII64 ? PaddedWidthII32 : PaddedWidthII64;
+ Ncv32u PaddedHeightMax = PaddedHeightII32 > PaddedHeightII64 ? PaddedHeightII32 : PaddedHeightII64;
+
+ NCVMatrixAlloc<Ncv32u> Tmp32_1(gpuAllocator, PaddedWidthII32, PaddedHeightII32);
+ ncvAssertReturn(Tmp32_1.isMemAllocated(), NPPST_MEM_INTERNAL_ERROR);
+ NCVMatrixAlloc<Ncv64u> Tmp64(gpuAllocator, PaddedWidthMax, PaddedHeightMax);
+ ncvAssertReturn(Tmp64.isMemAllocated(), NPPST_MEM_INTERNAL_ERROR);
+
+ NCVMatrixReuse<Ncv32u> Tmp32_2(Tmp64.getSegment(), gpuAllocator.alignment(), PaddedWidthII32, PaddedHeightII32);
+ ncvAssertReturn(Tmp32_2.isMemReused(), NPPST_MEM_INTERNAL_ERROR);
+ NCVMatrixReuse<Ncv64u> Tmp64_2(Tmp64.getSegment(), gpuAllocator.alignment(), PaddedWidthII64, PaddedHeightII64);
+ ncvAssertReturn(Tmp64_2.isMemReused(), NPPST_MEM_INTERNAL_ERROR);
+
+ NCVStatus ncvStat;
+ NCV_SET_SKIP_COND(gpuAllocator.isCounting());
+
+ NCV_SKIP_COND_BEGIN
+
+ ncvStat = scanRowsWrapperDevice
+ <true, Ncv8u, Ncv32u>
+ (d_src, srcStep, Tmp32_2.ptr(), PaddedWidthII32, roi);
+ ncvAssertReturnNcvStat(ncvStat);
+
+ ncvStat = nppiStTranspose_32u_C1R(Tmp32_2.ptr(), PaddedWidthII32*sizeof(Ncv32u),
+ Tmp32_1.ptr(), PaddedHeightII32*sizeof(Ncv32u), NcvSize32u(WidthII, roi.height));
+ ncvAssertReturnNcvStat(ncvStat);
+
+ ncvStat = scanRowsWrapperDevice
+ <false, Ncv32u, Ncv64u>
+ (Tmp32_1.ptr(), PaddedHeightII32, Tmp64_2.ptr(), PaddedHeightII64, NcvSize32u(roi.height, WidthII));
+ ncvAssertReturnNcvStat(ncvStat);
+
+ ncvStat = nppiStTranspose_64u_C1R(Tmp64_2.ptr(), PaddedHeightII64*sizeof(Ncv64u),
+ d_dst, dstStep*sizeof(Ncv64u), NcvSize32u(HeightII, WidthII));
+ ncvAssertReturnNcvStat(ncvStat);
+
+ NCV_SKIP_COND_END
+
+ return NPPST_SUCCESS;
+}
+
+
+NCVStatus nppiStIntegralGetSize_8u32u(NcvSize32u roiSize, Ncv32u *pBufsize, cudaDeviceProp &devProp)
+{
+ ncvAssertReturn(pBufsize != NULL, NPPST_NULL_POINTER_ERROR);
+ ncvAssertReturn(roiSize.width > 0 && roiSize.height > 0, NPPST_INVALID_ROI);
+
+ NCVMemStackAllocator gpuCounter(static_cast<Ncv32u>(devProp.textureAlignment));
+ ncvAssertReturn(gpuCounter.isInitialized(), NPPST_MEM_INTERNAL_ERROR);
+
+ NCVStatus ncvStat = ncvIntegralImage_device((Ncv8u*)NULL, roiSize.width,
+ (Ncv32u*)NULL, (roiSize.width+1) * sizeof(Ncv32u),
+ roiSize, gpuCounter);
+ ncvAssertReturnNcvStat(ncvStat);
+
+ *pBufsize = (Ncv32u)gpuCounter.maxSize();
+ return NPPST_SUCCESS;
+}
+
+
+NCVStatus nppiStIntegralGetSize_32f32f(NcvSize32u roiSize, Ncv32u *pBufsize, cudaDeviceProp &devProp)
+{
+ ncvAssertReturn(pBufsize != NULL, NPPST_NULL_POINTER_ERROR);
+ ncvAssertReturn(roiSize.width > 0 && roiSize.height > 0, NPPST_INVALID_ROI);
+
+ NCVMemStackAllocator gpuCounter(static_cast<Ncv32u>(devProp.textureAlignment));
+ ncvAssertReturn(gpuCounter.isInitialized(), NPPST_MEM_INTERNAL_ERROR);
+
+ NCVStatus ncvStat = ncvIntegralImage_device((Ncv32f*)NULL, roiSize.width * sizeof(Ncv32f),
+ (Ncv32f*)NULL, (roiSize.width+1) * sizeof(Ncv32f),
+ roiSize, gpuCounter);
+ ncvAssertReturnNcvStat(ncvStat);
+
+ *pBufsize = (Ncv32u)gpuCounter.maxSize();
+ return NPPST_SUCCESS;
+}
+
+
+NCVStatus nppiStSqrIntegralGetSize_8u64u(NcvSize32u roiSize, Ncv32u *pBufsize, cudaDeviceProp &devProp)
+{
+ ncvAssertReturn(pBufsize != NULL, NPPST_NULL_POINTER_ERROR);
+ ncvAssertReturn(roiSize.width > 0 && roiSize.height > 0, NPPST_INVALID_ROI);
+
+ NCVMemStackAllocator gpuCounter(static_cast<Ncv32u>(devProp.textureAlignment));
+ ncvAssertReturn(gpuCounter.isInitialized(), NPPST_MEM_INTERNAL_ERROR);
+
+ NCVStatus ncvStat = ncvSquaredIntegralImage_device(NULL, roiSize.width,
+ NULL, (roiSize.width+1) * sizeof(Ncv64u),
+ roiSize, gpuCounter);
+ ncvAssertReturnNcvStat(ncvStat);
+
+ *pBufsize = (Ncv32u)gpuCounter.maxSize();
+ return NPPST_SUCCESS;
+}
+
+
+NCVStatus nppiStIntegral_8u32u_C1R(Ncv8u *d_src, Ncv32u srcStep,
+ Ncv32u *d_dst, Ncv32u dstStep,
+ NcvSize32u roiSize, Ncv8u *pBuffer,
+ Ncv32u bufSize, cudaDeviceProp &devProp)
+{
+ NCVMemStackAllocator gpuAllocator(NCVMemoryTypeDevice, bufSize, static_cast<Ncv32u>(devProp.textureAlignment), pBuffer);
+ ncvAssertReturn(gpuAllocator.isInitialized(), NPPST_MEM_INTERNAL_ERROR);
+
+ NCVStatus ncvStat = ncvIntegralImage_device(d_src, srcStep, d_dst, dstStep, roiSize, gpuAllocator);
+ ncvAssertReturnNcvStat(ncvStat);
+
+ return NPPST_SUCCESS;
+}
+
+
+NCVStatus nppiStIntegral_32f32f_C1R(Ncv32f *d_src, Ncv32u srcStep,
+ Ncv32f *d_dst, Ncv32u dstStep,
+ NcvSize32u roiSize, Ncv8u *pBuffer,
+ Ncv32u bufSize, cudaDeviceProp &devProp)
+{
+ NCVMemStackAllocator gpuAllocator(NCVMemoryTypeDevice, bufSize, static_cast<Ncv32u>(devProp.textureAlignment), pBuffer);
+ ncvAssertReturn(gpuAllocator.isInitialized(), NPPST_MEM_INTERNAL_ERROR);
+
+ NCVStatus ncvStat = ncvIntegralImage_device(d_src, srcStep, d_dst, dstStep, roiSize, gpuAllocator);
+ ncvAssertReturnNcvStat(ncvStat);
+
+ return NPPST_SUCCESS;
+}
+
+
+NCVStatus nppiStSqrIntegral_8u64u_C1R(Ncv8u *d_src, Ncv32u srcStep,
+ Ncv64u *d_dst, Ncv32u dstStep,
+ NcvSize32u roiSize, Ncv8u *pBuffer,
+ Ncv32u bufSize, cudaDeviceProp &devProp)
+{
+ NCVMemStackAllocator gpuAllocator(NCVMemoryTypeDevice, bufSize, static_cast<Ncv32u>(devProp.textureAlignment), pBuffer);
+ ncvAssertReturn(gpuAllocator.isInitialized(), NPPST_MEM_INTERNAL_ERROR);
+
+ NCVStatus ncvStat = ncvSquaredIntegralImage_device(d_src, srcStep, d_dst, dstStep, roiSize, gpuAllocator);
+ ncvAssertReturnNcvStat(ncvStat);
+
+ return NPPST_SUCCESS;
+}
+
+
+NCVStatus nppiStIntegral_8u32u_C1R_host(Ncv8u *h_src, Ncv32u srcStep,
+ Ncv32u *h_dst, Ncv32u dstStep,
+ NcvSize32u roiSize)
+{
+ ncvAssertReturn(h_src != NULL && h_dst != NULL, NPPST_NULL_POINTER_ERROR);
+ ncvAssertReturn(roiSize.width > 0 && roiSize.height > 0, NPPST_INVALID_ROI);
+ ncvAssertReturn(srcStep >= roiSize.width &&
+ dstStep >= (roiSize.width + 1) * sizeof(Ncv32u) &&
+ dstStep % sizeof(Ncv32u) == 0, NPPST_INVALID_STEP);
+ dstStep /= sizeof(Ncv32u);
+
+ Ncv32u WidthII = roiSize.width + 1;
+ Ncv32u HeightII = roiSize.height + 1;
+
+ memset(h_dst, 0, WidthII * sizeof(Ncv32u));
+ for (Ncv32u i=1; i<HeightII; i++)
+ {
+ h_dst[i * dstStep] = 0;
+ for (Ncv32u j=1; j<WidthII; j++)
+ {
+ Ncv32u top = h_dst[(i-1) * dstStep + j];
+ Ncv32u left = h_dst[i * dstStep + (j - 1)];
+ Ncv32u topleft = h_dst[(i - 1) * dstStep + (j - 1)];
+ Ncv32u elem = h_src[(i - 1) * srcStep + (j - 1)];
+ h_dst[i * dstStep + j] = elem + left - topleft + top;
+ }
+ }
+
+ return NPPST_SUCCESS;
+}
+
+
+NCVStatus nppiStIntegral_32f32f_C1R_host(Ncv32f *h_src, Ncv32u srcStep,
+ Ncv32f *h_dst, Ncv32u dstStep,
+ NcvSize32u roiSize)
+{
+ ncvAssertReturn(h_src != NULL && h_dst != NULL, NPPST_NULL_POINTER_ERROR);
+ ncvAssertReturn(roiSize.width > 0 && roiSize.height > 0, NPPST_INVALID_ROI);
+ ncvAssertReturn(srcStep >= roiSize.width * sizeof(Ncv32f) &&
+ dstStep >= (roiSize.width + 1) * sizeof(Ncv32f) &&
+ srcStep % sizeof(Ncv32f) == 0 &&
+ dstStep % sizeof(Ncv32f) == 0, NPPST_INVALID_STEP);
+ srcStep /= sizeof(Ncv32f);
+ dstStep /= sizeof(Ncv32f);
+
+ Ncv32u WidthII = roiSize.width + 1;
+ Ncv32u HeightII = roiSize.height + 1;
+
+ memset(h_dst, 0, WidthII * sizeof(Ncv32u));
+ for (Ncv32u i=1; i<HeightII; i++)
+ {
+ h_dst[i * dstStep] = 0.0f;
+ for (Ncv32u j=1; j<WidthII; j++)
+ {
+ Ncv32f top = h_dst[(i-1) * dstStep + j];
+ Ncv32f left = h_dst[i * dstStep + (j - 1)];
+ Ncv32f topleft = h_dst[(i - 1) * dstStep + (j - 1)];
+ Ncv32f elem = h_src[(i - 1) * srcStep + (j - 1)];
+ h_dst[i * dstStep + j] = elem + left - topleft + top;
+ }
+ }
+
+ return NPPST_SUCCESS;
+}
+
+
+NCVStatus nppiStSqrIntegral_8u64u_C1R_host(Ncv8u *h_src, Ncv32u srcStep,
+ Ncv64u *h_dst, Ncv32u dstStep,
+ NcvSize32u roiSize)
+{
+ ncvAssertReturn(h_src != NULL && h_dst != NULL, NPPST_NULL_POINTER_ERROR);
+ ncvAssertReturn(roiSize.width > 0 && roiSize.height > 0, NPPST_INVALID_ROI);
+ ncvAssertReturn(srcStep >= roiSize.width &&
+ dstStep >= (roiSize.width + 1) * sizeof(Ncv64u) &&
+ dstStep % sizeof(Ncv64u) == 0, NPPST_INVALID_STEP);
+ dstStep /= sizeof(Ncv64u);
+
+ Ncv32u WidthII = roiSize.width + 1;
+ Ncv32u HeightII = roiSize.height + 1;
+
+ memset(h_dst, 0, WidthII * sizeof(Ncv64u));
+ for (Ncv32u i=1; i<HeightII; i++)
+ {
+ h_dst[i * dstStep] = 0;
+ for (Ncv32u j=1; j<WidthII; j++)
+ {
+ Ncv64u top = h_dst[(i-1) * dstStep + j];
+ Ncv64u left = h_dst[i * dstStep + (j - 1)];
+ Ncv64u topleft = h_dst[(i - 1) * dstStep + (j - 1)];
+ Ncv64u elem = h_src[(i - 1) * srcStep + (j - 1)];
+ h_dst[i * dstStep + j] = elem*elem + left - topleft + top;
+ }
+ }
+
+ return NPPST_SUCCESS;
+}
+
+
+//==============================================================================
+//
+// Decimate.cu
+//
+//==============================================================================
+
+
+const Ncv32u NUM_DOWNSAMPLE_NEAREST_THREADS_X = 32;
+const Ncv32u NUM_DOWNSAMPLE_NEAREST_THREADS_Y = 8;
+
+
+template<class T, NcvBool tbCacheTexture>
+__device__ T getElem_Decimate(Ncv32u x, T *d_src);
+
+
+template<>
+__device__ Ncv32u getElem_Decimate<Ncv32u, true>(Ncv32u x, Ncv32u *d_src)
+{
+ return tex1Dfetch(tex32u, x);
+}
+
+
+template<>
+__device__ Ncv32u getElem_Decimate<Ncv32u, false>(Ncv32u x, Ncv32u *d_src)
+{
+ return d_src[x];
+}
+
+
+template<>
+__device__ Ncv64u getElem_Decimate<Ncv64u, true>(Ncv32u x, Ncv64u *d_src)
+{
+ uint2 tmp = tex1Dfetch(tex64u, x);
+ Ncv64u res = (Ncv64u)tmp.y;
+ res <<= 32;
+ res |= tmp.x;
+ return res;
+}
+
+
+template<>
+__device__ Ncv64u getElem_Decimate<Ncv64u, false>(Ncv32u x, Ncv64u *d_src)
+{
+ return d_src[x];
+}
+
+
+template <class T, NcvBool tbCacheTexture>
+__global__ void decimate_C1R(T *d_src, Ncv32u srcStep, T *d_dst, Ncv32u dstStep,
+ NcvSize32u dstRoi, Ncv32u scale)
+{
+ int curX = blockIdx.x * blockDim.x + threadIdx.x;
+ int curY = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if (curX >= dstRoi.width || curY >= dstRoi.height)
+ {
+ return;
+ }
+
+ d_dst[curY * dstStep + curX] = getElem_Decimate<T, tbCacheTexture>((curY * srcStep + curX) * scale, d_src);
+}
+
+
+template <class T>
+static NCVStatus decimateWrapperDevice(T *d_src, Ncv32u srcStep,
+ T *d_dst, Ncv32u dstStep,
+ NcvSize32u srcRoi, Ncv32u scale,
+ NcvBool readThruTexture)
+{
+ ncvAssertReturn(d_src != NULL && d_dst != NULL, NPPST_NULL_POINTER_ERROR);
+ ncvAssertReturn(srcRoi.width > 0 && srcRoi.height > 0, NPPST_INVALID_ROI);
+ ncvAssertReturn(scale != 0, NPPST_INVALID_SCALE);
+ ncvAssertReturn(srcStep >= (Ncv32u)(srcRoi.width) * sizeof(T) &&
+ dstStep >= (Ncv32u)(srcRoi.width * sizeof(T) / scale), NPPST_INVALID_STEP);
+ srcStep /= sizeof(T);
+ dstStep /= sizeof(T);
+
+ NcvSize32u dstRoi;
+ dstRoi.width = srcRoi.width / scale;
+ dstRoi.height = srcRoi.height / scale;
+
+ dim3 grid((dstRoi.width + NUM_DOWNSAMPLE_NEAREST_THREADS_X - 1) / NUM_DOWNSAMPLE_NEAREST_THREADS_X,
+ (dstRoi.height + NUM_DOWNSAMPLE_NEAREST_THREADS_Y - 1) / NUM_DOWNSAMPLE_NEAREST_THREADS_Y);
+ dim3 block(NUM_DOWNSAMPLE_NEAREST_THREADS_X, NUM_DOWNSAMPLE_NEAREST_THREADS_Y);
+
+ if (!readThruTexture)
+ {
+ decimate_C1R
+ <T, false>
+ <<<grid, block, 0, nppStGetActiveCUDAstream()>>>
+ (d_src, srcStep, d_dst, dstStep, dstRoi, scale);
+ }
+ else
+ {
+ cudaChannelFormatDesc cfdTexSrc;
+
+ if (sizeof(T) == sizeof(Ncv32u))
+ {
+ cfdTexSrc = cudaCreateChannelDesc<Ncv32u>();
+
+ size_t alignmentOffset;
+ ncvAssertCUDAReturn(cudaBindTexture(&alignmentOffset, tex32u, d_src, cfdTexSrc, srcRoi.height * srcStep * sizeof(T)), NPPST_TEXTURE_BIND_ERROR);
+ ncvAssertReturn(alignmentOffset==0, NPPST_TEXTURE_BIND_ERROR);
+ }
+ else
+ {
+ cfdTexSrc = cudaCreateChannelDesc<uint2>();
+
+ size_t alignmentOffset;
+ ncvAssertCUDAReturn(cudaBindTexture(&alignmentOffset, tex64u, d_src, cfdTexSrc, srcRoi.height * srcStep * sizeof(T)), NPPST_TEXTURE_BIND_ERROR);
+ ncvAssertReturn(alignmentOffset==0, NPPST_TEXTURE_BIND_ERROR);
+ }
+
+ decimate_C1R
+ <T, true>
+ <<<grid, block, 0, nppStGetActiveCUDAstream()>>>
+ (d_src, srcStep, d_dst, dstStep, dstRoi, scale);
+ }
+
+ ncvAssertCUDALastErrorReturn(NPPST_CUDA_KERNEL_EXECUTION_ERROR);
+
+ return NPPST_SUCCESS;
+}
+
+
+template <class T>
+static NCVStatus decimateWrapperHost(T *h_src, Ncv32u srcStep,
+ T *h_dst, Ncv32u dstStep,
+ NcvSize32u srcRoi, Ncv32u scale)
+{
+ ncvAssertReturn(h_src != NULL && h_dst != NULL, NPPST_NULL_POINTER_ERROR);
+ ncvAssertReturn(srcRoi.width != 0 && srcRoi.height != 0, NPPST_INVALID_ROI);
+ ncvAssertReturn(scale != 0, NPPST_INVALID_SCALE);
+ ncvAssertReturn(srcStep >= (Ncv32u)(srcRoi.width) * sizeof(T) &&
+ dstStep >= (Ncv32u)(srcRoi.width * sizeof(T) / scale) &&
+ srcStep % sizeof(T) == 0 && dstStep % sizeof(T) == 0, NPPST_INVALID_STEP);
+ srcStep /= sizeof(T);
+ dstStep /= sizeof(T);
+
+ NcvSize32u dstRoi;
+ dstRoi.width = srcRoi.width / scale;
+ dstRoi.height = srcRoi.height / scale;
+
+ for (Ncv32u i=0; i<dstRoi.height; i++)
+ {
+ for (Ncv32u j=0; j<dstRoi.width; j++)
+ {
+ h_dst[i*dstStep+j] = h_src[i*scale*srcStep + j*scale];
+ }
+ }
+
+ return NPPST_SUCCESS;
+}
+
+
+#define implementNppDecimate(bit, typ) \
+ NCVStatus nppiStDecimate_##bit##typ##_C1R(Ncv##bit##typ *d_src, Ncv32u srcStep, \
+ Ncv##bit##typ *d_dst, Ncv32u dstStep, \
+ NcvSize32u srcRoi, Ncv32u scale, NcvBool readThruTexture) \
+ { \
+ return decimateWrapperDevice<Ncv##bit##u>((Ncv##bit##u *)d_src, srcStep, \
+ (Ncv##bit##u *)d_dst, dstStep, \
+ srcRoi, scale, readThruTexture); \
+ }
+
+
+#define implementNppDecimateHost(bit, typ) \
+ NCVStatus nppiStDecimate_##bit##typ##_C1R_host(Ncv##bit##typ *h_src, Ncv32u srcStep, \
+ Ncv##bit##typ *h_dst, Ncv32u dstStep, \
+ NcvSize32u srcRoi, Ncv32u scale) \
+ { \
+ return decimateWrapperHost<Ncv##bit##u>((Ncv##bit##u *)h_src, srcStep, \
+ (Ncv##bit##u *)h_dst, dstStep, \
+ srcRoi, scale); \
+ }
+
+
+implementNppDecimate(32, u)
+implementNppDecimate(32, s)
+implementNppDecimate(32, f)
+implementNppDecimate(64, u)
+implementNppDecimate(64, s)
+implementNppDecimate(64, f)
+implementNppDecimateHost(32, u)
+implementNppDecimateHost(32, s)
+implementNppDecimateHost(32, f)
+implementNppDecimateHost(64, u)
+implementNppDecimateHost(64, s)
+implementNppDecimateHost(64, f)
+
+
+//==============================================================================
+//
+// RectStdDev.cu
+//
+//==============================================================================
+
+
+const Ncv32u NUM_RECTSTDDEV_THREADS = 128;
+
+
+template <NcvBool tbCacheTexture>
+__device__ Ncv32u getElemSum(Ncv32u x, Ncv32u *d_sum)
+{
+ if (tbCacheTexture)
+ {
+ return tex1Dfetch(tex32u, x);
+ }
+ else
+ {
+ return d_sum[x];
+ }
+}
+
+
+template <NcvBool tbCacheTexture>
+__device__ Ncv64u getElemSqSum(Ncv32u x, Ncv64u *d_sqsum)
+{
+ if (tbCacheTexture)
+ {
+ uint2 tmp = tex1Dfetch(tex64u, x);
+ Ncv64u res = (Ncv64u)tmp.y;
+ res <<= 32;
+ res |= tmp.x;
+ return res;
+ }
+ else
+ {
+ return d_sqsum[x];
+ }
+}
+
+
+template <NcvBool tbCacheTexture>
+__global__ void rectStdDev_32f_C1R(Ncv32u *d_sum, Ncv32u sumStep,
+ Ncv64u *d_sqsum, Ncv32u sqsumStep,
+ Ncv32f *d_norm, Ncv32u normStep,
+ NcvSize32u roi, NcvRect32u rect, Ncv32f invRectArea)
+{
+ Ncv32u x_offs = blockIdx.x * NUM_RECTSTDDEV_THREADS + threadIdx.x;
+ if (x_offs >= roi.width)
+ {
+ return;
+ }
+
+ Ncv32u sum_offset = blockIdx.y * sumStep + x_offs;
+ Ncv32u sqsum_offset = blockIdx.y * sqsumStep + x_offs;
+
+ //OPT: try swapping order (could change cache hit/miss ratio)
+ Ncv32u sum_tl = getElemSum<tbCacheTexture>(sum_offset + rect.y * sumStep + rect.x, d_sum);
+ Ncv32u sum_bl = getElemSum<tbCacheTexture>(sum_offset + (rect.y + rect.height) * sumStep + rect.x, d_sum);
+ Ncv32u sum_tr = getElemSum<tbCacheTexture>(sum_offset + rect.y * sumStep + rect.x + rect.width, d_sum);
+ Ncv32u sum_br = getElemSum<tbCacheTexture>(sum_offset + (rect.y + rect.height) * sumStep + rect.x + rect.width, d_sum);
+ Ncv32u sum_val = sum_br + sum_tl - sum_tr - sum_bl;
+
+ Ncv64u sqsum_tl, sqsum_bl, sqsum_tr, sqsum_br;
+ sqsum_tl = getElemSqSum<tbCacheTexture>(sqsum_offset + rect.y * sqsumStep + rect.x, d_sqsum);
+ sqsum_bl = getElemSqSum<tbCacheTexture>(sqsum_offset + (rect.y + rect.height) * sqsumStep + rect.x, d_sqsum);
+ sqsum_tr = getElemSqSum<tbCacheTexture>(sqsum_offset + rect.y * sqsumStep + rect.x + rect.width, d_sqsum);
+ sqsum_br = getElemSqSum<tbCacheTexture>(sqsum_offset + (rect.y + rect.height) * sqsumStep + rect.x + rect.width, d_sqsum);
+ Ncv64u sqsum_val = sqsum_br + sqsum_tl - sqsum_tr - sqsum_bl;
+
+ Ncv32f mean = sum_val * invRectArea;
+
+ //////////////////////////////////////////////////////////////////////////
+ // sqsum_val_res = sqsum_val / rectArea
+ //////////////////////////////////////////////////////////////////////////
+
+ Ncv32f sqsum_val_1 = __ull2float_rz(sqsum_val);
+ Ncv64u sqsum_val_2 = __float2ull_rz(sqsum_val_1);
+ Ncv64u sqsum_val_3 = sqsum_val - sqsum_val_2;
+ Ncv32f sqsum_val_4 = __ull2float_rn(sqsum_val_3);
+ sqsum_val_1 *= invRectArea;
+ sqsum_val_4 *= invRectArea;
+ Ncv32f sqsum_val_res = sqsum_val_1 + sqsum_val_4;
+
+ //////////////////////////////////////////////////////////////////////////
+ // variance = sqsum_val_res - mean * mean
+ //////////////////////////////////////////////////////////////////////////
+
+#if defined DISABLE_MAD_SELECTIVELY
+ Ncv32f variance = sqsum_val_2 - __fmul_rn(mean, mean);
+#else
+ Ncv32f variance = sqsum_val_res - mean * mean;
+#endif
+
+ //////////////////////////////////////////////////////////////////////////
+ // stddev = sqrtf(variance)
+ //////////////////////////////////////////////////////////////////////////
+
+ //Ncv32f stddev = sqrtf(variance);
+ Ncv32f stddev = __fsqrt_rn(variance);
+
+ d_norm[blockIdx.y * normStep + x_offs] = stddev;
+}
+
+
+NCVStatus nppiStRectStdDev_32f_C1R(Ncv32u *d_sum, Ncv32u sumStep,
+ Ncv64u *d_sqsum, Ncv32u sqsumStep,
+ Ncv32f *d_norm, Ncv32u normStep,
+ NcvSize32u roi, NcvRect32u rect,
+ Ncv32f scaleArea, NcvBool readThruTexture)
+{
+ ncvAssertReturn(d_sum != NULL && d_sqsum != NULL && d_norm != NULL, NPPST_NULL_POINTER_ERROR);
+ ncvAssertReturn(roi.width > 0 && roi.height > 0, NPPST_INVALID_ROI);
+ ncvAssertReturn(sumStep >= (Ncv32u)(roi.width + rect.x + rect.width - 1) * sizeof(Ncv32u) &&
+ sqsumStep >= (Ncv32u)(roi.width + rect.x + rect.width - 1) * sizeof(Ncv64u) &&
+ normStep >= (Ncv32u)roi.width * sizeof(Ncv32f) &&
+ sumStep % sizeof(Ncv32u) == 0 &&
+ sqsumStep % sizeof(Ncv64u) == 0 &&
+ normStep % sizeof(Ncv32f) == 0, NPPST_INVALID_STEP);
+ ncvAssertReturn(scaleArea >= 1.0f, NPPST_INVALID_SCALE);
+ sumStep /= sizeof(Ncv32u);
+ sqsumStep /= sizeof(Ncv64u);
+ normStep /= sizeof(Ncv32f);
+
+ Ncv32f rectArea = rect.width * rect.height * scaleArea;
+ Ncv32f invRectArea = 1.0f / rectArea;
+
+ dim3 grid(((roi.width + NUM_RECTSTDDEV_THREADS - 1) / NUM_RECTSTDDEV_THREADS), roi.height);
+ dim3 block(NUM_RECTSTDDEV_THREADS);
+
+ if (!readThruTexture)
+ {
+ rectStdDev_32f_C1R
+ <false>
+ <<<grid, block, 0, nppStGetActiveCUDAstream()>>>
+ (d_sum, sumStep, d_sqsum, sqsumStep, d_norm, normStep, roi, rect, invRectArea);
+ }
+ else
+ {
+ cudaChannelFormatDesc cfdTexSrc;
+ cudaChannelFormatDesc cfdTexSqr;
+ cfdTexSrc = cudaCreateChannelDesc<Ncv32u>();
+ cfdTexSqr = cudaCreateChannelDesc<uint2>();
+
+ size_t alignmentOffset;
+ ncvAssertCUDAReturn(cudaBindTexture(&alignmentOffset, tex32u, d_sum, cfdTexSrc, (roi.height + rect.y + rect.height) * sumStep * sizeof(Ncv32u)), NPPST_TEXTURE_BIND_ERROR);
+ ncvAssertReturn(alignmentOffset==0, NPPST_TEXTURE_BIND_ERROR);
+ ncvAssertCUDAReturn(cudaBindTexture(&alignmentOffset, tex64u, d_sqsum, cfdTexSqr, (roi.height + rect.y + rect.height) * sqsumStep * sizeof(Ncv64u)), NPPST_TEXTURE_BIND_ERROR);
+ ncvAssertReturn(alignmentOffset==0, NPPST_TEXTURE_BIND_ERROR);
+
+ rectStdDev_32f_C1R
+ <true>
+ <<<grid, block, 0, nppStGetActiveCUDAstream()>>>
+ (NULL, sumStep, NULL, sqsumStep, d_norm, normStep, roi, rect, invRectArea);
+ }
+
+ ncvAssertCUDALastErrorReturn(NPPST_CUDA_KERNEL_EXECUTION_ERROR);
+
+ return NPPST_SUCCESS;
+}
+
+
+NCVStatus nppiStRectStdDev_32f_C1R_host(Ncv32u *h_sum, Ncv32u sumStep,
+ Ncv64u *h_sqsum, Ncv32u sqsumStep,
+ Ncv32f *h_norm, Ncv32u normStep,
+ NcvSize32u roi, NcvRect32u rect,
+ Ncv32f scaleArea)
+{
+ ncvAssertReturn(h_sum != NULL && h_sqsum != NULL && h_norm != NULL, NPPST_NULL_POINTER_ERROR);
+ ncvAssertReturn(roi.width > 0 && roi.height > 0, NPPST_INVALID_ROI);
+ ncvAssertReturn(sumStep >= (Ncv32u)(roi.width + rect.x + rect.width - 1) * sizeof(Ncv32u) &&
+ sqsumStep >= (Ncv32u)(roi.width + rect.x + rect.width - 1) * sizeof(Ncv64u) &&
+ normStep >= (Ncv32u)roi.width * sizeof(Ncv32f) &&
+ sumStep % sizeof(Ncv32u) == 0 &&
+ sqsumStep % sizeof(Ncv64u) == 0 &&
+ normStep % sizeof(Ncv32f) == 0, NPPST_INVALID_STEP);
+ ncvAssertReturn(scaleArea >= 1.0f, NPPST_INVALID_SCALE);
+ sumStep /= sizeof(Ncv32u);
+ sqsumStep /= sizeof(Ncv64u);
+ normStep /= sizeof(Ncv32f);
+
+ Ncv32f rectArea = rect.width * rect.height * scaleArea;
+ Ncv32f invRectArea = 1.0f / rectArea;
+
+ for (Ncv32u i=0; i<roi.height; i++)
+ {
+ for (Ncv32u j=0; j<roi.width; j++)
+ {
+ Ncv32u sum_offset = i * sumStep + j;
+ Ncv32u sqsum_offset = i * sqsumStep + j;
+
+ Ncv32u sum_tl = h_sum[sum_offset + rect.y * sumStep + rect.x];
+ Ncv32u sum_bl = h_sum[sum_offset + (rect.y + rect.height) * sumStep + rect.x];
+ Ncv32u sum_tr = h_sum[sum_offset + rect.y * sumStep + rect.x + rect.width];
+ Ncv32u sum_br = h_sum[sum_offset + (rect.y + rect.height) * sumStep + rect.x + rect.width];
+ Ncv64f sum_val = sum_br + sum_tl - sum_tr - sum_bl;
+
+ Ncv64u sqsum_tl = h_sqsum[sqsum_offset + rect.y * sqsumStep + rect.x];
+ Ncv64u sqsum_bl = h_sqsum[sqsum_offset + (rect.y + rect.height) * sqsumStep + rect.x];
+ Ncv64u sqsum_tr = h_sqsum[sqsum_offset + rect.y * sqsumStep + rect.x + rect.width];
+ Ncv64u sqsum_br = h_sqsum[sqsum_offset + (rect.y + rect.height) * sqsumStep + rect.x + rect.width];
+ Ncv64f sqsum_val = (Ncv64f)(sqsum_br + sqsum_tl - sqsum_tr - sqsum_bl);
+
+ Ncv64f mean = sum_val * invRectArea;
+ Ncv64f sqsum_val_2 = sqsum_val / rectArea;
+ Ncv64f variance = sqsum_val_2 - mean * mean;
+
+ h_norm[i * normStep + j] = (Ncv32f)sqrt(variance);
+ }
+ }
+
+ return NPPST_SUCCESS;
+}
+
+
+//==============================================================================
+//
+// Transpose.cu
+//
+//==============================================================================
+
+
+const Ncv32u TRANSPOSE_TILE_DIM = 16;
+const Ncv32u TRANSPOSE_BLOCK_ROWS = 16;
+
+
+/**
+* \brief Matrix transpose kernel
+*
+* Calculates transpose of the input image
+* \see TRANSPOSE_TILE_DIM
+*
+* \tparam T_in Type of input image elements
+* \tparam T_out Type of output image elements
+*
+* \param d_src [IN] Source image pointer
+* \param srcStride [IN] Source image stride
+* \param d_dst [OUT] Output image pointer
+* \param dstStride [IN] Output image stride
+*
+* \return None
+*/
+template <class T>
+__global__ void transpose(T *d_src, Ncv32u srcStride,
+ T *d_dst, Ncv32u dstStride, NcvSize32u srcRoi)
+{
+ __shared__ T tile[TRANSPOSE_TILE_DIM][TRANSPOSE_TILE_DIM+1];
+
+ Ncv32u blockIdx_x, blockIdx_y;
+
+ // do diagonal reordering
+ if (gridDim.x == gridDim.y)
+ {
+ blockIdx_y = blockIdx.x;
+ blockIdx_x = (blockIdx.x + blockIdx.y) % gridDim.x;
+ }
+ else
+ {
+ Ncv32u bid = blockIdx.x + gridDim.x * blockIdx.y;
+ blockIdx_y = bid % gridDim.y;
+ blockIdx_x = ((bid / gridDim.y) + blockIdx_y) % gridDim.x;
+ }
+
+ Ncv32u xIndex = blockIdx_x * TRANSPOSE_TILE_DIM + threadIdx.x;
+ Ncv32u yIndex = blockIdx_y * TRANSPOSE_TILE_DIM + threadIdx.y;
+ Ncv32u index_gmem = xIndex + yIndex * srcStride;
+
+ if (xIndex < srcRoi.width)
+ {
+ for (Ncv32u i=0; i<TRANSPOSE_TILE_DIM; i+=TRANSPOSE_BLOCK_ROWS)
+ {
+ if (yIndex + i < srcRoi.height)
+ {
+ tile[threadIdx.y+i][threadIdx.x] = d_src[index_gmem+i*srcStride];
+ }
+ }
+ }
+
+ __syncthreads();
+
+ xIndex = blockIdx_y * TRANSPOSE_TILE_DIM + threadIdx.x;
+ yIndex = blockIdx_x * TRANSPOSE_TILE_DIM + threadIdx.y;
+ index_gmem = xIndex + yIndex * dstStride;
+
+ if (xIndex < srcRoi.height)
+ {
+ for (Ncv32u i=0; i<TRANSPOSE_TILE_DIM; i+=TRANSPOSE_BLOCK_ROWS)
+ {
+ if (yIndex + i < srcRoi.width)
+ {
+ d_dst[index_gmem+i*dstStride] = tile[threadIdx.x][threadIdx.y+i];
+ }
+ }
+ }
+}
+
+
+template <class T>
+NCVStatus transposeWrapperDevice(T *d_src, Ncv32u srcStride,
+ T *d_dst, Ncv32u dstStride, NcvSize32u srcRoi)
+{
+ ncvAssertReturn(d_src != NULL && d_dst != NULL, NPPST_NULL_POINTER_ERROR);
+ ncvAssertReturn(srcRoi.width > 0 && srcRoi.height > 0, NPPST_INVALID_ROI);
+ ncvAssertReturn(srcStride >= srcRoi.width * sizeof(T) &&
+ dstStride >= srcRoi.height * sizeof(T) &&
+ srcStride % sizeof(T) == 0 && dstStride % sizeof(T) == 0, NPPST_INVALID_STEP);
+ srcStride /= sizeof(T);
+ dstStride /= sizeof(T);
+
+ dim3 grid((srcRoi.width + TRANSPOSE_TILE_DIM - 1) / TRANSPOSE_TILE_DIM,
+ (srcRoi.height + TRANSPOSE_TILE_DIM - 1) / TRANSPOSE_TILE_DIM);
+ dim3 block(TRANSPOSE_TILE_DIM, TRANSPOSE_TILE_DIM);
+ transpose
+ <T>
+ <<<grid, block, 0, nppStGetActiveCUDAstream()>>>
+ (d_src, srcStride, d_dst, dstStride, srcRoi);
+ ncvAssertCUDALastErrorReturn(NPPST_CUDA_KERNEL_EXECUTION_ERROR);
+
+ return NPPST_SUCCESS;
+}
+
+
+template <class T>
+static NCVStatus transposeWrapperHost(T *h_src, Ncv32u srcStride,
+ T *h_dst, Ncv32u dstStride, NcvSize32u srcRoi)
+{
+ ncvAssertReturn(h_src != NULL && h_dst != NULL, NPPST_NULL_POINTER_ERROR);
+ ncvAssertReturn(srcRoi.width > 0 && srcRoi.height > 0, NPPST_INVALID_ROI);
+ ncvAssertReturn(srcStride >= srcRoi.width * sizeof(T) &&
+ dstStride >= srcRoi.height * sizeof(T) &&
+ srcStride % sizeof(T) == 0 && dstStride % sizeof(T) == 0, NPPST_INVALID_STEP);
+ srcStride /= sizeof(T);
+ dstStride /= sizeof(T);
+
+ for (Ncv32u i=0; i<srcRoi.height; i++)
+ {
+ for (Ncv32u j=0; j<srcRoi.width; j++)
+ {
+ h_dst[j*dstStride+i] = h_src[i*srcStride + j];
+ }
+ }
+
+ return NPPST_SUCCESS;
+}
+
+
+#define implementNppTranspose(bit, typ) \
+ NCVStatus nppiStTranspose_##bit##typ##_C1R(Ncv##bit##typ *d_src, Ncv32u srcStep, \
+ Ncv##bit##typ *d_dst, Ncv32u dstStep, NcvSize32u srcRoi) \
+ { \
+ return transposeWrapperDevice<Ncv##bit##u>((Ncv##bit##u *)d_src, srcStep, \
+ (Ncv##bit##u *)d_dst, dstStep, srcRoi); \
+ }
+
+
+#define implementNppTransposeHost(bit, typ) \
+ NCVStatus nppiStTranspose_##bit##typ##_C1R_host(Ncv##bit##typ *h_src, Ncv32u srcStep, \
+ Ncv##bit##typ *h_dst, Ncv32u dstStep, \
+ NcvSize32u srcRoi) \
+ { \
+ return transposeWrapperHost<Ncv##bit##u>((Ncv##bit##u *)h_src, srcStep, \
+ (Ncv##bit##u *)h_dst, dstStep, srcRoi); \
+ }
+
+
+implementNppTranspose(32,u)
+implementNppTranspose(32,s)
+implementNppTranspose(32,f)
+implementNppTranspose(64,u)
+implementNppTranspose(64,s)
+implementNppTranspose(64,f)
+
+implementNppTransposeHost(32,u)
+implementNppTransposeHost(32,s)
+implementNppTransposeHost(32,f)
+implementNppTransposeHost(64,u)
+implementNppTransposeHost(64,s)
+implementNppTransposeHost(64,f)
+
+
+NCVStatus nppiStTranspose_128_C1R(void *d_src, Ncv32u srcStep,
+ void *d_dst, Ncv32u dstStep, NcvSize32u srcRoi)
+{
+ return transposeWrapperDevice<uint4>((uint4 *)d_src, srcStep, (uint4 *)d_dst, dstStep, srcRoi);
+}
+
+
+NCVStatus nppiStTranspose_128_C1R_host(void *d_src, Ncv32u srcStep,
+ void *d_dst, Ncv32u dstStep, NcvSize32u srcRoi)
+{
+ return transposeWrapperHost<uint4>((uint4 *)d_src, srcStep, (uint4 *)d_dst, dstStep, srcRoi);
+}
+
+
+//==============================================================================
+//
+// Compact.cu
+//
+//==============================================================================
+
+
+const Ncv32u NUM_REMOVE_THREADS = 256;
+
+
+template <bool bRemove, bool bWritePartial>
+__global__ void removePass1Scan(Ncv32u *d_src, Ncv32u srcLen,
+ Ncv32u *d_offsets, Ncv32u *d_blockSums,
+ Ncv32u elemRemove)
+{
+ Ncv32u blockId = blockIdx.y * 65535 + blockIdx.x;
+ Ncv32u elemAddrIn = blockId * NUM_REMOVE_THREADS + threadIdx.x;
+
+ if (elemAddrIn > srcLen + blockDim.x)
+ {
+ return;
+ }
+
+ __shared__ Ncv32u shmem[NUM_REMOVE_THREADS * 2];
+
+ Ncv32u scanElem = 0;
+ if (elemAddrIn < srcLen)
+ {
+ if (bRemove)
+ {
+ scanElem = (d_src[elemAddrIn] != elemRemove) ? 1 : 0;
+ }
+ else
+ {
+ scanElem = d_src[elemAddrIn];
+ }
+ }
+
+ Ncv32u localScanInc = blockScanInclusive<Ncv32u, NUM_REMOVE_THREADS>(scanElem, shmem);
+ __syncthreads();
+
+ if (elemAddrIn < srcLen)
+ {
+ if (threadIdx.x == NUM_REMOVE_THREADS-1 && bWritePartial)
+ {
+ d_blockSums[blockId] = localScanInc;
+ }
+
+ if (bRemove)
+ {
+ d_offsets[elemAddrIn] = localScanInc - scanElem;
+ }
+ else
+ {
+ d_src[elemAddrIn] = localScanInc - scanElem;
+ }
+ }
+}
+
+
+__global__ void removePass2Adjust(Ncv32u *d_offsets, Ncv32u srcLen, Ncv32u *d_blockSums)
+{
+ Ncv32u blockId = blockIdx.y * 65535 + blockIdx.x;
+ Ncv32u elemAddrIn = blockId * NUM_REMOVE_THREADS + threadIdx.x;
+ if (elemAddrIn >= srcLen)
+ {
+ return;
+ }
+
+ __shared__ Ncv32u valOffs;
+ valOffs = d_blockSums[blockId];
+ __syncthreads();
+
+ d_offsets[elemAddrIn] += valOffs;
+}
+
+
+__global__ void removePass3Compact(Ncv32u *d_src, Ncv32u srcLen,
+ Ncv32u *d_offsets, Ncv32u *d_dst,
+ Ncv32u elemRemove, Ncv32u *dstLenValue)
+{
+ Ncv32u blockId = blockIdx.y * 65535 + blockIdx.x;
+ Ncv32u elemAddrIn = blockId * NUM_REMOVE_THREADS + threadIdx.x;
+ if (elemAddrIn >= srcLen)
+ {
+ return;
+ }
+
+ Ncv32u elem = d_src[elemAddrIn];
+ Ncv32u elemAddrOut = d_offsets[elemAddrIn];
+ if (elem != elemRemove)
+ {
+ d_dst[elemAddrOut] = elem;
+ }
+
+ if (elemAddrIn == srcLen-1)
+ {
+ if (elem != elemRemove)
+ {
+ *dstLenValue = elemAddrOut + 1;
+ }
+ else
+ {
+ *dstLenValue = elemAddrOut;
+ }
+ }
+}
+
+
+NCVStatus compactVector_32u_device(Ncv32u *d_src, Ncv32u srcLen,
+ Ncv32u *d_dst, Ncv32u *dstLenPinned,
+ Ncv32u elemRemove,
+ INCVMemAllocator &gpuAllocator)
+{
+ ncvAssertReturn(gpuAllocator.isInitialized(), NPPST_MEM_INTERNAL_ERROR);
+ ncvAssertReturn((d_src != NULL && d_dst != NULL) || gpuAllocator.isCounting(), NPPST_NULL_POINTER_ERROR);
+
+ if (srcLen == 0)
+ {
+ if (dstLenPinned != NULL)
+ {
+ *dstLenPinned = 0;
+ }
+ return NPPST_SUCCESS;
+ }
+
+ std::vector<Ncv32u> partSumNums;
+ std::vector<Ncv32u> partSumOffsets;
+ Ncv32u partSumLastNum = srcLen;
+ Ncv32u partSumLastOffs = 0;
+ do
+ {
+ partSumNums.push_back(partSumLastNum);
+ partSumOffsets.push_back(partSumLastOffs);
+
+ Ncv32u curPartSumAlignedLength = alignUp(partSumLastNum * sizeof(Ncv32u),
+ gpuAllocator.alignment()) / sizeof(Ncv32u);
+ partSumLastOffs += curPartSumAlignedLength;
+
+ partSumLastNum = (partSumLastNum + NUM_REMOVE_THREADS - 1) / NUM_REMOVE_THREADS;
+ }
+ while (partSumLastNum>1);
+ partSumNums.push_back(partSumLastNum);
+ partSumOffsets.push_back(partSumLastOffs);
+
+ NCVVectorAlloc<Ncv32u> d_hierSums(gpuAllocator, partSumLastOffs+1);
+ ncvAssertReturn(gpuAllocator.isCounting() || d_hierSums.isMemAllocated(), NPPST_MEM_INTERNAL_ERROR);
+ NCVVectorAlloc<Ncv32u> d_numDstElements(gpuAllocator, 1);
+ ncvAssertReturn(gpuAllocator.isCounting() || d_numDstElements.isMemAllocated(), NPPST_MEM_INTERNAL_ERROR);
+
+ NCV_SET_SKIP_COND(gpuAllocator.isCounting());
+ NCV_SKIP_COND_BEGIN
+
+ dim3 block(NUM_REMOVE_THREADS);
+
+ //calculate zero-level partial sums for indices calculation
+ if (partSumNums.size() > 2)
+ {
+ dim3 grid(partSumNums[1]);
+
+ if (grid.x > 65535)
+ {
+ grid.y = (grid.x + 65534) / 65535;
+ grid.x = 65535;
+ }
+ removePass1Scan
+ <true, true>
+ <<<grid, block, 0, nppStGetActiveCUDAstream()>>>
+ (d_src, srcLen,
+ d_hierSums.ptr(),
+ d_hierSums.ptr() + partSumOffsets[1],
+ elemRemove);
+
+ ncvAssertCUDALastErrorReturn(NPPST_CUDA_KERNEL_EXECUTION_ERROR);
+
+ //calculate hierarchical partial sums
+ for (Ncv32u i=1; i<partSumNums.size()-1; i++)
+ {
+ dim3 grid(partSumNums[i+1]);
+ if (grid.x > 65535)
+ {
+ grid.y = (grid.x + 65534) / 65535;
+ grid.x = 65535;
+ }
+ if (grid.x != 1)
+ {
+ removePass1Scan
+ <false, true>
+ <<<grid, block, 0, nppStGetActiveCUDAstream()>>>
+ (d_hierSums.ptr() + partSumOffsets[i],
+ partSumNums[i], NULL,
+ d_hierSums.ptr() + partSumOffsets[i+1],
+ NULL);
+ }
+ else
+ {
+ removePass1Scan
+ <false, false>
+ <<<grid, block, 0, nppStGetActiveCUDAstream()>>>
+ (d_hierSums.ptr() + partSumOffsets[i],
+ partSumNums[i], NULL,
+ NULL,
+ NULL);
+ }
+
+ ncvAssertCUDALastErrorReturn(NPPST_CUDA_KERNEL_EXECUTION_ERROR);
+ }
+
+ //adjust hierarchical partial sums
+ for (Ncv32s i=(Ncv32s)partSumNums.size()-3; i>=0; i--)
+ {
+ dim3 grid(partSumNums[i+1]);
+ if (grid.x > 65535)
+ {
+ grid.y = (grid.x + 65534) / 65535;
+ grid.x = 65535;
+ }
+ removePass2Adjust
+ <<<grid, block, 0, nppStGetActiveCUDAstream()>>>
+ (d_hierSums.ptr() + partSumOffsets[i], partSumNums[i],
+ d_hierSums.ptr() + partSumOffsets[i+1]);
+
+ ncvAssertCUDALastErrorReturn(NPPST_CUDA_KERNEL_EXECUTION_ERROR);
+ }
+ }
+ else
+ {
+ dim3 grid(partSumNums[1]);
+ removePass1Scan
+ <true, false>
+ <<<grid, block, 0, nppStGetActiveCUDAstream()>>>
+ (d_src, srcLen,
+ d_hierSums.ptr(),
+ NULL, elemRemove);
+
+ ncvAssertCUDALastErrorReturn(NPPST_CUDA_KERNEL_EXECUTION_ERROR);
+ }
+
+ //compact source vector using indices
+ dim3 grid(partSumNums[1]);
+ if (grid.x > 65535)
+ {
+ grid.y = (grid.x + 65534) / 65535;
+ grid.x = 65535;
+ }
+ removePass3Compact
+ <<<grid, block, 0, nppStGetActiveCUDAstream()>>>
+ (d_src, srcLen, d_hierSums.ptr(), d_dst,
+ elemRemove, d_numDstElements.ptr());
+
+ ncvAssertCUDALastErrorReturn(NPPST_CUDA_KERNEL_EXECUTION_ERROR);
+
+ //get number of dst elements
+ if (dstLenPinned != NULL)
+ {
+ ncvAssertCUDAReturn(cudaMemcpyAsync(dstLenPinned, d_numDstElements.ptr(), sizeof(Ncv32u),
+ cudaMemcpyDeviceToHost, nppStGetActiveCUDAstream()), NPPST_MEM_RESIDENCE_ERROR);
+ ncvAssertCUDAReturn(cudaStreamSynchronize(nppStGetActiveCUDAstream()), NPPST_MEM_RESIDENCE_ERROR);
+ }
+
+ NCV_SKIP_COND_END
+
+ return NPPST_SUCCESS;
+}
+
+
+NCVStatus nppsStCompactGetSize_32u(Ncv32u srcLen, Ncv32u *pBufsize, cudaDeviceProp &devProp)
+{
+ ncvAssertReturn(pBufsize != NULL, NPPST_NULL_POINTER_ERROR);
+
+ if (srcLen == 0)
+ {
+ *pBufsize = 0;
+ return NPPST_SUCCESS;
+ }
+
+ NCVMemStackAllocator gpuCounter(static_cast<Ncv32u>(devProp.textureAlignment));
+ ncvAssertReturn(gpuCounter.isInitialized(), NPPST_MEM_INTERNAL_ERROR);
+
+ NCVStatus ncvStat = compactVector_32u_device(NULL, srcLen, NULL, NULL, 0xC001C0DE,
+ gpuCounter);
+ ncvAssertReturnNcvStat(ncvStat);
+
+ *pBufsize = (Ncv32u)gpuCounter.maxSize();
+ return NPPST_SUCCESS;
+}
+
+
+NCVStatus nppsStCompactGetSize_32s(Ncv32u srcLen, Ncv32u *pBufsize, cudaDeviceProp &devProp)
+{
+ return nppsStCompactGetSize_32u(srcLen, pBufsize, devProp);
+}
+
+
+NCVStatus nppsStCompactGetSize_32f(Ncv32u srcLen, Ncv32u *pBufsize, cudaDeviceProp &devProp)
+{
+ return nppsStCompactGetSize_32u(srcLen, pBufsize, devProp);
+}
+
+
+NCVStatus nppsStCompact_32u(Ncv32u *d_src, Ncv32u srcLen,
+ Ncv32u *d_dst, Ncv32u *p_dstLen,
+ Ncv32u elemRemove, Ncv8u *pBuffer,
+ Ncv32u bufSize, cudaDeviceProp &devProp)
+{
+ NCVMemStackAllocator gpuAllocator(NCVMemoryTypeDevice, bufSize, static_cast<Ncv32u>(devProp.textureAlignment), pBuffer);
+ ncvAssertReturn(gpuAllocator.isInitialized(), NPPST_MEM_INTERNAL_ERROR);
+
+ NCVStatus ncvStat = compactVector_32u_device(d_src, srcLen, d_dst, p_dstLen, elemRemove,
+ gpuAllocator);
+ ncvAssertReturnNcvStat(ncvStat);
+
+ return NPPST_SUCCESS;
+}
+
+
+NCVStatus nppsStCompact_32s(Ncv32s *d_src, Ncv32u srcLen,
+ Ncv32s *d_dst, Ncv32u *p_dstLen,
+ Ncv32s elemRemove, Ncv8u *pBuffer,
+ Ncv32u bufSize, cudaDeviceProp &devProp)
+{
+ return nppsStCompact_32u((Ncv32u *)d_src, srcLen, (Ncv32u *)d_dst, p_dstLen,
+ *(Ncv32u *)&elemRemove, pBuffer, bufSize, devProp);
+}
+
+
+NCVStatus nppsStCompact_32f(Ncv32f *d_src, Ncv32u srcLen,
+ Ncv32f *d_dst, Ncv32u *p_dstLen,
+ Ncv32f elemRemove, Ncv8u *pBuffer,
+ Ncv32u bufSize, cudaDeviceProp &devProp)
+{
+ return nppsStCompact_32u((Ncv32u *)d_src, srcLen, (Ncv32u *)d_dst, p_dstLen,
+ *(Ncv32u *)&elemRemove, pBuffer, bufSize, devProp);
+}
+
+
+NCVStatus nppsStCompact_32u_host(Ncv32u *h_src, Ncv32u srcLen,
+ Ncv32u *h_dst, Ncv32u *dstLen, Ncv32u elemRemove)
+{
+ ncvAssertReturn(h_src != NULL && h_dst != NULL, NPPST_NULL_POINTER_ERROR);
+
+ if (srcLen == 0)
+ {
+ if (dstLen != NULL)
+ {
+ *dstLen = 0;
+ }
+ return NPPST_SUCCESS;
+ }
+
+ Ncv32u dstIndex = 0;
+ for (Ncv32u srcIndex=0; srcIndex<srcLen; srcIndex++)
+ {
+ if (h_src[srcIndex] != elemRemove)
+ {
+ h_dst[dstIndex++] = h_src[srcIndex];
+ }
+ }
+
+ if (dstLen != NULL)
+ {
+ *dstLen = dstIndex;
+ }
+
+ return NPPST_SUCCESS;
+}
+
+
+NCVStatus nppsStCompact_32s_host(Ncv32s *h_src, Ncv32u srcLen,
+ Ncv32s *h_dst, Ncv32u *dstLen, Ncv32s elemRemove)
+{
+ return nppsStCompact_32u_host((Ncv32u *)h_src, srcLen, (Ncv32u *)h_dst, dstLen, *(Ncv32u *)&elemRemove);
+}
+
+
+NCVStatus nppsStCompact_32f_host(Ncv32f *h_src, Ncv32u srcLen,
+ Ncv32f *h_dst, Ncv32u *dstLen, Ncv32f elemRemove)
+{
+ return nppsStCompact_32u_host((Ncv32u *)h_src, srcLen, (Ncv32u *)h_dst, dstLen, *(Ncv32u *)&elemRemove);
+}
+
+
+//==============================================================================
+//
+// Filter.cu
+//
+//==============================================================================
+
+
+texture <float, 1, cudaReadModeElementType> texSrc;
+texture <float, 1, cudaReadModeElementType> texKernel;
+
+
+__forceinline__ __device__ float getValueMirrorRow(const int rowOffset,
+ int i,
+ int w)
+{
+ if (i < 0) i = 1 - i;
+ if (i >= w) i = w + w - i - 1;
+ return tex1Dfetch (texSrc, rowOffset + i);
+}
+
+
+__forceinline__ __device__ float getValueMirrorColumn(const int offset,
+ const int rowStep,
+ int j,
+ int h)
+{
+ if (j < 0) j = 1 - j;
+ if (j >= h) j = h + h - j - 1;
+ return tex1Dfetch (texSrc, offset + j * rowStep);
+}
+
+
+__global__ void FilterRowBorderMirror_32f_C1R(Ncv32u srcStep,
+ Ncv32f *pDst,
+ NcvSize32u dstSize,
+ Ncv32u dstStep,
+ NcvRect32u roi,
+ Ncv32s nKernelSize,
+ Ncv32s nAnchor,
+ Ncv32f multiplier)
+{
+ // position within ROI
+ const int ix = blockDim.x * blockIdx.x + threadIdx.x;
+ const int iy = blockDim.y * blockIdx.y + threadIdx.y;
+
+ if (ix >= roi.width || iy >= roi.height)
+ {
+ return;
+ }
+
+ const int p = nKernelSize - nAnchor - 1;
+
+ const int j = roi.y + iy;
+
+ const int rowOffset = j * srcStep + roi.x;
+
+ float sum = 0.0f;
+ for (int m = 0; m < nKernelSize; ++m)
+ {
+ sum += getValueMirrorRow (rowOffset, ix + m - p, roi.width)
+ * tex1Dfetch (texKernel, m);
+ }
+
+ pDst[iy * dstStep + ix] = sum * multiplier;
+}
+
+
+__global__ void FilterColumnBorderMirror_32f_C1R(Ncv32u srcStep,
+ Ncv32f *pDst,
+ NcvSize32u dstSize,
+ Ncv32u dstStep,
+ NcvRect32u roi,
+ Ncv32s nKernelSize,
+ Ncv32s nAnchor,
+ Ncv32f multiplier)
+{
+ const int ix = blockDim.x * blockIdx.x + threadIdx.x;
+ const int iy = blockDim.y * blockIdx.y + threadIdx.y;
+
+ if (ix >= roi.width || iy >= roi.height)
+ {
+ return;
+ }
+
+ const int p = nKernelSize - nAnchor - 1;
+ const int i = roi.x + ix;
+ const int offset = i + roi.y * srcStep;
+
+ float sum = 0.0f;
+ for (int m = 0; m < nKernelSize; ++m)
+ {
+ sum += getValueMirrorColumn (offset, srcStep, iy + m - p, roi.height)
+ * tex1Dfetch (texKernel, m);
+ }
+
+ pDst[ix + iy * dstStep] = sum * multiplier;
+}
+
+
+NCVStatus nppiStFilterRowBorder_32f_C1R(const Ncv32f *pSrc,
+ NcvSize32u srcSize,
+ Ncv32u nSrcStep,
+ Ncv32f *pDst,
+ NcvSize32u dstSize,
+ Ncv32u nDstStep,
+ NcvRect32u oROI,
+ NppStBorderType borderType,
+ const Ncv32f *pKernel,
+ Ncv32s nKernelSize,
+ Ncv32s nAnchor,
+ Ncv32f multiplier)
+{
+ ncvAssertReturn (pSrc != NULL &&
+ pDst != NULL &&
+ pKernel != NULL, NCV_NULL_PTR);
+
+ ncvAssertReturn (oROI.width > 0 && oROI.height > 0, NPPST_INVALID_ROI);
+
+ ncvAssertReturn (srcSize.width * sizeof (Ncv32f) <= nSrcStep &&
+ dstSize.width * sizeof (Ncv32f) <= nDstStep &&
+ oROI.width * sizeof (Ncv32f) <= nSrcStep &&
+ oROI.width * sizeof (Ncv32f) <= nDstStep &&
+ nSrcStep % sizeof (Ncv32f) == 0 &&
+ nDstStep % sizeof (Ncv32f) == 0, NPPST_INVALID_STEP);
+
+ Ncv32u srcStep = nSrcStep / sizeof (Ncv32f);
+ Ncv32u dstStep = nDstStep / sizeof (Ncv32f);
+
+ // adjust ROI size to be within source image
+ if (oROI.x + oROI.width > srcSize.width)
+ {
+ oROI.width = srcSize.width - oROI.x;
+ }
+
+ if (oROI.y + oROI.height > srcSize.height)
+ {
+ oROI.height = srcSize.height - oROI.y;
+ }
+
+ cudaChannelFormatDesc floatChannel = cudaCreateChannelDesc <float> ();
+ texSrc.normalized = false;
+ texKernel.normalized = false;
+
+ cudaBindTexture (0, texSrc, pSrc, floatChannel, srcSize.height * nSrcStep);
+ cudaBindTexture (0, texKernel, pKernel, floatChannel, nKernelSize * sizeof (Ncv32f));
+
+ dim3 ctaSize (32, 6);
+ dim3 gridSize ((oROI.width + ctaSize.x - 1) / ctaSize.x,
+ (oROI.height + ctaSize.y - 1) / ctaSize.y);
+
+ switch (borderType)
+ {
+ case nppStBorderNone:
+ return NPPST_ERROR;
+ case nppStBorderClamp:
+ return NPPST_ERROR;
+ case nppStBorderWrap:
+ return NPPST_ERROR;
+ case nppStBorderMirror:
+ FilterRowBorderMirror_32f_C1R <<<gridSize, ctaSize, 0, nppStGetActiveCUDAstream ()>>>
+ (srcStep, pDst, dstSize, dstStep, oROI, nKernelSize, nAnchor, multiplier);
+ ncvAssertCUDALastErrorReturn(NPPST_CUDA_KERNEL_EXECUTION_ERROR);
+ break;
+ default:
+ return NPPST_ERROR;
+ }
+
+ return NPPST_SUCCESS;
+}
+
+
+NCVStatus nppiStFilterColumnBorder_32f_C1R(const Ncv32f *pSrc,
+ NcvSize32u srcSize,
+ Ncv32u nSrcStep,
+ Ncv32f *pDst,
+ NcvSize32u dstSize,
+ Ncv32u nDstStep,
+ NcvRect32u oROI,
+ NppStBorderType borderType,
+ const Ncv32f *pKernel,
+ Ncv32s nKernelSize,
+ Ncv32s nAnchor,
+ Ncv32f multiplier)
+{
+ ncvAssertReturn (pSrc != NULL &&
+ pDst != NULL &&
+ pKernel != NULL, NCV_NULL_PTR);
+
+ ncvAssertReturn (oROI.width > 0 && oROI.height > 0, NPPST_INVALID_ROI);
+
+ ncvAssertReturn (srcSize.width * sizeof (Ncv32f) <= nSrcStep &&
+ dstSize.width * sizeof (Ncv32f) <= nDstStep &&
+ oROI.width * sizeof (Ncv32f) <= nSrcStep &&
+ oROI.width * sizeof (Ncv32f) <= nDstStep &&
+ nSrcStep % sizeof (Ncv32f) == 0 &&
+ nDstStep % sizeof (Ncv32f) == 0, NPPST_INVALID_STEP);
+
+ Ncv32u srcStep = nSrcStep / sizeof (Ncv32f);
+ Ncv32u dstStep = nDstStep / sizeof (Ncv32f);
+
+ // adjust ROI size to be within source image
+ if (oROI.x + oROI.width > srcSize.width)
+ {
+ oROI.width = srcSize.width - oROI.x;
+ }
+
+ if (oROI.y + oROI.height > srcSize.height)
+ {
+ oROI.height = srcSize.height - oROI.y;
+ }
+
+ cudaChannelFormatDesc floatChannel = cudaCreateChannelDesc <float> ();
+ texSrc.normalized = false;
+ texKernel.normalized = false;
+
+ cudaBindTexture (0, texSrc, pSrc, floatChannel, srcSize.height * nSrcStep);
+ cudaBindTexture (0, texKernel, pKernel, floatChannel, nKernelSize * sizeof (Ncv32f));
+
+ dim3 ctaSize (32, 6);
+ dim3 gridSize ((oROI.width + ctaSize.x - 1) / ctaSize.x,
+ (oROI.height + ctaSize.y - 1) / ctaSize.y);
+
+ switch (borderType)
+ {
+ case nppStBorderClamp:
+ return NPPST_ERROR;
+ case nppStBorderWrap:
+ return NPPST_ERROR;
+ case nppStBorderMirror:
+ FilterColumnBorderMirror_32f_C1R <<<gridSize, ctaSize, 0, nppStGetActiveCUDAstream ()>>>
+ (srcStep, pDst, dstSize, dstStep, oROI, nKernelSize, nAnchor, multiplier);
+ ncvAssertCUDALastErrorReturn(NPPST_CUDA_KERNEL_EXECUTION_ERROR);
+ break;
+ default:
+ return NPPST_ERROR;
+ }
+
+ return NPPST_SUCCESS;
+}
+
+
+//==============================================================================
+//
+// FrameInterpolate.cu
+//
+//==============================================================================
+
+
+inline Ncv32u iDivUp(Ncv32u num, Ncv32u denom)
+{
+ return (num + denom - 1)/denom;
+}
+
+
+texture<float, 2, cudaReadModeElementType> tex_src1;
+texture<float, 2, cudaReadModeElementType> tex_src0;
+
+
+__global__ void BlendFramesKernel(const float *u, const float *v, // forward flow
+ const float *ur, const float *vr, // backward flow
+ const float *o0, const float *o1, // coverage masks
+ int w, int h, int s,
+ float theta, float *out)
+{
+ const int ix = threadIdx.x + blockDim.x * blockIdx.x;
+ const int iy = threadIdx.y + blockDim.y * blockIdx.y;
+
+ const int pos = ix + s * iy;
+
+ if (ix >= w || iy >= h) return;
+
+ float _u = u[pos];
+ float _v = v[pos];
+
+ float _ur = ur[pos];
+ float _vr = vr[pos];
+
+ float x = (float)ix + 0.5f;
+ float y = (float)iy + 0.5f;
+ bool b0 = o0[pos] > 1e-4f;
+ bool b1 = o1[pos] > 1e-4f;
+
+ if (b0 && b1)
+ {
+ // pixel is visible on both frames
+ out[pos] = tex2D(tex_src0, x - _u * theta, y - _v * theta) * (1.0f - theta) +
+ tex2D(tex_src1, x + _u * (1.0f - theta), y + _v * (1.0f - theta)) * theta;
+ }
+ else if (b0)
+ {
+ // visible on the first frame only
+ out[pos] = tex2D(tex_src0, x - _u * theta, y - _v * theta);
+ }
+ else
+ {
+ // visible on the second frame only
+ out[pos] = tex2D(tex_src1, x - _ur * (1.0f - theta), y - _vr * (1.0f - theta));
+ }
+}
+
+
+NCVStatus BlendFrames(const Ncv32f *src0,
+ const Ncv32f *src1,
+ const Ncv32f *ufi,
+ const Ncv32f *vfi,
+ const Ncv32f *ubi,
+ const Ncv32f *vbi,
+ const Ncv32f *o1,
+ const Ncv32f *o2,
+ Ncv32u width,
+ Ncv32u height,
+ Ncv32u stride,
+ Ncv32f theta,
+ Ncv32f *out)
+{
+ tex_src1.addressMode[0] = cudaAddressModeClamp;
+ tex_src1.addressMode[1] = cudaAddressModeClamp;
+ tex_src1.filterMode = cudaFilterModeLinear;
+ tex_src1.normalized = false;
+
+ tex_src0.addressMode[0] = cudaAddressModeClamp;
+ tex_src0.addressMode[1] = cudaAddressModeClamp;
+ tex_src0.filterMode = cudaFilterModeLinear;
+ tex_src0.normalized = false;
+
+ cudaChannelFormatDesc desc = cudaCreateChannelDesc <float> ();
+ const Ncv32u pitch = stride * sizeof (float);
+ ncvAssertCUDAReturn (cudaBindTexture2D (0, tex_src1, src1, desc, width, height, pitch), NPPST_TEXTURE_BIND_ERROR);
+ ncvAssertCUDAReturn (cudaBindTexture2D (0, tex_src0, src0, desc, width, height, pitch), NPPST_TEXTURE_BIND_ERROR);
+
+ dim3 threads (32, 4);
+ dim3 blocks (iDivUp (width, threads.x), iDivUp (height, threads.y));
+
+ BlendFramesKernel<<<blocks, threads, 0, nppStGetActiveCUDAstream ()>>>
+ (ufi, vfi, ubi, vbi, o1, o2, width, height, stride, theta, out);
+
+ ncvAssertCUDALastErrorReturn(NPPST_CUDA_KERNEL_EXECUTION_ERROR);
+
+ return NPPST_SUCCESS;
+}
+
+
+NCVStatus nppiStGetInterpolationBufferSize(NcvSize32u srcSize,
+ Ncv32u nStep,
+ Ncv32u *hpSize)
+{
+ NCVStatus status = NPPST_ERROR;
+ status = nppiStVectorWarpGetBufferSize(srcSize, nStep, hpSize);
+ return status;
+}
+
+
+NCVStatus nppiStInterpolateFrames(const NppStInterpolationState *pState)
+{
+ // check state validity
+ ncvAssertReturn (pState->pSrcFrame0 != 0 &&
+ pState->pSrcFrame1 != 0 &&
+ pState->pFU != 0 &&
+ pState->pFV != 0 &&
+ pState->pBU != 0 &&
+ pState->pBV != 0 &&
+ pState->pNewFrame != 0 &&
+ pState->ppBuffers[0] != 0 &&
+ pState->ppBuffers[1] != 0 &&
+ pState->ppBuffers[2] != 0 &&
+ pState->ppBuffers[3] != 0 &&
+ pState->ppBuffers[4] != 0 &&
+ pState->ppBuffers[5] != 0, NPPST_NULL_POINTER_ERROR);
+
+ ncvAssertReturn (pState->size.width > 0 &&
+ pState->size.height > 0, NPPST_ERROR);
+
+ ncvAssertReturn (pState->nStep >= pState->size.width * sizeof (Ncv32f) &&
+ pState->nStep > 0 &&
+ pState->nStep % sizeof (Ncv32f) == 0,
+ NPPST_INVALID_STEP);
+
+ // change notation
+ Ncv32f *cov0 = pState->ppBuffers[0];
+ Ncv32f *cov1 = pState->ppBuffers[1];
+ Ncv32f *fwdU = pState->ppBuffers[2]; // forward u
+ Ncv32f *fwdV = pState->ppBuffers[3]; // forward v
+ Ncv32f *bwdU = pState->ppBuffers[4]; // backward u
+ Ncv32f *bwdV = pState->ppBuffers[5]; // backward v
+ // warp flow
+ ncvAssertReturnNcvStat (
+ nppiStVectorWarp_PSF2x2_32f_C1 (pState->pFU,
+ pState->size,
+ pState->nStep,
+ pState->pFU,
+ pState->pFV,
+ pState->nStep,
+ cov0,
+ pState->pos,
+ fwdU) );
+ ncvAssertReturnNcvStat (
+ nppiStVectorWarp_PSF2x2_32f_C1 (pState->pFV,
+ pState->size,
+ pState->nStep,
+ pState->pFU,
+ pState->pFV,
+ pState->nStep,
+ cov0,
+ pState->pos,
+ fwdV) );
+ // warp backward flow
+ ncvAssertReturnNcvStat (
+ nppiStVectorWarp_PSF2x2_32f_C1 (pState->pBU,
+ pState->size,
+ pState->nStep,
+ pState->pBU,
+ pState->pBV,
+ pState->nStep,
+ cov1,
+ 1.0f - pState->pos,
+ bwdU) );
+ ncvAssertReturnNcvStat (
+ nppiStVectorWarp_PSF2x2_32f_C1 (pState->pBV,
+ pState->size,
+ pState->nStep,
+ pState->pBU,
+ pState->pBV,
+ pState->nStep,
+ cov1,
+ 1.0f - pState->pos,
+ bwdU) );
+ // interpolate frame
+ ncvAssertReturnNcvStat (
+ BlendFrames (pState->pSrcFrame0,
+ pState->pSrcFrame1,
+ fwdU,
+ fwdV,
+ bwdU,
+ bwdV,
+ cov0,
+ cov1,
+ pState->size.width,
+ pState->size.height,
+ pState->nStep / sizeof (Ncv32f),
+ pState->pos,
+ pState->pNewFrame) );
+
+ return NPPST_SUCCESS;
+}
+
+
+//==============================================================================
+//
+// VectorWarpFrame.cu
+//
+//==============================================================================
+
+
+#if __CUDA_ARCH__ < 200
+
+// FP32 atomic add
+static __forceinline__ __device__ float _atomicAdd(float *addr, float val)
+{
+ float old = *addr, assumed;
+
+ do {
+ assumed = old;
+ old = int_as_float(__iAtomicCAS((int*)addr,
+ float_as_int(assumed),
+ float_as_int(val+assumed)));
+ } while( assumed!=old );
+
+ return old;
+}
+#else
+#define _atomicAdd atomicAdd
+#endif
+
+
+__global__ void ForwardWarpKernel_PSF2x2(const float *u,
+ const float *v,
+ const float *src,
+ const int w,
+ const int h,
+ const int flow_stride,
+ const int image_stride,
+ const float time_scale,
+ float *normalization_factor,
+ float *dst)
+{
+ int j = threadIdx.x + blockDim.x * blockIdx.x;
+ int i = threadIdx.y + blockDim.y * blockIdx.y;
+
+ if (i >= h || j >= w) return;
+
+ int flow_row_offset = i * flow_stride;
+ int image_row_offset = i * image_stride;
+
+ //bottom left corner of a target pixel
+ float cx = u[flow_row_offset + j] * time_scale + (float)j + 1.0f;
+ float cy = v[flow_row_offset + j] * time_scale + (float)i + 1.0f;
+ // pixel containing bottom left corner
+ float px;
+ float py;
+ float dx = modff (cx, &px);
+ float dy = modff (cy, &py);
+ // target pixel integer coords
+ int tx;
+ int ty;
+ tx = (int) px;
+ ty = (int) py;
+ float value = src[image_row_offset + j];
+ float weight;
+ // fill pixel containing bottom right corner
+ if (!((tx >= w) || (tx < 0) || (ty >= h) || (ty < 0)))
+ {
+ weight = dx * dy;
+ _atomicAdd (dst + ty * image_stride + tx, value * weight);
+ _atomicAdd (normalization_factor + ty * image_stride + tx, weight);
+ }
+
+ // fill pixel containing bottom left corner
+ tx -= 1;
+ if (!((tx >= w) || (tx < 0) || (ty >= h) || (ty < 0)))
+ {
+ weight = (1.0f - dx) * dy;
+ _atomicAdd (dst + ty * image_stride + tx, value * weight);
+ _atomicAdd (normalization_factor + ty * image_stride + tx, weight);
+ }
+
+ // fill pixel containing upper left corner
+ ty -= 1;
+ if (!((tx >= w) || (tx < 0) || (ty >= h) || (ty < 0)))
+ {
+ weight = (1.0f - dx) * (1.0f - dy);
+ _atomicAdd (dst + ty * image_stride + tx, value * weight);
+ _atomicAdd (normalization_factor + ty * image_stride + tx, weight);
+ }
+
+ // fill pixel containing upper right corner
+ tx += 1;
+ if (!((tx >= w) || (tx < 0) || (ty >= h) || (ty < 0)))
+ {
+ weight = dx * (1.0f - dy);
+ _atomicAdd (dst + ty * image_stride + tx, value * weight);
+ _atomicAdd (normalization_factor + ty * image_stride + tx, weight);
+ }
+}
+
+
+__global__ void ForwardWarpKernel_PSF1x1(const float *u,
+ const float *v,
+ const float *src,
+ const int w,
+ const int h,
+ const int flow_stride,
+ const int image_stride,
+ const float time_scale,
+ float *dst)
+{
+ int j = threadIdx.x + blockDim.x * blockIdx.x;
+ int i = threadIdx.y + blockDim.y * blockIdx.y;
+
+ if (i >= h || j >= w) return;
+
+ int flow_row_offset = i * flow_stride;
+ int image_row_offset = i * image_stride;
+
+ float u_ = u[flow_row_offset + j];
+ float v_ = v[flow_row_offset + j];
+
+ //bottom left corner of target pixel
+ float cx = u_ * time_scale + (float)j + 1.0f;
+ float cy = v_ * time_scale + (float)i + 1.0f;
+ // pixel containing bottom left corner
+ int tx = __float2int_rn (cx);
+ int ty = __float2int_rn (cy);
+
+ float value = src[image_row_offset + j];
+ // fill pixel
+ if (!((tx >= w) || (tx < 0) || (ty >= h) || (ty < 0)))
+ {
+ _atomicAdd (dst + ty * image_stride + tx, value);
+ }
+}
+
+
+__global__ void NormalizeKernel(const float *normalization_factor, int w, int h, int s, float *image)
+{
+ int i = threadIdx.y + blockDim.y * blockIdx.y;
+ int j = threadIdx.x + blockDim.x * blockIdx.x;
+
+ if (i >= h || j >= w) return;
+
+ const int pos = i * s + j;
+
+ float scale = normalization_factor[pos];
+
+ float invScale = (scale == 0.0f) ? 1.0f : (1.0f / scale);
+
+ image[pos] *= invScale;
+}
+
+
+__global__ void MemsetKernel(const float value, int w, int h, float *image)
+{
+ int i = threadIdx.y + blockDim.y * blockIdx.y;
+ int j = threadIdx.x + blockDim.x * blockIdx.x;
+
+ if (i >= h || j >= w) return;
+
+ const int pos = i * w + j;
+
+ image[pos] = value;
+}
+
+
+NCVStatus nppiStVectorWarpGetBufferSize (NcvSize32u srcSize, Ncv32u nSrcStep, Ncv32u *hpSize)
+{
+ ncvAssertReturn (hpSize != NULL, NPPST_NULL_POINTER_ERROR);
+ ncvAssertReturn (srcSize.width * sizeof (Ncv32f) <= nSrcStep,
+ NPPST_INVALID_STEP);
+
+ *hpSize = nSrcStep * srcSize.height;
+
+ return NPPST_SUCCESS;
+}
+
+
+// does not require normalization
+NCVStatus nppiStVectorWarp_PSF1x1_32f_C1(const Ncv32f *pSrc,
+ NcvSize32u srcSize,
+ Ncv32u nSrcStep,
+ const Ncv32f *pU,
+ const Ncv32f *pV,
+ Ncv32u nVFStep,
+ Ncv32f timeScale,
+ Ncv32f *pDst)
+{
+ ncvAssertReturn (pSrc != NULL &&
+ pU != NULL &&
+ pV != NULL &&
+ pDst != NULL, NPPST_NULL_POINTER_ERROR);
+
+ ncvAssertReturn (srcSize.width * sizeof (Ncv32f) <= nSrcStep &&
+ srcSize.width * sizeof (Ncv32f) <= nVFStep,
+ NPPST_INVALID_STEP);
+
+ Ncv32u srcStep = nSrcStep / sizeof (Ncv32f);
+ Ncv32u vfStep = nVFStep / sizeof (Ncv32f);
+
+ dim3 ctaSize (32, 6);
+ dim3 gridSize (iDivUp (srcSize.width, ctaSize.x), iDivUp (srcSize.height, ctaSize.y));
+
+ ForwardWarpKernel_PSF1x1 <<<gridSize, ctaSize, 0, nppStGetActiveCUDAstream()>>>
+ (pU, pV, pSrc, srcSize.width, srcSize.height, vfStep, srcStep, timeScale, pDst);
+
+ ncvAssertCUDALastErrorReturn(NPPST_CUDA_KERNEL_EXECUTION_ERROR);
+
+ return NPPST_SUCCESS;
+}
+
+
+NCVStatus nppiStVectorWarp_PSF2x2_32f_C1(const Ncv32f *pSrc,
+ NcvSize32u srcSize,
+ Ncv32u nSrcStep,
+ const Ncv32f *pU,
+ const Ncv32f *pV,
+ Ncv32u nVFStep,
+ Ncv32f *pBuffer,
+ Ncv32f timeScale,
+ Ncv32f *pDst)
+{
+ ncvAssertReturn (pSrc != NULL &&
+ pU != NULL &&
+ pV != NULL &&
+ pDst != NULL &&
+ pBuffer != NULL, NPPST_NULL_POINTER_ERROR);
+
+ ncvAssertReturn (srcSize.width * sizeof (Ncv32f) <= nSrcStep &&
+ srcSize.width * sizeof (Ncv32f) <= nVFStep, NPPST_INVALID_STEP);
+
+ Ncv32u srcStep = nSrcStep / sizeof (Ncv32f);
+ Ncv32u vfStep = nVFStep / sizeof(Ncv32f);
+
+ dim3 ctaSize(32, 6);
+ dim3 gridSize (iDivUp (srcSize.width, ctaSize.x), iDivUp (srcSize.height, ctaSize.y));
+
+ MemsetKernel <<<gridSize, ctaSize, 0, nppStGetActiveCUDAstream()>>>
+ (0, srcSize.width, srcSize.height, pBuffer);
+
+ ncvAssertCUDALastErrorReturn(NPPST_CUDA_KERNEL_EXECUTION_ERROR);
+
+ ForwardWarpKernel_PSF2x2 <<<gridSize, ctaSize, 0, nppStGetActiveCUDAstream()>>>
+ (pU, pV, pSrc, srcSize.width, srcSize.height, vfStep, srcStep, timeScale, pBuffer, pDst);
+
+ ncvAssertCUDALastErrorReturn(NPPST_CUDA_KERNEL_EXECUTION_ERROR);
+
+ NormalizeKernel <<<gridSize, ctaSize, 0, nppStGetActiveCUDAstream()>>>
+ (pBuffer, srcSize.width, srcSize.height, srcStep, pDst);
+
+ ncvAssertCUDALastErrorReturn(NPPST_CUDA_KERNEL_EXECUTION_ERROR);
+
+ return NPPST_SUCCESS;
+}
+
+
+//==============================================================================
+//
+// Resize.cu
+//
+//==============================================================================
+
+
+texture <float, 2, cudaReadModeElementType> texSrc2D;
+
+
+__forceinline__
+__device__ float processLine(int spos,
+ float xmin,
+ float xmax,
+ int ixmin,
+ int ixmax,
+ float fxmin,
+ float cxmax)
+{
+ // first element
+ float wsum = 1.0f - xmin + fxmin;
+ float sum = tex1Dfetch(texSrc, spos) * (1.0f - xmin + fxmin);
+ spos++;
+ for (int ix = ixmin + 1; ix < ixmax; ++ix)
+ {
+ sum += tex1Dfetch(texSrc, spos);
+ spos++;
+ wsum += 1.0f;
+ }
+ sum += tex1Dfetch(texSrc, spos) * (cxmax - xmax);
+ wsum += cxmax - xmax;
+ return sum / wsum;
+}
+
+
+__global__ void resizeSuperSample_32f(NcvSize32u srcSize,
+ Ncv32u srcStep,
+ NcvRect32u srcROI,
+ Ncv32f *dst,
+ NcvSize32u dstSize,
+ Ncv32u dstStep,
+ NcvRect32u dstROI,
+ Ncv32f scaleX,
+ Ncv32f scaleY)
+{
+ // position within dst ROI
+ const int ix = blockIdx.x * blockDim.x + threadIdx.x;
+ const int iy = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if (ix >= dstROI.width || iy >= dstROI.height)
+ {
+ return;
+ }
+
+ float rw = (float) srcROI.width;
+ float rh = (float) srcROI.height;
+
+ // source position
+ float x = scaleX * (float) ix;
+ float y = scaleY * (float) iy;
+
+ // x sampling range
+ float xBegin = fmax (x - scaleX, 0.0f);
+ float xEnd = fmin (x + scaleX, rw - 1.0f);
+ // y sampling range
+ float yBegin = fmax (y - scaleY, 0.0f);
+ float yEnd = fmin (y + scaleY, rh - 1.0f);
+ // x range of source samples
+ float floorXBegin = floorf (xBegin);
+ float ceilXEnd = ceilf (xEnd);
+ int iXBegin = srcROI.x + (int) floorXBegin;
+ int iXEnd = srcROI.x + (int) ceilXEnd;
+ // y range of source samples
+ float floorYBegin = floorf (yBegin);
+ float ceilYEnd = ceilf (yEnd);
+ int iYBegin = srcROI.y + (int) floorYBegin;
+ int iYEnd = srcROI.y + (int) ceilYEnd;
+
+ // first row
+ int pos = iYBegin * srcStep + iXBegin;
+
+ float wsum = 1.0f - yBegin + floorYBegin;
+
+ float sum = processLine (pos, xBegin, xEnd, iXBegin, iXEnd, floorXBegin,
+ ceilXEnd) * (1.0f - yBegin + floorYBegin);
+ pos += srcStep;
+ for (int iy = iYBegin + 1; iy < iYEnd; ++iy)
+ {
+ sum += processLine (pos, xBegin, xEnd, iXBegin, iXEnd, floorXBegin,
+ ceilXEnd);
+ pos += srcStep;
+ wsum += 1.0f;
+ }
+
+ sum += processLine (pos, xBegin, xEnd, iXBegin, iXEnd, floorXBegin,
+ ceilXEnd) * (ceilYEnd - yEnd);
+ wsum += ceilYEnd - yEnd;
+ sum /= wsum;
+
+ dst[(ix + dstROI.x) + (iy + dstROI.y) * dstStep] = sum;
+}
+
+
+// bicubic interpolation
+__forceinline__
+__device__ float bicubicCoeff(float x_)
+{
+ float x = fabsf(x_);
+ if (x <= 1.0f)
+ {
+ return x * x * (1.5f * x - 2.5f) + 1.0f;
+ }
+ else if (x < 2.0f)
+ {
+ return x * (x * (-0.5f * x + 2.5f) - 4.0f) + 2.0f;
+ }
+ else
+ {
+ return 0.0f;
+ }
+}
+
+
+__global__ void resizeBicubic(NcvSize32u srcSize,
+ NcvRect32u srcROI,
+ NcvSize32u dstSize,
+ Ncv32u dstStep,
+ Ncv32f *dst,
+ NcvRect32u dstROI,
+ Ncv32f scaleX,
+ Ncv32f scaleY)
+{
+ const int ix = blockIdx.x * blockDim.x + threadIdx.x;
+ const int iy = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if (ix >= dstROI.width || iy >= dstROI.height)
+ {
+ return;
+ }
+
+ const float dx = 1.0f / srcROI.width;
+ const float dy = 1.0f / srcROI.height;
+
+ float rx = (float) srcROI.x;
+ float ry = (float) srcROI.y;
+
+ float rw = (float) srcROI.width;
+ float rh = (float) srcROI.height;
+
+ float x = scaleX * (float) ix;
+ float y = scaleY * (float) iy;
+
+ // sampling range
+ // border mode is clamp
+ float xmin = fmax (ceilf (x - 2.0f), 0.0f);
+ float xmax = fmin (floorf (x + 2.0f), rw - 1.0f);
+
+ float ymin = fmax (ceilf (y - 2.0f), 0.0f);
+ float ymax = fmin (floorf (y + 2.0f), rh - 1.0f);
+
+ // shift data window to match ROI
+ rx += 0.5f;
+ ry += 0.5f;
+
+ x += rx;
+ y += ry;
+
+ xmin += rx;
+ xmax += rx;
+ ymin += ry;
+ ymax += ry;
+
+ float sum = 0.0f;
+ float wsum = 0.0f;
+
+ for (float cy = ymin; cy <= ymax; cy += 1.0f)
+ {
+ for (float cx = xmin; cx <= xmax; cx += 1.0f)
+ {
+ float xDist = x - cx;
+ float yDist = y - cy;
+ float wx = bicubicCoeff (xDist);
+ float wy = bicubicCoeff (yDist);
+ wx *= wy;
+ sum += wx * tex2D (texSrc2D, cx * dx, cy * dy);
+ wsum += wx;
+ }
+ }
+ dst[(ix + dstROI.x)+ (iy + dstROI.y) * dstStep] = (!wsum)? 0 : sum / wsum;
+}
+
+
+NCVStatus nppiStResize_32f_C1R(const Ncv32f *pSrc,
+ NcvSize32u srcSize,
+ Ncv32u nSrcStep,
+ NcvRect32u srcROI,
+ Ncv32f *pDst,
+ NcvSize32u dstSize,
+ Ncv32u nDstStep,
+ NcvRect32u dstROI,
+ Ncv32f xFactor,
+ Ncv32f yFactor,
+ NppStInterpMode interpolation)
+{
+ NCVStatus status = NPPST_SUCCESS;
+
+ ncvAssertReturn (pSrc != NULL && pDst != NULL, NPPST_NULL_POINTER_ERROR);
+ ncvAssertReturn (xFactor != 0.0 && yFactor != 0.0, NPPST_INVALID_SCALE);
+
+ ncvAssertReturn (nSrcStep >= sizeof (Ncv32f) * (Ncv32u) srcSize.width &&
+ nDstStep >= sizeof (Ncv32f) * (Ncv32f) dstSize.width,
+ NPPST_INVALID_STEP);
+
+ Ncv32u srcStep = nSrcStep / sizeof (Ncv32f);
+ Ncv32u dstStep = nDstStep / sizeof (Ncv32f);
+
+ // TODO: preprocess ROI to prevent out of bounds access
+
+ if (interpolation == nppStSupersample)
+ {
+ // bind texture
+ cudaBindTexture (0, texSrc, pSrc, srcSize.height * nSrcStep);
+ // invoke kernel
+ dim3 ctaSize (32, 6);
+ dim3 gridSize ((dstROI.width + ctaSize.x - 1) / ctaSize.x,
+ (dstROI.height + ctaSize.y - 1) / ctaSize.y);
+
+ resizeSuperSample_32f <<<gridSize, ctaSize, 0, nppStGetActiveCUDAstream ()>>>
+ (srcSize, srcStep, srcROI, pDst, dstSize, dstStep, dstROI, 1.0f / xFactor, 1.0f / yFactor);
+ }
+ else if (interpolation == nppStBicubic)
+ {
+ texSrc2D.addressMode[0] = cudaAddressModeMirror;
+ texSrc2D.addressMode[1] = cudaAddressModeMirror;
+ texSrc2D.normalized = true;
+
+ cudaChannelFormatDesc desc = cudaCreateChannelDesc <float> ();
+
+ cudaBindTexture2D (0, texSrc2D, pSrc, desc, srcSize.width, srcSize.height,
+ nSrcStep);
+
+ dim3 ctaSize (32, 6);
+ dim3 gridSize ((dstSize.width + ctaSize.x - 1) / ctaSize.x,
+ (dstSize.height + ctaSize.y - 1) / ctaSize.y);
+
+ resizeBicubic <<<gridSize, ctaSize, 0, nppStGetActiveCUDAstream ()>>>
+ (srcSize, srcROI, dstSize, dstStep, pDst, dstROI, 1.0f / xFactor, 1.0f / yFactor);
+ }
+ else
+ {
+ status = NPPST_ERROR;
+ }
+
+ ncvAssertCUDALastErrorReturn(NPPST_CUDA_KERNEL_EXECUTION_ERROR);
+
+ return status;
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (C) 2009-2010, NVIDIA Corporation, all rights reserved.
+ * Third party copyrights are property of their respective owners.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * Ported to PCL by Koen Buys : Attention Work in progress!
+ */
+
+#ifndef _npp_staging_hpp_
+#define _npp_staging_hpp_
+
+#include "NCV.hpp"
+
+
+/**
+* \file NPP_staging.hpp
+* NPP Staging Library
+*/
+
+
+/** \defgroup core_npp NPPST Core
+ * Basic functions for CUDA streams management.
+ * @{
+ */
+
+
+/**
+ * Gets an active CUDA stream used by NPPST
+ * NOT THREAD SAFE
+ * \return Current CUDA stream
+ */
+NCV_EXPORTS
+cudaStream_t nppStGetActiveCUDAstream();
+
+
+/**
+ * Sets an active CUDA stream used by NPPST
+ * NOT THREAD SAFE
+ * \param cudaStream [IN] cudaStream CUDA stream to become current
+ * \return CUDA stream used before
+ */
+NCV_EXPORTS
+cudaStream_t nppStSetActiveCUDAstream(cudaStream_t cudaStream);
+
+
+/*@}*/
+
+
+/** \defgroup nppi NPPST Image Processing
+* @{
+*/
+
+
+/** Border type
+ *
+ * Filtering operations assume that each pixel has a neighborhood of pixels.
+ * The following structure describes possible ways to define non-existent pixels.
+ */
+enum NppStBorderType
+{
+ nppStBorderNone = 0, ///< There is no need to define additional pixels, image is extended already
+ nppStBorderClamp = 1, ///< Clamp out of range position to borders
+ nppStBorderWrap = 2, ///< Wrap out of range position. Image becomes periodic.
+ nppStBorderMirror = 3 ///< reflect out of range position across borders
+};
+
+
+/**
+ * Filter types for image resizing
+ */
+enum NppStInterpMode
+{
+ nppStSupersample, ///< Supersampling. For downscaling only
+ nppStBicubic ///< Bicubic convolution filter, a = -0.5 (cubic Hermite spline)
+};
+
+
+/** Frame interpolation state
+ *
+ * This structure holds parameters required for frame interpolation.
+ * Forward displacement field is a per-pixel mapping from frame 0 to frame 1.
+ * Backward displacement field is a per-pixel mapping from frame 1 to frame 0.
+ */
+
+ struct NppStInterpolationState
+{
+ NcvSize32u size; ///< frame size
+ Ncv32u nStep; ///< pitch
+ Ncv32f pos; ///< new frame position
+ Ncv32f *pSrcFrame0; ///< frame 0
+ Ncv32f *pSrcFrame1; ///< frame 1
+ Ncv32f *pFU; ///< forward horizontal displacement
+ Ncv32f *pFV; ///< forward vertical displacement
+ Ncv32f *pBU; ///< backward horizontal displacement
+ Ncv32f *pBV; ///< backward vertical displacement
+ Ncv32f *pNewFrame; ///< new frame
+ Ncv32f *ppBuffers[6]; ///< temporary buffers
+};
+
+
+/** Size of a buffer required for interpolation.
+ *
+ * Requires several such buffers. See \see NppStInterpolationState.
+ *
+ * \param srcSize [IN] Frame size (both frames must be of the same size)
+ * \param nStep [IN] Frame line step
+ * \param hpSize [OUT] Where to store computed size (host memory)
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStGetInterpolationBufferSize(NcvSize32u srcSize,
+ Ncv32u nStep,
+ Ncv32u *hpSize);
+
+
+/** Interpolate frames (images) using provided optical flow (displacement field).
+ * 32-bit floating point images, single channel
+ *
+ * \param pState [IN] structure containing all required parameters (host memory)
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStInterpolateFrames(const NppStInterpolationState *pState);
+
+
+/** Row linear filter. 32-bit floating point image, single channel
+ *
+ * Apply horizontal linear filter
+ *
+ * \param pSrc [IN] Source image pointer (CUDA device memory)
+ * \param srcSize [IN] Source image size
+ * \param nSrcStep [IN] Source image line step
+ * \param pDst [OUT] Destination image pointer (CUDA device memory)
+ * \param dstSize [OUT] Destination image size
+ * \param nDstStep
+ * \param oROI [IN] Region of interest in the source image
+ * \param borderType [IN] Type of border
+ * \param pKernel [IN] Pointer to row kernel values (CUDA device memory)
+ * \param nKernelSize [IN] Size of the kernel in pixels
+ * \param nAnchor [IN] The kernel row alignment with respect to the position of the input pixel
+ * \param multiplier [IN] Value by which the computed result is multiplied
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStFilterRowBorder_32f_C1R(const Ncv32f *pSrc,
+ NcvSize32u srcSize,
+ Ncv32u nSrcStep,
+ Ncv32f *pDst,
+ NcvSize32u dstSize,
+ Ncv32u nDstStep,
+ NcvRect32u oROI,
+ NppStBorderType borderType,
+ const Ncv32f *pKernel,
+ Ncv32s nKernelSize,
+ Ncv32s nAnchor,
+ Ncv32f multiplier);
+
+
+/** Column linear filter. 32-bit floating point image, single channel
+ *
+ * Apply vertical linear filter
+ *
+ * \param pSrc [IN] Source image pointer (CUDA device memory)
+ * \param srcSize [IN] Source image size
+ * \param nSrcStep [IN] Source image line step
+ * \param pDst [OUT] Destination image pointer (CUDA device memory)
+ * \param dstSize [OUT] Destination image size
+ * \param nDstStep
+ * \param oROI [IN] Region of interest in the source image
+ * \param borderType [IN] Type of border
+ * \param pKernel [IN] Pointer to column kernel values (CUDA device memory)
+ * \param nKernelSize [IN] Size of the kernel in pixels
+ * \param nAnchor [IN] The kernel column alignment with respect to the position of the input pixel
+ * \param multiplier [IN] Value by which the computed result is multiplied
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStFilterColumnBorder_32f_C1R(const Ncv32f *pSrc,
+ NcvSize32u srcSize,
+ Ncv32u nSrcStep,
+ Ncv32f *pDst,
+ NcvSize32u dstSize,
+ Ncv32u nDstStep,
+ NcvRect32u oROI,
+ NppStBorderType borderType,
+ const Ncv32f *pKernel,
+ Ncv32s nKernelSize,
+ Ncv32s nAnchor,
+ Ncv32f multiplier);
+
+
+/** Size of buffer required for vector image warping.
+ *
+ * \param srcSize [IN] Source image size
+ * \param nSrcStep [IN] Source image line step
+ * \param hpSize [OUT] Where to store computed size (host memory)
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStVectorWarpGetBufferSize(NcvSize32u srcSize,
+ Ncv32u nSrcStep,
+ Ncv32u *hpSize);
+
+
+/** Warp image using provided 2D vector field and 1x1 point spread function.
+ * 32-bit floating point image, single channel
+ *
+ * During warping pixels from the source image may fall between pixels of the destination image.
+ * PSF (point spread function) describes how the source image pixel affects pixels of the destination.
+ * For 1x1 PSF only single pixel with the largest intersection is affected (similar to nearest interpolation).
+ *
+ * Destination image size and line step must be the same as the source image size and line step
+ *
+ * \param pSrc [IN] Source image pointer (CUDA device memory)
+ * \param srcSize [IN] Source image size
+ * \param nSrcStep [IN] Source image line step
+ * \param pU [IN] Pointer to horizontal displacement field (CUDA device memory)
+ * \param pV [IN] Pointer to vertical displacement field (CUDA device memory)
+ * \param nVFStep [IN] Displacement field line step
+ * \param timeScale [IN] Value by which displacement field will be scaled for warping
+ * \param pDst [OUT] Destination image pointer (CUDA device memory)
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStVectorWarp_PSF1x1_32f_C1(const Ncv32f *pSrc,
+ NcvSize32u srcSize,
+ Ncv32u nSrcStep,
+ const Ncv32f *pU,
+ const Ncv32f *pV,
+ Ncv32u nVFStep,
+ Ncv32f timeScale,
+ Ncv32f *pDst);
+
+
+/** Warp image using provided 2D vector field and 2x2 point spread function.
+ * 32-bit floating point image, single channel
+ *
+ * During warping pixels from the source image may fall between pixels of the destination image.
+ * PSF (point spread function) describes how the source image pixel affects pixels of the destination.
+ * For 2x2 PSF all four intersected pixels will be affected.
+ *
+ * Destination image size and line step must be the same as the source image size and line step
+ *
+ * \param pSrc [IN] Source image pointer (CUDA device memory)
+ * \param srcSize [IN] Source image size
+ * \param nSrcStep [IN] Source image line step
+ * \param pU [IN] Pointer to horizontal displacement field (CUDA device memory)
+ * \param pV [IN] Pointer to vertical displacement field (CUDA device memory)
+ * \param nVFStep [IN] Displacement field line step
+ * \param pBuffer
+ * \param timeScale [IN] Value by which displacement field will be scaled for warping
+ * \param pDst [OUT] Destination image pointer (CUDA device memory)
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStVectorWarp_PSF2x2_32f_C1(const Ncv32f *pSrc,
+ NcvSize32u srcSize,
+ Ncv32u nSrcStep,
+ const Ncv32f *pU,
+ const Ncv32f *pV,
+ Ncv32u nVFStep,
+ Ncv32f *pBuffer,
+ Ncv32f timeScale,
+ Ncv32f *pDst);
+
+
+/** Resize. 32-bit floating point image, single channel
+ *
+ * Resizes image using specified filter (interpolation type)
+ *
+ * \param pSrc [IN] Source image pointer (CUDA device memory)
+ * \param srcSize [IN] Source image size
+ * \param nSrcStep [IN] Source image line step
+ * \param srcROI [IN] Source image region of interest
+ * \param pDst [OUT] Destination image pointer (CUDA device memory)
+ * \param dstSize [IN] Destination image size
+ * \param nDstStep [IN] Destination image line step
+ * \param dstROI [IN] Destination image region of interest
+ * \param xFactor [IN] Row scale factor
+ * \param yFactor [IN] Column scale factor
+ * \param interpolation [IN] Interpolation type
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStResize_32f_C1R(const Ncv32f *pSrc,
+ NcvSize32u srcSize,
+ Ncv32u nSrcStep,
+ NcvRect32u srcROI,
+ Ncv32f *pDst,
+ NcvSize32u dstSize,
+ Ncv32u nDstStep,
+ NcvRect32u dstROI,
+ Ncv32f xFactor,
+ Ncv32f yFactor,
+ NppStInterpMode interpolation);
+
+
+/**
+ * Downsamples (decimates) an image using the nearest neighbor algorithm. 32-bit unsigned pixels, single channel.
+ *
+ * \param d_src [IN] Source image pointer (CUDA device memory)
+ * \param srcStep [IN] Source image line step
+ * \param d_dst [OUT] Destination image pointer (CUDA device memory)
+ * \param dstStep [IN] Destination image line step
+ * \param srcRoi [IN] Region of interest in the source image
+ * \param scale [IN] Downsampling scale factor (positive integer)
+ * \param readThruTexture [IN] Performance hint to cache source in texture (true) or read directly (false)
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStDecimate_32u_C1R(Ncv32u *d_src, Ncv32u srcStep,
+ Ncv32u *d_dst, Ncv32u dstStep,
+ NcvSize32u srcRoi, Ncv32u scale,
+ NcvBool readThruTexture);
+
+
+/**
+ * Downsamples (decimates) an image using the nearest neighbor algorithm. 32-bit signed pixels, single channel.
+ * \see nppiStDecimate_32u_C1R
+ */
+NCV_EXPORTS
+NCVStatus nppiStDecimate_32s_C1R(Ncv32s *d_src, Ncv32u srcStep,
+ Ncv32s *d_dst, Ncv32u dstStep,
+ NcvSize32u srcRoi, Ncv32u scale,
+ NcvBool readThruTexture);
+
+
+/**
+ * Downsamples (decimates) an image using the nearest neighbor algorithm. 32-bit float pixels, single channel.
+ * \see nppiStDecimate_32u_C1R
+ */
+NCV_EXPORTS
+NCVStatus nppiStDecimate_32f_C1R(Ncv32f *d_src, Ncv32u srcStep,
+ Ncv32f *d_dst, Ncv32u dstStep,
+ NcvSize32u srcRoi, Ncv32u scale,
+ NcvBool readThruTexture);
+
+
+/**
+* Downsamples (decimates) an image using the nearest neighbor algorithm. 64-bit unsigned pixels, single channel.
+* \see nppiStDecimate_32u_C1R
+*/
+NCV_EXPORTS
+NCVStatus nppiStDecimate_64u_C1R(Ncv64u *d_src, Ncv32u srcStep,
+ Ncv64u *d_dst, Ncv32u dstStep,
+ NcvSize32u srcRoi, Ncv32u scale,
+ NcvBool readThruTexture);
+
+
+/**
+ * Downsamples (decimates) an image using the nearest neighbor algorithm. 64-bit signed pixels, single channel.
+ * \see nppiStDecimate_32u_C1R
+ */
+NCV_EXPORTS
+NCVStatus nppiStDecimate_64s_C1R(Ncv64s *d_src, Ncv32u srcStep,
+ Ncv64s *d_dst, Ncv32u dstStep,
+ NcvSize32u srcRoi, Ncv32u scale,
+ NcvBool readThruTexture);
+
+
+/**
+ * Downsamples (decimates) an image using the nearest neighbor algorithm. 64-bit float pixels, single channel.
+ * \see nppiStDecimate_32u_C1R
+ */
+NCV_EXPORTS
+NCVStatus nppiStDecimate_64f_C1R(Ncv64f *d_src, Ncv32u srcStep,
+ Ncv64f *d_dst, Ncv32u dstStep,
+ NcvSize32u srcRoi, Ncv32u scale,
+ NcvBool readThruTexture);
+
+
+/**
+ * Downsamples (decimates) an image using the nearest neighbor algorithm. 32-bit unsigned pixels, single channel. Host implementation.
+ *
+ * \param h_src [IN] Source image pointer (Host or pinned memory)
+ * \param srcStep [IN] Source image line step
+ * \param h_dst [OUT] Destination image pointer (Host or pinned memory)
+ * \param dstStep [IN] Destination image line step
+ * \param srcRoi [IN] Region of interest in the source image
+ * \param scale [IN] Downsampling scale factor (positive integer)
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStDecimate_32u_C1R_host(Ncv32u *h_src, Ncv32u srcStep,
+ Ncv32u *h_dst, Ncv32u dstStep,
+ NcvSize32u srcRoi, Ncv32u scale);
+
+
+/**
+ * Downsamples (decimates) an image using the nearest neighbor algorithm. 32-bit signed pixels, single channel. Host implementation.
+ * \see nppiStDecimate_32u_C1R_host
+ */
+NCV_EXPORTS
+NCVStatus nppiStDecimate_32s_C1R_host(Ncv32s *h_src, Ncv32u srcStep,
+ Ncv32s *h_dst, Ncv32u dstStep,
+ NcvSize32u srcRoi, Ncv32u scale);
+
+
+/**
+ * Downsamples (decimates) an image using the nearest neighbor algorithm. 32-bit float pixels, single channel. Host implementation.
+ * \see nppiStDecimate_32u_C1R_host
+ */
+NCV_EXPORTS
+NCVStatus nppiStDecimate_32f_C1R_host(Ncv32f *h_src, Ncv32u srcStep,
+ Ncv32f *h_dst, Ncv32u dstStep,
+ NcvSize32u srcRoi, Ncv32u scale);
+
+
+/**
+ * Downsamples (decimates) an image using the nearest neighbor algorithm. 64-bit unsigned pixels, single channel. Host implementation.
+ * \see nppiStDecimate_32u_C1R_host
+ */
+NCV_EXPORTS
+NCVStatus nppiStDecimate_64u_C1R_host(Ncv64u *h_src, Ncv32u srcStep,
+ Ncv64u *h_dst, Ncv32u dstStep,
+ NcvSize32u srcRoi, Ncv32u scale);
+
+
+/**
+ * Downsamples (decimates) an image using the nearest neighbor algorithm. 64-bit signed pixels, single channel. Host implementation.
+ * \see nppiStDecimate_32u_C1R_host
+ */
+NCV_EXPORTS
+NCVStatus nppiStDecimate_64s_C1R_host(Ncv64s *h_src, Ncv32u srcStep,
+ Ncv64s *h_dst, Ncv32u dstStep,
+ NcvSize32u srcRoi, Ncv32u scale);
+
+
+/**
+ * Downsamples (decimates) an image using the nearest neighbor algorithm. 64-bit float pixels, single channel. Host implementation.
+ * \see nppiStDecimate_32u_C1R_host
+ */
+NCV_EXPORTS
+NCVStatus nppiStDecimate_64f_C1R_host(Ncv64f *h_src, Ncv32u srcStep,
+ Ncv64f *h_dst, Ncv32u dstStep,
+ NcvSize32u srcRoi, Ncv32u scale);
+
+
+/**
+ * Computes standard deviation for each rectangular region of the input image using integral images.
+ *
+ * \param d_sum [IN] Integral image pointer (CUDA device memory)
+ * \param sumStep [IN] Integral image line step
+ * \param d_sqsum [IN] Squared integral image pointer (CUDA device memory)
+ * \param sqsumStep [IN] Squared integral image line step
+ * \param d_norm [OUT] Stddev image pointer (CUDA device memory). Each pixel contains stddev of a rect with top-left corner at the original location in the image
+ * \param normStep [IN] Stddev image line step
+ * \param roi [IN] Region of interest in the source image
+ * \param rect [IN] Rectangular region to calculate stddev over
+ * \param scaleArea [IN] Multiplication factor to account decimated scale
+ * \param readThruTexture [IN] Performance hint to cache source in texture (true) or read directly (false)
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStRectStdDev_32f_C1R(Ncv32u *d_sum, Ncv32u sumStep,
+ Ncv64u *d_sqsum, Ncv32u sqsumStep,
+ Ncv32f *d_norm, Ncv32u normStep,
+ NcvSize32u roi, NcvRect32u rect,
+ Ncv32f scaleArea, NcvBool readThruTexture);
+
+
+/**
+ * Computes standard deviation for each rectangular region of the input image using integral images. Host implementation
+ *
+ * \param h_sum [IN] Integral image pointer (Host or pinned memory)
+ * \param sumStep [IN] Integral image line step
+ * \param h_sqsum [IN] Squared integral image pointer (Host or pinned memory)
+ * \param sqsumStep [IN] Squared integral image line step
+ * \param h_norm [OUT] Stddev image pointer (Host or pinned memory). Each pixel contains stddev of a rect with top-left corner at the original location in the image
+ * \param normStep [IN] Stddev image line step
+ * \param roi [IN] Region of interest in the source image
+ * \param rect [IN] Rectangular region to calculate stddev over
+ * \param scaleArea [IN] Multiplication factor to account decimated scale
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStRectStdDev_32f_C1R_host(Ncv32u *h_sum, Ncv32u sumStep,
+ Ncv64u *h_sqsum, Ncv32u sqsumStep,
+ Ncv32f *h_norm, Ncv32u normStep,
+ NcvSize32u roi, NcvRect32u rect,
+ Ncv32f scaleArea);
+
+
+/**
+ * Transposes an image. 32-bit unsigned pixels, single channel
+ *
+ * \param d_src [IN] Source image pointer (CUDA device memory)
+ * \param srcStride [IN] Source image line step
+ * \param d_dst [OUT] Destination image pointer (CUDA device memory)
+ * \param dstStride [IN] Destination image line step
+ * \param srcRoi [IN] Region of interest of the source image
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStTranspose_32u_C1R(Ncv32u *d_src, Ncv32u srcStride,
+ Ncv32u *d_dst, Ncv32u dstStride, NcvSize32u srcRoi);
+
+
+/**
+ * Transposes an image. 32-bit signed pixels, single channel
+ * \see nppiStTranspose_32u_C1R
+ */
+NCV_EXPORTS
+NCVStatus nppiStTranspose_32s_C1R(Ncv32s *d_src, Ncv32u srcStride,
+ Ncv32s *d_dst, Ncv32u dstStride, NcvSize32u srcRoi);
+
+
+/**
+ * Transposes an image. 32-bit float pixels, single channel
+ * \see nppiStTranspose_32u_C1R
+ */
+NCV_EXPORTS
+NCVStatus nppiStTranspose_32f_C1R(Ncv32f *d_src, Ncv32u srcStride,
+ Ncv32f *d_dst, Ncv32u dstStride, NcvSize32u srcRoi);
+
+
+/**
+ * Transposes an image. 64-bit unsigned pixels, single channel
+ * \see nppiStTranspose_32u_C1R
+ */
+NCV_EXPORTS
+NCVStatus nppiStTranspose_64u_C1R(Ncv64u *d_src, Ncv32u srcStride,
+ Ncv64u *d_dst, Ncv32u dstStride, NcvSize32u srcRoi);
+
+
+/**
+ * Transposes an image. 64-bit signed pixels, single channel
+ * \see nppiStTranspose_32u_C1R
+ */
+NCV_EXPORTS
+NCVStatus nppiStTranspose_64s_C1R(Ncv64s *d_src, Ncv32u srcStride,
+ Ncv64s *d_dst, Ncv32u dstStride, NcvSize32u srcRoi);
+
+
+/**
+ * Transposes an image. 64-bit float pixels, single channel
+ * \see nppiStTranspose_32u_C1R
+ */
+NCV_EXPORTS
+NCVStatus nppiStTranspose_64f_C1R(Ncv64f *d_src, Ncv32u srcStride,
+ Ncv64f *d_dst, Ncv32u dstStride, NcvSize32u srcRoi);
+
+
+/**
+ * Transposes an image. 128-bit pixels of any type, single channel
+ * \see nppiStTranspose_32u_C1R
+ */
+NCV_EXPORTS
+NCVStatus nppiStTranspose_128_C1R(void *d_src, Ncv32u srcStep,
+ void *d_dst, Ncv32u dstStep, NcvSize32u srcRoi);
+
+
+/**
+ * Transposes an image. 32-bit unsigned pixels, single channel. Host implementation
+ *
+ * \param h_src [IN] Source image pointer (Host or pinned memory)
+ * \param srcStride [IN] Source image line step
+ * \param h_dst [OUT] Destination image pointer (Host or pinned memory)
+ * \param dstStride [IN] Destination image line step
+ * \param srcRoi [IN] Region of interest of the source image
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStTranspose_32u_C1R_host(Ncv32u *h_src, Ncv32u srcStride,
+ Ncv32u *h_dst, Ncv32u dstStride, NcvSize32u srcRoi);
+
+
+/**
+ * Transposes an image. 32-bit signed pixels, single channel. Host implementation
+ * \see nppiStTranspose_32u_C1R_host
+ */
+NCV_EXPORTS
+NCVStatus nppiStTranspose_32s_C1R_host(Ncv32s *h_src, Ncv32u srcStride,
+ Ncv32s *h_dst, Ncv32u dstStride, NcvSize32u srcRoi);
+
+
+/**
+ * Transposes an image. 32-bit float pixels, single channel. Host implementation
+ * \see nppiStTranspose_32u_C1R_host
+ */
+NCV_EXPORTS
+NCVStatus nppiStTranspose_32f_C1R_host(Ncv32f *h_src, Ncv32u srcStride,
+ Ncv32f *h_dst, Ncv32u dstStride, NcvSize32u srcRoi);
+
+
+/**
+ * Transposes an image. 64-bit unsigned pixels, single channel. Host implementation
+ * \see nppiStTranspose_32u_C1R_host
+ */
+NCV_EXPORTS
+NCVStatus nppiStTranspose_64u_C1R_host(Ncv64u *h_src, Ncv32u srcStride,
+ Ncv64u *h_dst, Ncv32u dstStride, NcvSize32u srcRoi);
+
+
+/**
+ * Transposes an image. 64-bit signed pixels, single channel. Host implementation
+ * \see nppiStTranspose_32u_C1R_host
+ */
+NCV_EXPORTS
+NCVStatus nppiStTranspose_64s_C1R_host(Ncv64s *h_src, Ncv32u srcStride,
+ Ncv64s *h_dst, Ncv32u dstStride, NcvSize32u srcRoi);
+
+
+/**
+ * Transposes an image. 64-bit float pixels, single channel. Host implementation
+ * \see nppiStTranspose_32u_C1R_host
+ */
+NCV_EXPORTS
+NCVStatus nppiStTranspose_64f_C1R_host(Ncv64f *h_src, Ncv32u srcStride,
+ Ncv64f *h_dst, Ncv32u dstStride, NcvSize32u srcRoi);
+
+
+/**
+ * Transposes an image. 128-bit pixels of any type, single channel. Host implementation
+ * \see nppiStTranspose_32u_C1R_host
+ */
+NCV_EXPORTS
+NCVStatus nppiStTranspose_128_C1R_host(void *d_src, Ncv32u srcStep,
+ void *d_dst, Ncv32u dstStep, NcvSize32u srcRoi);
+
+
+/**
+ * Calculates the size of the temporary buffer for integral image creation
+ *
+ * \param roiSize [IN] Size of the input image
+ * \param pBufsize [OUT] Pointer to host variable that returns the size of the temporary buffer (in bytes)
+ * \param devProp [IN] CUDA device properties structure, containing texture alignment information
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStIntegralGetSize_8u32u(NcvSize32u roiSize, Ncv32u *pBufsize, cudaDeviceProp &devProp);
+
+
+/**
+ * Calculates the size of the temporary buffer for integral image creation
+ * \see nppiStIntegralGetSize_8u32u
+ */
+NCV_EXPORTS
+NCVStatus nppiStIntegralGetSize_32f32f(NcvSize32u roiSize, Ncv32u *pBufsize, cudaDeviceProp &devProp);
+
+
+/**
+ * Creates an integral image representation for the input image
+ *
+ * \param d_src [IN] Source image pointer (CUDA device memory)
+ * \param srcStep [IN] Source image line step
+ * \param d_dst [OUT] Destination integral image pointer (CUDA device memory)
+ * \param dstStep [IN] Destination image line step
+ * \param roiSize [IN] Region of interest of the source image
+ * \param pBuffer [IN] Pointer to the pre-allocated temporary buffer (CUDA device memory)
+ * \param bufSize [IN] Size of the pBuffer in bytes
+ * \param devProp [IN] CUDA device properties structure, containing texture alignment information
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStIntegral_8u32u_C1R(Ncv8u *d_src, Ncv32u srcStep,
+ Ncv32u *d_dst, Ncv32u dstStep, NcvSize32u roiSize,
+ Ncv8u *pBuffer, Ncv32u bufSize, cudaDeviceProp &devProp);
+
+
+/**
+ * Creates an integral image representation for the input image
+ * \see nppiStIntegral_8u32u_C1R
+ */
+NCV_EXPORTS
+NCVStatus nppiStIntegral_32f32f_C1R(Ncv32f *d_src, Ncv32u srcStep,
+ Ncv32f *d_dst, Ncv32u dstStep, NcvSize32u roiSize,
+ Ncv8u *pBuffer, Ncv32u bufSize, cudaDeviceProp &devProp);
+
+
+/**
+ * Creates an integral image representation for the input image. Host implementation
+ *
+ * \param h_src [IN] Source image pointer (Host or pinned memory)
+ * \param srcStep [IN] Source image line step
+ * \param h_dst [OUT] Destination integral image pointer (Host or pinned memory)
+ * \param dstStep [IN] Destination image line step
+ * \param roiSize [IN] Region of interest of the source image
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStIntegral_8u32u_C1R_host(Ncv8u *h_src, Ncv32u srcStep,
+ Ncv32u *h_dst, Ncv32u dstStep, NcvSize32u roiSize);
+
+
+/**
+ * Creates an integral image representation for the input image. Host implementation
+ * \see nppiStIntegral_8u32u_C1R_host
+ */
+NCV_EXPORTS
+NCVStatus nppiStIntegral_32f32f_C1R_host(Ncv32f *h_src, Ncv32u srcStep,
+ Ncv32f *h_dst, Ncv32u dstStep, NcvSize32u roiSize);
+
+
+/**
+ * Calculates the size of the temporary buffer for squared integral image creation
+ *
+ * \param roiSize [IN] Size of the input image
+ * \param pBufsize [OUT] Pointer to host variable that returns the size of the temporary buffer (in bytes)
+ * \param devProp [IN] CUDA device properties structure, containing texture alignment information
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStSqrIntegralGetSize_8u64u(NcvSize32u roiSize, Ncv32u *pBufsize, cudaDeviceProp &devProp);
+
+
+/**
+ * Creates a squared integral image representation for the input image
+ *
+ * \param d_src [IN] Source image pointer (CUDA device memory)
+ * \param srcStep [IN] Source image line step
+ * \param d_dst [OUT] Destination squared integral image pointer (CUDA device memory)
+ * \param dstStep [IN] Destination image line step
+ * \param roiSize [IN] Region of interest of the source image
+ * \param pBuffer [IN] Pointer to the pre-allocated temporary buffer (CUDA device memory)
+ * \param bufSize [IN] Size of the pBuffer in bytes
+ * \param devProp [IN] CUDA device properties structure, containing texture alignment information
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStSqrIntegral_8u64u_C1R(Ncv8u *d_src, Ncv32u srcStep,
+ Ncv64u *d_dst, Ncv32u dstStep, NcvSize32u roiSize,
+ Ncv8u *pBuffer, Ncv32u bufSize, cudaDeviceProp &devProp);
+
+
+/**
+ * Creates a squared integral image representation for the input image. Host implementation
+ *
+ * \param h_src [IN] Source image pointer (Host or pinned memory)
+ * \param srcStep [IN] Source image line step
+ * \param h_dst [OUT] Destination squared integral image pointer (Host or pinned memory)
+ * \param dstStep [IN] Destination image line step
+ * \param roiSize [IN] Region of interest of the source image
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppiStSqrIntegral_8u64u_C1R_host(Ncv8u *h_src, Ncv32u srcStep,
+ Ncv64u *h_dst, Ncv32u dstStep, NcvSize32u roiSize);
+
+
+/*@}*/
+
+
+/** \defgroup npps NPPST Signal Processing
+* @{
+*/
+
+
+/**
+ * Calculates the size of the temporary buffer for vector compaction. 32-bit unsigned values
+ *
+ * \param srcLen [IN] Length of the input vector in elements
+ * \param pBufsize [OUT] Pointer to host variable that returns the size of the temporary buffer (in bytes)
+ * \param devProp [IN] CUDA device properties structure, containing texture alignment information
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppsStCompactGetSize_32u(Ncv32u srcLen, Ncv32u *pBufsize, cudaDeviceProp &devProp);
+
+
+/**
+ * Calculates the size of the temporary buffer for vector compaction. 32-bit signed values
+ * \see nppsStCompactGetSize_32u
+ */
+NCVStatus nppsStCompactGetSize_32s(Ncv32u srcLen, Ncv32u *pBufsize, cudaDeviceProp &devProp);
+
+
+/**
+ * Calculates the size of the temporary buffer for vector compaction. 32-bit float values
+ * \see nppsStCompactGetSize_32u
+ */
+NCVStatus nppsStCompactGetSize_32f(Ncv32u srcLen, Ncv32u *pBufsize, cudaDeviceProp &devProp);
+
+
+/**
+ * Compacts the input vector by removing elements of specified value. 32-bit unsigned values
+ *
+ * \param d_src [IN] Source vector pointer (CUDA device memory)
+ * \param srcLen [IN] Source vector length
+ * \param d_dst [OUT] Destination vector pointer (CUDA device memory)
+ * \param p_dstLen [OUT] Pointer to the destination vector length (Pinned memory or NULL)
+ * \param elemRemove [IN] The value to be removed
+ * \param pBuffer [IN] Pointer to the pre-allocated temporary buffer (CUDA device memory)
+ * \param bufSize [IN] Size of the pBuffer in bytes
+ * \param devProp [IN] CUDA device properties structure, containing texture alignment information
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppsStCompact_32u(Ncv32u *d_src, Ncv32u srcLen,
+ Ncv32u *d_dst, Ncv32u *p_dstLen,
+ Ncv32u elemRemove, Ncv8u *pBuffer,
+ Ncv32u bufSize, cudaDeviceProp &devProp);
+
+
+/**
+ * Compacts the input vector by removing elements of specified value. 32-bit signed values
+ * \see nppsStCompact_32u
+ */
+NCV_EXPORTS
+NCVStatus nppsStCompact_32s(Ncv32s *d_src, Ncv32u srcLen,
+ Ncv32s *d_dst, Ncv32u *p_dstLen,
+ Ncv32s elemRemove, Ncv8u *pBuffer,
+ Ncv32u bufSize, cudaDeviceProp &devProp);
+
+
+/**
+ * Compacts the input vector by removing elements of specified value. 32-bit float values
+ * \see nppsStCompact_32u
+ */
+NCV_EXPORTS
+NCVStatus nppsStCompact_32f(Ncv32f *d_src, Ncv32u srcLen,
+ Ncv32f *d_dst, Ncv32u *p_dstLen,
+ Ncv32f elemRemove, Ncv8u *pBuffer,
+ Ncv32u bufSize, cudaDeviceProp &devProp);
+
+
+/**
+ * Compacts the input vector by removing elements of specified value. 32-bit unsigned values. Host implementation
+ *
+ * \param h_src [IN] Source vector pointer (CUDA device memory)
+ * \param srcLen [IN] Source vector length
+ * \param h_dst [OUT] Destination vector pointer (CUDA device memory)
+ * \param dstLen [OUT] Pointer to the destination vector length (can be NULL)
+ * \param elemRemove [IN] The value to be removed
+ *
+ * \return NCV status code
+ */
+NCV_EXPORTS
+NCVStatus nppsStCompact_32u_host(Ncv32u *h_src, Ncv32u srcLen,
+ Ncv32u *h_dst, Ncv32u *dstLen, Ncv32u elemRemove);
+
+
+/**
+ * Compacts the input vector by removing elements of specified value. 32-bit signed values. Host implementation
+ * \see nppsStCompact_32u_host
+ */
+NCV_EXPORTS
+NCVStatus nppsStCompact_32s_host(Ncv32s *h_src, Ncv32u srcLen,
+ Ncv32s *h_dst, Ncv32u *dstLen, Ncv32s elemRemove);
+
+
+/**
+ * Compacts the input vector by removing elements of specified value. 32-bit float values. Host implementation
+ * \see nppsStCompact_32u_host
+ */
+NCV_EXPORTS
+NCVStatus nppsStCompact_32f_host(Ncv32f *h_src, Ncv32u srcLen,
+ Ncv32f *h_dst, Ncv32u *dstLen, Ncv32f elemRemove);
+
+
+/*@}*/
+
+
+#endif // _npp_staging_hpp_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * @authors: Cedric Cagniart, Koen Buys, Anatoly Baksheev
+ *
+ */
+
+#include <pcl/gpu/people/tree.h>
+#include <pcl/gpu/people/label_common.h>
+#include <pcl/gpu/utils/device/limits.hpp>
+#include <pcl/gpu/utils/safe_call.hpp>
+#include <pcl/gpu/utils/texture_binder.hpp>
+#include <stdio.h>
+#include <limits>
+#include <assert.h>
+#include "internal.h"
+
+using pcl::gpu::people::trees::Node;
+using pcl::gpu::people::trees::Label;
+using pcl::gpu::people::trees::AttribLocation;
+using pcl::gpu::people::trees::Attrib;
+using pcl::gpu::people::trees::focal;
+using pcl::gpu::people::trees::NUM_LABELS;
+
+using namespace std;
+typedef unsigned int uint;
+
+#ifdef __CDT_PARSER__ // This is an eclipse specific hack, does nothing to the code
+#define __global__
+#define __device__
+#define __shared__
+#define __forceinline__
+#define __constant__
+#define __float2int_rn
+#endif
+
+namespace pcl
+{
+ namespace device
+ {
+ /**
+ * \brief This combines two probabilities into a single according to their weight = p(d|{I,x})
+ **/
+ __global__ void
+ KernelCUDA_CombineProb (PtrStepSz<prob_histogram> probIn1,
+ float weight1,
+ PtrStepSz<prob_histogram> probIn2,
+ float weight2,
+ PtrStepSz<prob_histogram> probOut)
+ {
+ // map block and thread onto image coordinates
+ int u = blockIdx.x * blockDim.x + threadIdx.x;
+ int v = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if( u >= probIn1.cols || v >= probIn1.rows )
+ return;
+
+ for(int l = 0; l < NUM_LABELS; ++l)
+ {
+ // TODO: replace this with a histogram copy
+ probOut.ptr(v)[u].probs[l] = weight1 * probIn1.ptr(v)[u].probs[l] + weight2 * probIn2.ptr(v)[u].probs[l];
+ }
+ }
+
+ /**
+ * \brief This sums a probabilities into a another according to its weight = p(d|{I,x})
+ **/
+ __global__ void
+ KernelCUDA_WeightedSumProb (PtrStepSz<prob_histogram> probIn,
+ float weight,
+ PtrStepSz<prob_histogram> probOut)
+ {
+ // map block and thread onto image coordinates
+ int u = blockIdx.x * blockDim.x + threadIdx.x;
+ int v = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if( u >= probIn.cols || v >= probIn.rows )
+ return;
+
+ for(int l = 0; l < NUM_LABELS; ++l)
+ {
+ // TODO: replace this with a histogram copy
+ probOut.ptr(v)[u].probs[l] += weight * probIn.ptr(v)[u].probs[l];
+ }
+ }
+
+ /** \brief This merges the histogram of probabilities into a final label **/
+ __global__ void
+ KernelCUDA_SelectLabel (PtrStepSz<Label> labels, PtrStepSz<prob_histogram> Prob)
+ {
+ // map block and thread onto image coordinates
+ int u = blockIdx.x * blockDim.x + threadIdx.x;
+ int v = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if( u >= labels.cols || v >= labels.rows )
+ return;
+
+ float maxValue = 0;
+ int maxID = 31; // 31 equals to NOLABEL in label_common.h for some reason not resolved here
+ prob_histogram p = Prob.ptr(v)[u];
+ for(int l = 0; l < NUM_LABELS; ++l)
+ {
+ // TODO: problem is that this one doesn't handle ties very well
+ if(maxValue < p.probs[l])
+ {
+ maxValue = p.probs[l];
+ maxID = l;
+ }
+ if(maxValue == p.probs[l])
+ {
+ //DAMN WE HAVE A TIE
+ //TODO: solve this
+
+ //Workflow
+ // 1) test if this is actually the largest value in the histogram
+
+ // 2a) if not take the other one and continue
+
+ // 2b) if it is, take the 1 neighbourhood
+
+ }
+ }
+ labels.ptr(v)[u] = maxID;
+ }
+
+ /**
+ * \brief Does Gaussian Blur in the horizontal row direction
+ * \param[in] kernelSize needs to be odd! This should be fetched in the calling function before calling this method
+ * TODO: replace this with OpenCV or NPP implementation
+ **/
+ __global__ void
+ KernelCUDA_GaussianBlurHor (PtrStepSz<prob_histogram> probIn,
+ const float* kernel,
+ const int kernelSize,
+ PtrStepSz<prob_histogram> probOut)
+ {
+ // map block and thread onto image coordinates a single pixel for each thread
+ int u = blockIdx.x * blockDim.x + threadIdx.x;
+ int v = blockIdx.y * blockDim.y + threadIdx.y;
+
+ // Skip when outside the image
+ if( u >= probIn.cols || v >= probIn.rows )
+ return;
+
+ // Do this for all the labels of this pixel
+ for(int l = 0; l< NUM_LABELS; l++)
+ {
+ float sum = 0; // This contains the kernel convolution
+ int j = 0; // This contains the offset in the kernel
+
+ // KernelSize needs to be odd! This should be fetched in the calling function before calling this method
+ for(int i = -__float2int_rn(kernelSize/2); i < __float2int_rn(kernelSize/2); i++)
+ {
+ // check if index goes outside image, pixels are skipped
+ if((u+i) < 0 || (u+i) > probIn.cols)
+ {
+ j++; // skip to the next point
+ }
+ else
+ {
+ //int k = u+i;
+
+ // This line fails, why??
+ sum += probIn.ptr(v)[u+i].probs[l] * kernel[j];
+ j++;
+ }
+ }
+ probOut.ptr(v)[u].probs[l] = sum;
+ //probOut.ptr(v)[u].probs[l] = probIn.ptr(v)[u].probs[l];
+ }
+ }
+
+ /**
+ * \brief Does Gaussian Blur in the horizontal row direction
+ * \param[in] kernelSize needs to be odd! This should be fetched in the calling function before calling this method
+ * TODO: replace this with OpenCV or NPP implementation
+ *
+ **/
+ __global__ void
+ KernelCUDA_GaussianBlurVer (PtrStepSz<prob_histogram> probIn,
+ const float* kernel,
+ const int kernelSize,
+ PtrStepSz<prob_histogram> probOut)
+ {
+ // map block and thread onto image coordinates a single pixel for each thread
+ int u = blockIdx.x * blockDim.x + threadIdx.x;
+ int v = blockIdx.y * blockDim.y + threadIdx.y;
+
+ // Skip when outside the image
+ if( u >= probIn.cols || v >= probIn.rows )
+ return;
+
+ // Do this for all the labels of this pixel
+ for(int l = 0; l< NUM_LABELS; l++)
+ {
+ float sum = 0; // This contains the kernel convolution
+ int j = 0; // This contains the offset in the kernel
+
+ // KernelSize needs to be odd! This should be fetched in the calling function before calling this method
+ for(int i = -__float2int_rn(kernelSize/2); i < __float2int_rn(kernelSize/2); i++)
+ {
+ // check if index goes outside image, pixels are skipped
+ if((v+i) < 0 || (v+i) > probIn.rows)
+ {
+ j++; // skip to the next point
+ }
+ else
+ {
+ sum += probIn.ptr(v+i)[u].probs[l] * kernel[j];
+ j++;
+ }
+ }
+ probOut.ptr(v)[u].probs[l] = sum;
+ }
+ }
+
+ /** \brief This will merge the votes from the different trees into one final vote, including probabilistic's **/
+ void
+ ProbabilityProc::CUDA_SelectLabel ( const Depth& depth,
+ Labels& labels,
+ LabelProbability& probabilities)
+ {
+ std::cout << "[pcl::device::ProbabilityProc::CUDA_SelectLabel] : (I) : Called" << std::endl;
+ //labels.create(depth.rows(), depth.cols());
+ //probabilities.create(depth.rows(), depth.cols());
+
+ dim3 block(32, 8);
+ dim3 grid(divUp(depth.cols(), block.x), divUp(depth.rows(), block.y) );
+
+ KernelCUDA_SelectLabel<<< grid, block >>>( labels, probabilities );
+
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaThreadSynchronize() );
+ }
+
+ /** \brief This will combine two probabilities according their weight **/
+ void
+ ProbabilityProc::CUDA_CombineProb ( const Depth& depth,
+ LabelProbability& probIn1,
+ float weight1,
+ LabelProbability& probIn2,
+ float weight2,
+ LabelProbability& probOut)
+ {
+ dim3 block(32, 8);
+ dim3 grid(divUp(depth.cols(), block.x), divUp(depth.rows(), block.y) );
+
+ // CUDA kernel call
+ KernelCUDA_CombineProb<<< grid, block >>>( probIn1, weight1, probIn2, weight2, probOut );
+
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaThreadSynchronize() );
+ }
+
+ /** \brief This will combine two probabilities according their weight **/
+ void
+ ProbabilityProc::CUDA_WeightedSumProb ( const Depth& depth,
+ LabelProbability& probIn,
+ float weight,
+ LabelProbability& probOut)
+ {
+ dim3 block(32, 8);
+ dim3 grid(divUp(depth.cols(), block.x), divUp(depth.rows(), block.y) );
+
+ // CUDA kernel call
+ KernelCUDA_WeightedSumProb<<< grid, block >>>( probIn, weight, probOut );
+
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaThreadSynchronize() );
+ }
+
+ /** \brief This will blur the input labelprobability with the given kernel **/
+ int
+ ProbabilityProc::CUDA_GaussianBlur( const Depth& depth,
+ LabelProbability& probIn,
+ DeviceArray<float>& kernel,
+ LabelProbability& probOut)
+ {
+ // Allocate the memory
+ LabelProbability probTemp(depth.rows(), depth.cols());
+ // Call the method
+ return CUDA_GaussianBlur(depth, probIn, kernel, probTemp, probOut);
+ }
+
+ /** \brief This will blur the input labelprobability with the given kernel, this version avoids extended allocation **/
+ int
+ ProbabilityProc::CUDA_GaussianBlur( const Depth& depth,
+ LabelProbability& probIn,
+ DeviceArray<float>& kernel,
+ LabelProbability& probTemp,
+ LabelProbability& probOut)
+ {
+ dim3 block(32, 8);
+ dim3 grid(divUp(depth.cols(), block.x), divUp(depth.rows(), block.y) );
+
+ if(kernel.size()/sizeof(float) % 2 == 0) //kernelSize is even, should be odd
+ return -1;
+
+ std::cout << "[pcl::device::ProbabilityProc::CUDA_GaussianBlur] : (I) : called c: " << probIn.cols() << " r: " << probIn.rows() << std::endl;
+ //PCL_INFO("[pcl::device::ProbabilityProc::CUDA_GaussianBlur] : (I) : called c: %d r: %d\n", probIn.cols(), probIn.rows());
+
+ // CUDA kernel call Vertical
+ KernelCUDA_GaussianBlurVer<<< grid, block >>>( probIn, kernel, kernel.size(), probTemp );
+ //KernelCUDA_GaussianBlurVer<<< grid, block >>>( probIn, kernel, kernel.size(), probOut );
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaThreadSynchronize() );
+
+ // CUDA kernel call Horizontal
+ KernelCUDA_GaussianBlurHor<<< grid, block >>>( probTemp, kernel, kernel.size(), probOut );
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaThreadSynchronize() );
+ return 1;
+ }
+ }
+}
+
--- /dev/null
+
+#if 0
+
+/*
+* Software License Agreement (BSD License)
+*
+* Point Cloud Library (PCL) - www.pointclouds.org
+* Copyright (c) 2010-2012, Willow Garage, Inc.
+*
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* $Id: $
+* @authors: Anatoly Baskheev
+*
+*/
+
+#include "internal.h"
+#include <pcl/gpu/utils/device/funcattrib.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+ __device__ __forceinline__ float3 operator*(const Intr& intr, const float3 p)
+ {
+ float3 r;
+ r.x = intr.fx * p.x + intr.cx * p.z;
+ r.y = intr.fy * p.y + intr.cy * p.z;
+ r.z = p.z;
+ return r;
+ }
+
+ __device__ __forceinline__
+ void getProjectedRadiusSearchBox (int rows, int cols, const device::Intr& intr, const float3& point, float squared_radius,
+ int &minX, int &maxX, int &minY, int &maxY)
+ {
+ int min, max;
+
+ float3 q = intr * point;
+
+ // http://www.wolframalpha.com/input/?i=K+%3D+%7B%7Ba%2C+0%2C+b%7D%2C+%7B0%2C+c%2C+d%7D%2C+%7B0%2C+0%2C+1%7D%7D%2C+matrix%5BK+.+transpose%5BK%5D%5D
+
+ float coeff8 = 1; //K_KT_.coeff (8);
+ float coeff7 = intr.cy; //K_KT_.coeff (7);
+ float coeff4 = intr.fy * intr.fy + intr.cy*intr.cy; //K_KT_.coeff (4);
+
+ float coeff6 = intr.cx; //K_KT_.coeff (6);
+ float coeff0 = intr.fx * intr.fx + intr.cx*intr.cx; //K_KT_.coeff (0);
+
+ float a = squared_radius * coeff8 - q.z * q.z;
+ float b = squared_radius * coeff7 - q.y * q.z;
+ float c = squared_radius * coeff4 - q.y * q.y;
+
+ // a and c are multiplied by two already => - 4ac -> - ac
+ float det = b * b - a * c;
+
+ if (det < 0)
+ {
+ minY = 0;
+ maxY = rows - 1;
+ }
+ else
+ {
+ float y1 = (b - sqrt (det)) / a;
+ float y2 = (b + sqrt (det)) / a;
+
+ min = std::min (static_cast<int> (floor (y1)), static_cast<int> (floor (y2)));
+ max = std::max (static_cast<int> (ceil (y1)), static_cast<int> (ceil (y2)));
+ minY = std::min (rows - 1, std::max (0, min));
+ maxY = std::max (std::min (rows - 1, max), 0);
+ }
+
+ b = squared_radius * coeff6 - q.x * q.z;
+ c = squared_radius * coeff0 - q.x * q.x;
+
+ det = b * b - a * c;
+ if (det < 0)
+ {
+ minX = 0;
+ maxX = cols - 1;
+ }
+ else
+ {
+ float x1 = (b - sqrt (det)) / a;
+ float x2 = (b + sqrt (det)) / a;
+
+ min = std::min (static_cast<int> (floor (x1)), static_cast<int> (floor (x2)));
+ max = std::max (static_cast<int> (ceil (x1)), static_cast<int> (ceil (x2)));
+ minX = std::min (cols- 1, std::max (0, min));
+ maxX = std::max (std::min (cols - 1, max), 0);
+ }
+ }
+
+ struct Shs
+ {
+ PtrSz<int> indices;
+ PtrStepSz<float8> cloud;
+ Intr intr;
+ float radius;
+
+ mutable PtrStep<unsigned char> output_mask;
+
+
+ __device__ __forceinline__ void operator()() const
+ {
+
+ }
+ };
+ }
+}
+
+void pcl::device::shs(const DeviceArray2D<float4> &cloud, float tolerance/*radius*/, const std::vector<int>& indices_in, float delta_hue, Mask& output)
+{
+ int cols = cloud.cols();
+ int rows = cloud.rows();
+
+ output.create(rows, cols);
+ device::setZero(output);
+
+ DeviceArray<int> indices_device;
+ indices_device.upload(indices_in);
+}
+
+#if 0
+
+void optimized_shs5(const PointCloud<PointXYZRGB> &cloud, float tolerance, const PointIndices &indices_in, cv::Mat flowermat, float delta_hue)
+{
+ int rows = 480;
+ int cols = 640;
+ device::Intr intr(525, 525, cols/2-0.5f, rows/2-0.5f);
+
+ //FILE *f = fopen("log.txt", "w");
+
+ cv::Mat huebuf(cloud.height, cloud.width, CV_32F);
+ float *hue = huebuf.ptr<float>();
+
+ for(size_t i = 0; i < cloud.points.size(); ++i)
+ {
+ PointXYZHSV h;
+ PointXYZRGB p = cloud.points[i];
+ PointXYZRGBtoXYZHSV(p, h);
+ hue[i] = h.h;
+ }
+ unsigned char *mask = flowermat.ptr<unsigned char>();
+
+
+ SearchD search;
+ search.setInputCloud(cloud.makeShared());
+
+ vector< vector<int> > storage(100);
+
+ // omp_set_num_threads(1);
+ // Process all points in the indices vector
+ //#pragma omp parallel for
+ for (int k = 0; k < static_cast<int> (indices_in.indices.size ()); ++k)
+ {
+ int i = indices_in.indices[k];
+ if (mask[i])
+ continue;
+
+ mask[i] = 255;
+
+ // int id = omp_get_thread_num();
+ //std::vector<int>& seed_queue = storage[id];
+ std::vector<int> seed_queue;
+ seed_queue.clear();
+ seed_queue.reserve(cloud.size());
+ int sq_idx = 0;
+ seed_queue.push_back (i);
+
+ PointXYZRGB p = cloud.points[i];
+ float h = hue[i];
+
+ while (sq_idx < (int)seed_queue.size ())
+ {
+ int index = seed_queue[sq_idx];
+ const PointXYZRGB& q = cloud.points[index];
+
+ if(!isFinite (q))
+ continue;
+
+ // search window
+ double squared_radius = tolerance * tolerance;
+ //unsigned int left, right, top, bottom;
+ //search.getProjectedRadiusSearchBox (q, squared_radius, left, right, top, bottom);
+
+ int left, right, top, bottom;
+ getProjectedRadiusSearchBox(rows, cols, intr, q, squared_radius, left, right, top, bottom);
+
+ //fprintf(f, "%d) %d %d %d %d\n", index, left, right, top, bottom);
+
+
+ int yEnd = (bottom + 1) * cloud.width + right + 1;
+ int idx = top * cloud.width + left;
+ int skip = cloud.width - right + left - 1;
+ int xEnd = idx - left + right + 1;
+
+ for (; xEnd != yEnd; idx += skip, xEnd += cloud.width)
+ {
+ for (; idx < xEnd; ++idx)
+ {
+ if (mask[idx])
+ continue;
+
+ if (sqnorm(cloud.points[idx], q) <= squared_radius)
+ {
+ float h_l = hue[idx];
+
+ if (fabs(h_l - h) < delta_hue)
+ {
+ if(idx & 1)
+ seed_queue.push_back (idx);
+ mask[idx] = 255;
+ }
+
+ }
+ }
+ }
+ sq_idx++;
+
+ }
+ }
+}
+
+#endif
+
+
+#endif
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * @authors: Anatoly Baskheev
+ *
+ */
+
+#include "internal.h"
+#include <pcl/gpu/utils/device/funcattrib.hpp>
+#include <exception>
+
+namespace pcl
+{
+ namespace device
+ {
+ template<int N>
+ __global__ void smoothKernel(const PtrStep<unsigned char> src, const PtrStep<unsigned short> depth, PtrStepSz<unsigned char> dst,
+ int patch, int depthThres, int num_parts)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ int patch2 = patch/2;
+ if (x >= (dst.cols - patch2*2) || y >= (dst.rows - patch2*2))
+ return;
+
+ int depth_center = depth.ptr(y + patch2)[x+patch2];
+
+ unsigned int hist4[(N + 4 - 1) / 4];
+
+ #pragma unroll
+ for(int i = 0; i < sizeof(hist4)/sizeof(hist4[0]); ++i)
+ hist4[i] = 0;
+
+ for (int dy = 0; dy < patch; ++dy)
+ {
+ const unsigned short* d = depth.ptr(dy + y);
+ const unsigned char * s = src.ptr(dy + y);
+
+ for (int dx = 0; dx < patch; ++dx)
+ {
+ if (abs(d[dx+x] - depth_center) < depthThres)
+ {
+ int l = s[dx+x];
+ if (l < num_parts)
+ {
+ int bin = l / sizeof(int);
+ int off = l - bin * sizeof(int);
+
+ hist4[bin] += (unsigned)(1 << 8*off);
+ }
+ }
+ }
+ }
+
+ int max_label = src.ptr(y + patch2)[x+patch2];
+ int max_value = 0;
+
+ #pragma unroll
+ for(int i = 0; i < sizeof(hist4)/sizeof(hist4[0]); ++i)
+ {
+ int bin = hist4[i];
+ int val;
+
+ val = (bin >> 0) & 0xFF;
+ if (max_value < val)
+ {
+ max_value = val;
+ max_label = sizeof(int)*i+0;
+ }
+
+ val = (bin >> 8) & 0xFF;
+ if (max_value < val)
+ {
+ max_value = val;
+ max_label = sizeof(int)*i+1;
+ }
+
+ val = (bin >> 16) & 0xFF;
+ if (max_value < val)
+ {
+ max_value = val;
+ max_label = sizeof(int)*i+2;
+ }
+
+ val = (bin >> 24) & 0xFF;
+ if (max_value < val)
+ {
+ max_value = val;
+ max_label = sizeof(int)*i+3;
+ }
+ }
+
+ dst.ptr(y + patch2)[x+patch2] = max_label;
+ }
+ }
+}
+
+void pcl::device::smoothLabelImage(const Labels& src, const Depth& depth, Labels& dst, int num_parts, int patch_size, int depthThres)
+{
+ dst.create(src.rows(), src.cols());
+
+ dim3 block(32, 8);
+ dim3 grid(divUp(src.cols(), block.x), divUp(src.rows(), block.y));
+
+ if (num_parts <= 28)
+ pcl::device::smoothKernel<28><<<grid, block>>>(src, depth, dst, patch_size, depthThres, num_parts);
+ else
+ if (num_parts <= 32)
+ pcl::device::smoothKernel<32><<<grid, block>>>(src, depth, dst, patch_size, depthThres, num_parts);
+ else
+ throw std::exception(); //should instanciate another smoothKernel<N>
+
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
+
+
+
--- /dev/null
+#include "internal.h"
+#include <pcl/gpu/utils/safe_call.hpp>
+#include <pcl/gpu/utils/texture_binder.hpp>
+#include <pcl/gpu/utils/device/limits.hpp>
+#include "npp.h"
+
+#include <stdio.h>
+
+namespace pcl
+{
+ namespace device
+ {
+ texture<uchar4, cudaTextureType1D, cudaReadModeElementType> cmapTex;
+
+ __global__ void colorKernel(const PtrStepSz<unsigned char> labels, PtrStep<uchar4> output)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x < labels.cols && y < labels.rows)
+ {
+ int l = labels.ptr(y)[x];
+ output.ptr(y)[x] = tex1Dfetch(cmapTex, l);
+ }
+ }
+
+ __global__ void mixedColorKernel(const PtrStepSz<unsigned char> labels, PtrStepSz<uchar4> rgba, PtrStep<uchar4> output)
+ {
+ int x = threadIdx.x + blockIdx.x * blockDim.x;
+ int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+ if (x < labels.cols && y < labels.rows)
+ {
+ uchar4 c = rgba.ptr(y)[x];
+
+ int l = labels.ptr(y)[x];
+ if (l != 8) // RHip but should be background
+ c = tex1Dfetch(cmapTex, l);
+
+ output.ptr(y)[x] = c;
+ }
+ }
+ }
+}
+
+void pcl::device::colorLMap(const Labels& labels, const DeviceArray<uchar4>& map, Image& rgba)
+{
+ cmapTex.addressMode[0] = cudaAddressModeClamp;
+ TextureBinder binder(map, cmapTex);
+
+ dim3 block(32, 8);
+ dim3 grid( divUp(labels.cols(), block.x), divUp(labels.rows(), block.y) );
+
+ colorKernel<<< grid, block >>>( labels, rgba );
+
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaThreadSynchronize() );
+}
+
+void pcl::device::mixedColorMap(const Labels& labels, const DeviceArray<uchar4>& map, const Image& rgba, Image& output)
+{
+ cmapTex.addressMode[0] = cudaAddressModeClamp;
+ TextureBinder binder(map, cmapTex);
+
+ dim3 block(32, 8);
+ dim3 grid(divUp(labels.cols(), block.x), divUp(labels.rows(), block.y));
+
+ mixedColorKernel<<<grid, block>>>(labels, rgba, output);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+/// TODO implement getError string for NPP and move this to the same place with cudaSafeCall
+
+#if defined(__GNUC__)
+ #define nppSafeCall(expr) pcl::gpu::___nppSafeCall(expr, __FILE__, __LINE__, __func__)
+#else /* defined(__CUDACC__) || defined(__MSVC__) */
+ #define nppSafeCall(expr) pcl::gpu::___nppSafeCall(expr, __FILE__, __LINE__)
+#endif
+
+namespace pcl
+{
+ namespace gpu
+ {
+
+ void ___nppSafeCall(int err_code, const char *file, const int line, const char *func = "")
+ {
+ if (err_code < 0)
+ {
+ char buf[4096];
+ sprintf(buf, "NppErrorCode = %d", err_code);
+ error(buf, file, line, func);
+ }
+ }
+ }
+}
+
+
+void pcl::device::setZero(Mask& mask)
+{
+ NppiSize sz;
+ sz.width = mask.cols();
+ sz.height = mask.rows();
+ nppSafeCall( nppiSet_8u_C1R( 0, mask, (int)mask.step(), sz) );
+}
+
+void pcl::device::Dilatation::prepareRect5x5Kernel(DeviceArray<unsigned char>& kernel)
+{
+ if (kernel.size() == KSIZE_X * KSIZE_Y)
+ return;
+
+ std::vector<unsigned char> host(KSIZE_X * KSIZE_Y, (unsigned char)255);
+ kernel.upload(host);
+}
+
+void pcl::device::Dilatation::invoke(const Mask& src, const Kernel& kernel, Mask& dst)
+{
+ dst.create(src.rows(), src.cols());
+ setZero(dst);
+
+ NppiSize sz;
+ sz.width = src.cols() - KSIZE_X;
+ sz.height = src.rows() - KSIZE_Y;
+
+ NppiSize ksz;
+ ksz.width = KSIZE_X;
+ ksz.height = KSIZE_Y;
+
+ NppiPoint anchor;
+ anchor.x = ANCH_X;
+ anchor.y = ANCH_Y;
+
+ // This one uses Nvidia performance primitives
+ nppSafeCall( nppiDilate_8u_C1R(src.ptr(ANCH_Y) + ANCH_X, (int)src.step(),
+ dst.ptr(ANCH_Y) + ANCH_X, (int)dst.step(), sz, kernel, ksz, anchor) );
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+ namespace device
+ {
+ __global__ void fgDepthKernel(const PtrStepSz<unsigned short> depth1, const PtrStep<unsigned char> inv_mask, PtrStep<unsigned short> depth2)
+ {
+ int x = blockIdx.x * blockDim.x + threadIdx.x;
+ int y = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if (x < depth1.cols && y < depth1.rows)
+ {
+ unsigned short d = depth1.ptr(y)[x];
+ depth2.ptr(y)[x] = inv_mask.ptr(y)[x] ? d : numeric_limits<unsigned short>::max();
+ }
+ }
+ }
+}
+
+void pcl::device::prepareForeGroundDepth(const Depth& depth1, Mask& inverse_mask, Depth& depth2)
+{
+ int cols = depth1.cols();
+ int rows = depth1.rows();
+
+ depth2.create(rows, cols);
+
+ dim3 block(32, 8);
+ dim3 grid( divUp(cols, block.x), divUp(rows, block.y) );
+
+ fgDepthKernel<<< grid, block >>>( depth1, inverse_mask, depth2 );
+
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaThreadSynchronize() );
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+/// compute hue functionality
+
+namespace pcl
+{
+ namespace device
+ {
+ __device__ __host__ __forceinline__ float computeHueFunc (int rgba)
+ {
+ int r = (rgba ) & 0xFF;
+ int g = (rgba >> 8) & 0xFF;
+ int b = (rgba >> 16) & 0xFF;
+
+ int v = max (r, max (g, b));
+ float h;
+
+ float div_inv = 1.f / (v - min (r, min (g, b)) );
+
+ if (v == 0)
+ return -1;
+
+ if (r == v)
+ h = ( (g - b)) * div_inv;
+ else if (g == v)
+ h = (2 + (b - r)) * div_inv;
+ else
+ h = (4 + (r - g)) * div_inv;
+
+ h *= 60;
+ if (h < 0)
+ h += 360;
+
+ return h;
+ }
+
+ }
+}
+
+float pcl::device::computeHue(int rgba)
+{
+ return computeHueFunc(rgba);
+}
+
+namespace pcl
+{
+ namespace device
+ {
+ __global__ void computeHueKernel(const PtrStepSz<int> rgba, const PtrStep<unsigned short> depth, PtrStep<float> hue)
+ {
+ int x = blockIdx.x * blockDim.x + threadIdx.x;
+ int y = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if (x < rgba.cols && y < rgba.rows)
+ {
+ const float qnan = numeric_limits<float>::quiet_NaN();
+
+ unsigned short d = depth.ptr(y)[x];
+ hue.ptr(y)[x] = (d == 0) ? qnan : computeHueFunc(rgba.ptr(y)[x]);
+ }
+ }
+ }
+}
+
+
+void pcl::device::computeHueWithNans(const Image& rgba, const Depth& depth, HueImage& hue)
+{
+ hue.create(rgba.rows(), rgba.cols());
+
+ dim3 block(32, 8);
+ dim3 grid;
+
+ grid.x = divUp(rgba.cols(), block.x);
+ grid.y = divUp(rgba.rows(), block.y);
+
+ computeHueKernel<<<grid, block>>>(rgba, depth, hue);
+
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
+
+namespace pcl
+{
+ namespace device
+ {
+ __global__ void reprojectDepthKenrel(const PtrStepSz<unsigned short> depth, const Intr intr, PtrStep<float4> cloud)
+ {
+ int x = blockIdx.x * blockDim.x + threadIdx.x;
+ int y = blockIdx.y * blockDim.y + threadIdx.y;
+
+ const float qnan = numeric_limits<float>::quiet_NaN();
+
+ if (x < depth.cols && y < depth.rows)
+ {
+ float4 p = make_float4(qnan, qnan, qnan, qnan);
+
+ int d = depth.ptr(y)[x];
+ float z = d * 0.001f; // mm -> meters
+
+ p.x = z * (x - intr.cx) / intr.fx;
+ p.y = z * (y - intr.cy) / intr.fy;
+ p.z = z;
+
+ cloud.ptr(y)[x] = p;
+ }
+ }
+ }
+}
+
+void pcl::device::computeCloud(const Depth& depth, const Intr& intr, Cloud& cloud)
+{
+ cloud.create(depth.rows(), depth.cols());
+
+ dim3 block(32, 8);
+ dim3 grid;
+ grid.x = divUp(depth.cols(), block.x);
+ grid.y = divUp(depth.rows(), block.y);
+
+ reprojectDepthKenrel<<<grid, block>>>(depth, intr, cloud);
+
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * @authors: Anatoly Baksheev
+ */
+
+
+#ifndef PCL_GPU_PEOPLE_CUDA_ASYNC_COPY_H_
+#define PCL_GPU_PEOPLE_CUDA_ASYNC_COPY_H_
+
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/utils/safe_call.hpp>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ template<class T>
+ class AsyncCopy
+ {
+ public:
+ AsyncCopy(T* ptr, size_t size) : ptr_(ptr)
+ {
+ cudaSafeCall( cudaHostRegister(ptr_, size, 0) );
+ cudaSafeCall( cudaStreamCreate(&stream_) );
+ }
+
+ AsyncCopy(std::vector<T>& data) : ptr_(&data[0])
+ {
+ cudaSafeCall( cudaHostRegister(ptr_, data.size(), 0) );
+ cudaSafeCall( cudaStreamCreate(&stream_) );
+ }
+
+ ~AsyncCopy()
+ {
+ cudaSafeCall( cudaHostUnregister(ptr_) );
+ cudaSafeCall( cudaStreamDestroy(stream_) );
+ }
+
+ void download(const DeviceArray<T>& arr)
+ {
+ cudaSafeCall( cudaMemcpyAsync(ptr_, arr.ptr(), arr.sizeBytes(), cudaMemcpyDeviceToHost, stream_) );
+ }
+
+ void download(const DeviceArray2D<T>& arr)
+ {
+ cudaSafeCall( cudaMemcpy2DAsync(ptr_, arr.cols(), arr.ptr(), arr.step(), arr.colsBytes(), arr.rows(), cudaMemcpyDeviceToHost, stream_) );
+ }
+
+ void upload(const DeviceArray<T>& arr) const
+ {
+ cudaSafeCall( cudaMemcpyAsync(arr.ptr(), ptr_, arr.size(), cudaMemcpyHostToDevice, stream_) );
+ }
+
+ void upload(const DeviceArray2D<T>& arr) const
+ {
+ cudaSafeCall( cudaMemcpy2DAsync(arr.ptr(), arr.step(), ptr_, arr.cols(), arr.colsBytes(), arr.rows(), cudaMemcpyHostToDevice, stream_) );
+ }
+
+ void waitForCompeltion()
+ {
+ cudaSafeCall( cudaStreamSynchronize(stream_) );
+ }
+ private:
+ cudaStream_t stream_;
+ T* ptr_ ;
+ };
+ }
+
+ namespace device
+ {
+ using pcl::gpu::AsyncCopy;
+ }
+}
+
+#endif /* PCL_GPU_PEOPLE_CUDA_ASYNC_COPY_H_ */
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * @author: Koen Buys
+ */
+
+#include <pcl/gpu/people/face_detector.h>
+#include <pcl/console/print.h>
+#include <pcl/gpu/utils/safe_call.hpp>
+#include <pcl/point_types_conversion.h>
+
+#include <boost/foreach.hpp>
+#include <boost/property_tree/xml_parser.hpp>
+#include <boost/property_tree/ptree.hpp>
+
+#include <cuda_runtime_api.h>
+
+#define NVBIN_HAAR_SIZERESERVED 16
+#define NVBIN_HAAR_VERSION 0x1
+
+#define PCL_ASSERT_NCVSTAT(ncvOp) \
+ do \
+ { \
+ NCVStatus _ncv_return_status = ncvOp; \
+ PCL_ASSERT_ERROR_PRINT_RETURN(NCV_SUCCESS==_ncv_return_status, "ncv_return_status!=NCV_SUCCESS", _ncv_return_status); \
+ } while (0)
+
+#define PCL_ASSERT_CUDA_RETURN(cudacall, errCode) \
+ do \
+ { \
+ cudaError_t res = cudacall; \
+ ncvAssertPrintReturn(cudaSuccess==res, "cudaError_t!=cudaSuccess", errCode); \
+ } while (0)
+
+using boost::property_tree::ptree;
+
+pcl::gpu::people::FaceDetector::FaceDetector(int cols, int rows)
+{
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::FaceDetector] : (D) : Constructor called\n");
+
+ cols_ = cols; rows_ = rows;
+
+ cuda_dev_id_ = 0;
+ cudaSafeCall ( cudaSetDevice (cuda_dev_id_));
+ cudaSafeCall ( cudaGetDeviceProperties (&cuda_dev_prop_, cuda_dev_id_));
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::FaceDetector] : (D) : Using GPU: %d ( %s ), arch= %d . %d\n",cuda_dev_id_, cuda_dev_prop_.name, cuda_dev_prop_.major, cuda_dev_prop_.minor);
+
+}
+
+/**
+ * \brief This loads the Haar description file from a XML file format
+ */
+NCVStatus
+pcl::gpu::people::FaceDetector::loadFromXML2(const std::string &filename,
+ HaarClassifierCascadeDescriptor &haar,
+ std::vector<HaarStage64> &haar_stages,
+ std::vector<HaarClassifierNode128> &haarClassifierNodes,
+ std::vector<HaarFeature64> &haar_features)
+{
+ NCVStatus ncv_return_status; // TODO remove this type
+
+ boost::property_tree::ptree pt;
+
+ haar.NumStages = 0;
+ haar.NumClassifierRootNodes = 0;
+ haar.NumClassifierTotalNodes = 0;
+ haar.NumFeatures = 0;
+ haar.ClassifierSize.width = 0;
+ haar.ClassifierSize.height = 0;
+
+ try // Fetch error loading the file
+ {
+ read_xml(filename,pt);
+ }
+ catch(boost::exception const& exb)
+ {
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : Unable to read filename with boost exception\n");
+ return NCV_HAAR_XML_LOADING_EXCEPTION;
+ }
+ catch (std::exception const& ex)
+ {
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : Unable to read filename with exception %s\n", ex.what());
+ return NCV_HAAR_XML_LOADING_EXCEPTION;
+ }
+ catch (...)
+ {
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : Unable to read filename\n");
+ return NCV_HAAR_XML_LOADING_EXCEPTION;
+ }
+
+ haar.bHasStumpsOnly = true;
+ haar.bNeedsTiltedII = false;
+
+ Ncv32u cur_max_tree_depth;
+
+ std::vector<HaarClassifierNode128> host_temp_classifier_not_root_nodes;
+ haar_stages.resize(0);
+ haarClassifierNodes.resize(0);
+ haar_features.resize(0);
+
+ try // Fetch all parsing errors
+ {
+ int level1 = 0; // For debug output only
+ /// LEVEL1 (opencv_storage)
+ BOOST_FOREACH(const ptree::value_type &top_node, pt)
+ {
+ if(!strcmp(top_node.first.c_str(), "opencv_storage")) // Else NCV_HAAR_XML_LOADING_EXCEPTION
+ {
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : level 1, XMLnode %d, first : %s\n", level1, top_node.first.c_str());
+
+ int level2 = 0;
+ boost::property_tree::ptree pt2 = top_node.second;
+
+ /// LEVEL2 (haarcascade)
+ BOOST_FOREACH(const ptree::value_type &w, pt2)
+ {
+ if(!strcmp(w.second.get("<xmlattr>.type_id","").c_str(), "opencv-haar-classifier")) // Else NCV_HAAR_XML_LOADING_EXCEPTION
+ {
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : level 2, XMLnode %d, first : %s\n", level2, w.first.c_str());
+
+ // Parse the size field, giving the classifier patch size
+ std::string size_string = w.second.get<std::string>("size"); // data field contains both width and height so we put it in string first
+ std::cout << "loadFromXML2: level 2 : size string: " << size_string << std::endl;
+ std::istringstream l( size_string );
+ l >> std::skipws >> haar.ClassifierSize.width >> haar.ClassifierSize.height;
+ if ( !l || haar.ClassifierSize.width <= 0 || haar.ClassifierSize.height <= 0 )
+ {
+ PCL_WARN("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : level 2 : size node format error\n"); // format error: line doesn't start with an int.
+ return (NCV_HAAR_XML_LOADING_EXCEPTION);
+ }
+ else
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : level2 : size int1 %d, int2 %d\n", haar.ClassifierSize.width, haar.ClassifierSize.height);
+
+ int level3 = 0;
+ /// LEVEL3 (Stages)
+ BOOST_FOREACH(const ptree::value_type &stage, w.second.get_child("stages"))
+ {
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : level 3, XMLnode %d, first : %s\n", level3, stage.first.c_str());
+
+ HaarStage64 current_stage;
+ current_stage.setStartClassifierRootNodeOffset(haarClassifierNodes.size());
+ Ncv32u tmp_num_classifier_root_nodes = 0;
+
+ float stage_threshold = stage.second.get<float>("stage_threshold");
+ int parent = stage.second.get<int>("parent");
+ int next = stage.second.get<int>("next");
+
+ current_stage.setStageThreshold(stage_threshold);
+
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : level 3 stage_threshold %f, parent %d, next %d\n", stage_threshold, parent, next);
+
+ int level4 = 0;
+ /// LEVEL4 (Trees)
+ BOOST_FOREACH(const ptree::value_type &tree, stage.second.get_child("trees"))
+ {
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : level 4, XMLnode %d, first : %s\n", level4, tree.first.c_str());
+ Ncv32u node_identifier = 0;
+
+ int level5 = 0;
+ ptree root = tree.second;
+
+ /// LEVEL5 (Root_node)
+ BOOST_FOREACH(const ptree::value_type &root_node, root)
+ {
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : level 5, node %d, first : %s\n", level5, root_node.first.c_str());
+
+ if(!strcmp(root_node.first.c_str(), "_"))
+ {
+ HaarClassifierNode128 current_node;
+
+ float node_threshold = root_node.second.get<float>("threshold");
+ float left_val = root_node.second.get<float>("left_val"); // TODO Test if left node is available! see Nvidia code for solution
+ bool left_val_available = true; // TODO set correctly
+ float right_val = root_node.second.get<float>("right_val");
+ bool right_val_available = true; // TODO set correctly
+ bool tilted = root_node.second.get<bool>("feature.tilted");
+
+ current_node.setThreshold(node_threshold);
+
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : level 5 node_threshold %f, left_val %f, right_val %f, tilted %d\n", node_threshold, left_val, right_val, tilted);
+
+ HaarClassifierNodeDescriptor32 node_left;
+ ncv_return_status = node_left.create(left_val); // TODO check ncv_return_status return value line below and return
+ current_node.setLeftNodeDesc(node_left);
+
+ HaarClassifierNodeDescriptor32 node_right;
+ ncv_return_status = node_right.create(right_val);
+ current_node.setRightNodeDesc(node_right);
+
+ haar.bNeedsTiltedII = (tilted != 0);
+ Ncv32u feature_identifier = 0;
+
+ /// LEVEL6 (Rects)
+ BOOST_FOREACH(const ptree::value_type &rect, root_node.second.get_child("feature.rects"))
+ {
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : level 6, first : %s\n", rect.first.c_str());
+
+ std::string r = rect.second.data();
+
+ std::istringstream re( r );
+ int rectangle_u = 0, rectangle_v = 0, rectangle_width = 0, rectangle_height =0;
+ float rectWeight = 0;
+ re >> std::skipws >> rectangle_u >> rectangle_v >> rectangle_width >> rectangle_height >> rectWeight;
+
+ if ( !re )
+ {
+ // format error: line doesn't start with an int.
+ PCL_WARN("[pcl::gpu::people::FaceDetector::loadFromXML2] : (W) : level 6 : rect format error\n");
+ }
+ else
+ {
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : level 6 : rectangle_u %d, rectangle_v %d, rectW %d, rectH %d, rectWeight %f\n", rectangle_u, rectangle_v, rectangle_width, rectangle_height, rectWeight);
+ HaarFeature64 current_feature;
+ ncv_return_status = current_feature.setRect(rectangle_u, rectangle_v, rectangle_width, rectangle_height, haar.ClassifierSize.width, haar.ClassifierSize.height);
+ current_feature.setWeight(rectWeight);
+ PCL_ASSERT_NCVSTAT(ncv_return_status);
+ haar_features.push_back(current_feature);
+ feature_identifier++;
+ }
+ }
+
+ HaarFeatureDescriptor32 temp_feature_descriptor;
+ ncv_return_status = temp_feature_descriptor.create(haar.bNeedsTiltedII, left_val_available, right_val_available, feature_identifier, haar_features.size() - feature_identifier);
+
+ //ncv_return_status = temp_feature_descriptor.create(haar.bNeedsTiltedII, feature_identifier, haar_features.size() - feature_identifier);
+ ncvAssertReturn(NCV_SUCCESS == ncv_return_status, ncv_return_status);
+ current_node.setFeatureDesc(temp_feature_descriptor);
+
+ if (!node_identifier)
+ {
+ //root node
+ haarClassifierNodes.push_back(current_node);
+ cur_max_tree_depth = 1;
+ }
+ else
+ {
+ //other node
+ host_temp_classifier_not_root_nodes.push_back(current_node);
+ // TODO replace with PCL_DEBUG in the future
+ PCL_INFO("[pcl::gpu::people::FaceDetector::loadFromXML2] : (I) : Found non root node number %d", host_temp_classifier_not_root_nodes.size());
+ cur_max_tree_depth++;
+ }
+ node_identifier++;
+ }
+ else
+ PCL_WARN("[pcl::gpu::people::FaceDetector::loadFromXML2] : (W) : Found fifth level node that is atypical : %s\n", root_node.first.c_str());
+ level5++;
+ }
+ tmp_num_classifier_root_nodes++;
+ level4++;
+ }
+
+ current_stage.setNumClassifierRootNodes(tmp_num_classifier_root_nodes);
+ haar_stages.push_back(current_stage);
+
+ // TODO make this DEBUG later on
+ PCL_INFO("[pcl::gpu::people::FaceDetector::loadFromXML2] : (I) : level 3 stage %d loaded with %d Root Nodes, %f Threshold, %d Root Node Offset", haar_stages.size(), tmp_num_classifier_root_nodes, current_stage.getStartClassifierRootNodeOffset());
+
+ level3++;
+ }
+ }
+ else // Probably we stumbled upon a comment at this level or somebody adjusted the XML file
+ PCL_WARN("loadFromXML2: Found second level node that is atypical : %s\n", w.first.c_str());
+ }
+ level2++;
+ }
+ else
+ {
+ PCL_WARN("[pcl::gpu::people::FaceDetector::loadFromXML2] : (W) : Found first level node that is atypical : %s\n", top_node.first.c_str());
+ return (NCV_HAAR_XML_LOADING_EXCEPTION);
+ }
+ level1++;
+ }
+ }
+ catch(boost::exception const& exb)
+ {
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : Unable to process content with boost exception\n");
+ return (NCV_HAAR_XML_LOADING_EXCEPTION);
+ }
+ catch (std::exception const& ex)
+ {
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : Unable to process content with std exception %s\n", ex.what());
+ return (NCV_HAAR_XML_LOADING_EXCEPTION);
+ }
+ catch (...)
+ {
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : Unable to process content\n");
+ return (NCV_HAAR_XML_LOADING_EXCEPTION);
+ }
+
+ //fill in cascade stats
+ haar.NumStages = haar_stages.size();
+ haar.NumClassifierRootNodes = haarClassifierNodes.size();
+ haar.NumClassifierTotalNodes = haar.NumClassifierRootNodes + host_temp_classifier_not_root_nodes.size();
+ haar.NumFeatures = haar_features.size();
+
+ //merge root and leaf nodes in one classifiers array, leaf nodes are sorted behind the root nodes
+ Ncv32u offset_root = haarClassifierNodes.size();
+
+ for (Ncv32u i=0; i<haarClassifierNodes.size(); i++)
+ {
+ HaarClassifierNodeDescriptor32 node_left = haarClassifierNodes[i].getLeftNodeDesc();
+ if (!node_left.isLeaf())
+ {
+ Ncv32u new_offset = node_left.getNextNodeOffset() + offset_root;
+ node_left.create(new_offset);
+ }
+ haarClassifierNodes[i].setLeftNodeDesc(node_left);
+
+ HaarClassifierNodeDescriptor32 node_right = haarClassifierNodes[i].getRightNodeDesc();
+ if (!node_right.isLeaf())
+ {
+ Ncv32u new_offset = node_right.getNextNodeOffset() + offset_root;
+ node_right.create(new_offset);
+ }
+ haarClassifierNodes[i].setRightNodeDesc(node_right);
+ }
+
+ for (Ncv32u i=0; i<host_temp_classifier_not_root_nodes.size(); i++)
+ {
+ HaarClassifierNodeDescriptor32 node_left = host_temp_classifier_not_root_nodes[i].getLeftNodeDesc();
+ if (!node_left.isLeaf())
+ {
+ Ncv32u new_offset = node_left.getNextNodeOffset() + offset_root;
+ node_left.create(new_offset);
+ }
+ host_temp_classifier_not_root_nodes[i].setLeftNodeDesc(node_left);
+
+ HaarClassifierNodeDescriptor32 node_right = host_temp_classifier_not_root_nodes[i].getRightNodeDesc();
+ if (!node_right.isLeaf())
+ {
+ Ncv32u new_offset = node_right.getNextNodeOffset() + offset_root;
+ node_right.create(new_offset);
+ }
+ host_temp_classifier_not_root_nodes[i].setRightNodeDesc(node_right);
+
+ haarClassifierNodes.push_back(host_temp_classifier_not_root_nodes[i]);
+ }
+ return (NCV_SUCCESS);
+}
+
+/**
+ * \brief This loads the Haar description file from a NVBIN file format
+ */
+NCVStatus
+pcl::gpu::people::FaceDetector::loadFromNVBIN(const std::string &filename,
+ HaarClassifierCascadeDescriptor &haar,
+ std::vector<HaarStage64> &haar_stages,
+ std::vector<HaarClassifierNode128> &haarClassifierNodes,
+ std::vector<HaarFeature64> &haar_features)
+{
+ size_t readCount;
+ FILE *fp = fopen(filename.c_str(), "rb");
+ ncvAssertReturn(fp != NULL, NCV_FILE_ERROR);
+ Ncv32u fileVersion;
+ readCount = fread(&fileVersion, sizeof(Ncv32u), 1, fp);
+ PCL_ASSERT_ERROR_PRINT_RETURN(1 == readCount, "return NCV_FILE_ERROR", NCV_FILE_ERROR);
+ PCL_ASSERT_ERROR_PRINT_RETURN(fileVersion == NVBIN_HAAR_VERSION, "return NCV_FILE_ERROR", NCV_FILE_ERROR);
+ Ncv32u fsize;
+ readCount = fread(&fsize, sizeof(Ncv32u), 1, fp);
+ PCL_ASSERT_ERROR_PRINT_RETURN(1 == readCount, "return NCV_FILE_ERROR", NCV_FILE_ERROR);
+ fseek(fp, 0, SEEK_END);
+ Ncv32u fsizeActual = ftell(fp);
+ PCL_ASSERT_ERROR_PRINT_RETURN(fsize == fsizeActual, "return NCV_FILE_ERROR", NCV_FILE_ERROR);
+
+ std::vector<unsigned char> fdata;
+ fdata.resize(fsize);
+ Ncv32u dataOffset = 0;
+ fseek(fp, 0, SEEK_SET);
+ readCount = fread(&fdata[0], fsize, 1, fp);
+ PCL_ASSERT_ERROR_PRINT_RETURN(1 == readCount, "return NCV_FILE_ERROR", NCV_FILE_ERROR);
+ fclose(fp);
+
+ //data
+ dataOffset = NVBIN_HAAR_SIZERESERVED;
+ haar.NumStages = *(Ncv32u *)(&fdata[0]+dataOffset);
+ dataOffset += sizeof(Ncv32u);
+ haar.NumClassifierRootNodes = *(Ncv32u *)(&fdata[0]+dataOffset);
+ dataOffset += sizeof(Ncv32u);
+ haar.NumClassifierTotalNodes = *(Ncv32u *)(&fdata[0]+dataOffset);
+ dataOffset += sizeof(Ncv32u);
+ haar.NumFeatures = *(Ncv32u *)(&fdata[0]+dataOffset);
+ dataOffset += sizeof(Ncv32u);
+ haar.ClassifierSize = *(NcvSize32u *)(&fdata[0]+dataOffset);
+ dataOffset += sizeof(NcvSize32u);
+ haar.bNeedsTiltedII = *(NcvBool *)(&fdata[0]+dataOffset);
+ dataOffset += sizeof(NcvBool);
+ haar.bHasStumpsOnly = *(NcvBool *)(&fdata[0]+dataOffset);
+ dataOffset += sizeof(NcvBool);
+
+ haar_stages.resize(haar.NumStages);
+ haarClassifierNodes.resize(haar.NumClassifierTotalNodes);
+ haar_features.resize(haar.NumFeatures);
+
+ Ncv32u szStages = haar.NumStages * sizeof(HaarStage64);
+ Ncv32u szClassifiers = haar.NumClassifierTotalNodes * sizeof(HaarClassifierNode128);
+ Ncv32u szFeatures = haar.NumFeatures * sizeof(HaarFeature64);
+
+ memcpy(&haar_stages[0], &fdata[0]+dataOffset, szStages);
+ dataOffset += szStages;
+ memcpy(&haarClassifierNodes[0], &fdata[0]+dataOffset, szClassifiers);
+ dataOffset += szClassifiers;
+ memcpy(&haar_features[0], &fdata[0]+dataOffset, szFeatures);
+ dataOffset += szFeatures;
+
+ return NCV_SUCCESS;
+}
+
+/*
+ * \brief Depending on file format load the Haar description file
+ */
+NCVStatus
+pcl::gpu::people::FaceDetector::ncvHaarLoadFromFile_host(const std::string &filename,
+ HaarClassifierCascadeDescriptor &haar,
+ NCVVector<HaarStage64> &h_haar_stages,
+ NCVVector<HaarClassifierNode128> &h_haar_nodes,
+ NCVVector<HaarFeature64> &h_haar_features)
+{
+ PCL_ASSERT_ERROR_PRINT_RETURN(h_haar_stages.memType() == NCVMemoryTypeHostPinned &&
+ h_haar_nodes.memType() == NCVMemoryTypeHostPinned &&
+ h_haar_features.memType() == NCVMemoryTypeHostPinned, "return NCV_MEM_RESIDENCE_ERROR", NCV_MEM_RESIDENCE_ERROR);
+
+ NCVStatus ncv_return_status;
+
+ std::string fext = filename.substr(filename.find_last_of(".") + 1);
+ std::transform(fext.begin(), fext.end(), fext.begin(), ::tolower);
+
+ std::vector<HaarStage64> haar_stages;
+ std::vector<HaarClassifierNode128> haar_nodes;
+ std::vector<HaarFeature64> haar_features;
+
+ if (fext == "nvbin")
+ {
+ ncv_return_status = loadFromNVBIN(filename, haar, haar_stages, haar_nodes, haar_features);
+ PCL_ASSERT_NCVSTAT(ncv_return_status);
+ }
+ else if (fext == "xml")
+ {
+ ncv_return_status = loadFromXML2(filename, haar, haar_stages, haar_nodes, haar_features);
+ PCL_ASSERT_NCVSTAT(ncv_return_status);
+ }
+ else
+ {
+ return NCV_HAAR_XML_LOADING_EXCEPTION;
+ }
+
+ PCL_ASSERT_ERROR_PRINT_RETURN(h_haar_stages.length() >= haar_stages.size(), "Return NCV_MEM_INSUFFICIENT_CAPACITY", NCV_MEM_INSUFFICIENT_CAPACITY);
+ PCL_ASSERT_ERROR_PRINT_RETURN(h_haar_nodes.length() >= haar_nodes.size(), "Return NCV_MEM_INSUFFICIENT_CAPACITY", NCV_MEM_INSUFFICIENT_CAPACITY);
+ PCL_ASSERT_ERROR_PRINT_RETURN(h_haar_features.length() >= haar_features.size(), "Return NCV_MEM_INSUFFICIENT_CAPACITY", NCV_MEM_INSUFFICIENT_CAPACITY);
+
+ memcpy(h_haar_stages.ptr(), &haar_stages[0], haar_stages.size()*sizeof(HaarStage64));
+ memcpy(h_haar_nodes.ptr(), &haar_nodes[0], haar_nodes.size()*sizeof(HaarClassifierNode128));
+ memcpy(h_haar_features.ptr(), &haar_features[0], haar_features.size()*sizeof(HaarFeature64));
+
+ return NCV_SUCCESS;
+}
+
+/*
+ * \brief Scans the Haar description file for the sizes of the Stages, Nodes and Features
+ */
+NCVStatus
+pcl::gpu::people::FaceDetector::ncvHaarGetClassifierSize(const std::string &filename,
+ Ncv32u &numStages,
+ Ncv32u &numNodes,
+ Ncv32u &numFeatures)
+{
+ size_t readCount;
+ NCVStatus ncv_return_status;
+
+ std::string fext = filename.substr(filename.find_last_of(".") + 1);
+ std::transform(fext.begin(), fext.end(), fext.begin(), ::tolower);
+
+ if (fext == "nvbin")
+ {
+ FILE *fp = fopen(filename.c_str(), "rb");
+ PCL_ASSERT_ERROR_PRINT_RETURN(fp != NULL, "Return NCV_FILE_ERROR", NCV_FILE_ERROR);
+ Ncv32u fileVersion;
+ readCount = fread(&fileVersion, sizeof(Ncv32u), 1, fp);
+ PCL_ASSERT_ERROR_PRINT_RETURN(1 == readCount, "Return NCV_FILE_ERROR", NCV_FILE_ERROR);
+ PCL_ASSERT_ERROR_PRINT_RETURN(fileVersion == NVBIN_HAAR_VERSION, "Return NCV_FILE_ERROR", NCV_FILE_ERROR);
+ fseek(fp, NVBIN_HAAR_SIZERESERVED, SEEK_SET);
+ Ncv32u tmp;
+ readCount = fread(&numStages, sizeof(Ncv32u), 1, fp);
+ PCL_ASSERT_ERROR_PRINT_RETURN(1 == readCount, "Return NCV_FILE_ERROR", NCV_FILE_ERROR);
+ readCount = fread(&tmp, sizeof(Ncv32u), 1, fp);
+ PCL_ASSERT_ERROR_PRINT_RETURN(1 == readCount, "Return NCV_FILE_ERROR", NCV_FILE_ERROR);
+ readCount = fread(&numNodes, sizeof(Ncv32u), 1, fp);
+ PCL_ASSERT_ERROR_PRINT_RETURN(1 == readCount, "Return NCV_FILE_ERROR", NCV_FILE_ERROR);
+ readCount = fread(&numFeatures, sizeof(Ncv32u), 1, fp);
+ PCL_ASSERT_ERROR_PRINT_RETURN(1 == readCount, "Return NCV_FILE_ERROR", NCV_FILE_ERROR);
+ fclose(fp);
+ }
+ else if (fext == "xml")
+ {
+ HaarClassifierCascadeDescriptor haar;
+ std::vector<HaarStage64> haar_stages;
+ std::vector<HaarClassifierNode128> haar_nodes;
+ std::vector<HaarFeature64> haar_features;
+
+ ncv_return_status = loadFromXML2(filename, haar, haar_stages, haar_nodes, haar_features);
+ PCL_ASSERT_NCVSTAT(ncv_return_status); // TODO convert this to PCL methodS
+
+ numStages = haar.NumStages;
+ numNodes = haar.NumClassifierTotalNodes;
+ numFeatures = haar.NumFeatures;
+ }
+ else
+ {
+ return NCV_HAAR_XML_LOADING_EXCEPTION;
+ }
+
+ return NCV_SUCCESS;
+}
+
+/*
+ * \brief the Wrapper that calls the actual Nvidia code
+ */
+NCVStatus
+pcl::gpu::people::FaceDetector::NCVprocess(pcl::PointCloud<pcl::RGB>& cloud_in,
+ pcl::PointCloud<pcl::Intensity32u>& cloud_out,
+ HaarClassifierCascadeDescriptor &haar,
+ NCVVector<HaarStage64> &d_haar_stages,
+ NCVVector<HaarClassifierNode128> &d_haar_nodes,
+ NCVVector<HaarFeature64> &d_haar_features,
+ NCVVector<HaarStage64> &h_haar_stages,
+ INCVMemAllocator &gpu_allocator,
+ INCVMemAllocator &cpu_allocator,
+ cudaDeviceProp &device_properties,
+ Ncv32u width,
+ Ncv32u height,
+ NcvBool bFilterRects,
+ NcvBool bLargestFace)
+{
+ pcl::PointCloud<Intensity32u> input_gray;
+
+ PointCloudRGBtoI(cloud_in, input_gray);
+
+ cloud_out.width = input_gray.width;
+ cloud_out.height = input_gray.height;
+ cloud_out.points.resize (input_gray.points.size ());
+
+ PCL_ASSERT_ERROR_PRINT_RETURN(!gpu_allocator.isCounting(),"retcode=NCV_NULL_PTR", NCV_NULL_PTR);
+
+ NCVStatus ncv_return_status;
+
+ NCV_SET_SKIP_COND(gpu_allocator.isCounting());
+
+ NCVMatrixAlloc<Ncv8u> d_src(gpu_allocator, width, height);
+ PCL_ASSERT_ERROR_PRINT_RETURN(d_src.isMemAllocated(), "retcode=NCV_ALLOCATOR_BAD_ALLOC", NCV_ALLOCATOR_BAD_ALLOC);
+ NCVMatrixAlloc<Ncv8u> h_src(cpu_allocator, width, height);
+ PCL_ASSERT_ERROR_PRINT_RETURN(h_src.isMemAllocated(), "retcode=NCV_ALLOCATOR_BAD_ALLOC", NCV_ALLOCATOR_BAD_ALLOC);
+ NCVVectorAlloc<NcvRect32u> d_rects(gpu_allocator, 100);
+ PCL_ASSERT_ERROR_PRINT_RETURN(d_rects.isMemAllocated(), "retcode=NCV_ALLOCATOR_BAD_ALLOC", NCV_ALLOCATOR_BAD_ALLOC);
+
+ NCV_SKIP_COND_BEGIN
+
+ for(int i=0; i<input_gray.points.size(); i++)
+ {
+ memcpy(h_src.ptr(), &input_gray.points[i].intensity, sizeof(input_gray.points[i].intensity));
+ }
+
+ ncv_return_status = h_src.copySolid(d_src, 0);
+ PCL_ASSERT_NCVSTAT(ncv_return_status);
+ PCL_ASSERT_CUDA_RETURN(cudaStreamSynchronize(0), NCV_CUDA_ERROR);
+
+ NCV_SKIP_COND_END
+
+ NcvSize32u roi;
+ roi.width = d_src.width();
+ roi.height = d_src.height();
+
+ Ncv32u number_of_detections;
+ ncv_return_status = ncvDetectObjectsMultiScale_device ( d_src,
+ roi,
+ d_rects,
+ number_of_detections,
+ haar,
+ h_haar_stages,
+ d_haar_stages,
+ d_haar_nodes,
+ d_haar_features,
+ haar.ClassifierSize,
+ (bFilterRects || bLargestFace) ? 4 : 0,
+ 1.2f,
+ 1,
+ (bLargestFace ? NCVPipeObjDet_FindLargestObject : 0) | NCVPipeObjDet_VisualizeInPlace,
+ gpu_allocator,
+ cpu_allocator,
+ device_properties,
+ 0);
+
+ PCL_ASSERT_NCVSTAT(ncv_return_status);
+ PCL_ASSERT_CUDA_RETURN(cudaStreamSynchronize(0), NCV_CUDA_ERROR);
+
+ NCV_SKIP_COND_BEGIN
+
+ ncv_return_status = d_src.copySolid(h_src, 0);
+ PCL_ASSERT_NCVSTAT(ncv_return_status);
+ PCL_ASSERT_CUDA_RETURN(cudaStreamSynchronize(0), NCV_CUDA_ERROR);
+
+ // Copy result back into output cloud
+ for(int i=0; i<cloud_out.points.size(); i++)
+ {
+ memcpy(&cloud_out.points[i].intensity, h_src.ptr() /* + i * ??? */, sizeof(cloud_out.points[i].intensity));
+ }
+
+ NCV_SKIP_COND_END
+
+ return NCV_SUCCESS;
+}
+
+/*
+ * \brief This does the GPU allocations and configurations
+ */
+int
+pcl::gpu::people::FaceDetector::configure(std::string cascade_file_name)
+{
+ cascade_file_name_ = cascade_file_name;
+
+ // Load the classifier from file (assuming its size is about 1 mb), using a simple allocator
+ gpu_allocator_ = new NCVMemNativeAllocator(NCVMemoryTypeDevice, static_cast<Ncv32u>(cuda_dev_prop_.textureAlignment));
+ PCL_ASSERT_ERROR_PRINT_RETURN(gpu_allocator_->isInitialized(), "[pcl::gpu::people::FaceDetector::FaceDetector] : Error creating cascade GPU allocator", -1);
+
+ cpu_allocator_ = new NCVMemNativeAllocator(NCVMemoryTypeHostPinned, static_cast<Ncv32u>(cuda_dev_prop_.textureAlignment));
+ PCL_ASSERT_ERROR_PRINT_RETURN(cpu_allocator_->isInitialized(), "[pcl::gpu::people::FaceDetector::FaceDetector] : Error creating cascade CPU allocator", -1);
+
+ NCVStatus ncv_return_status;
+ Ncv32u haarNumStages, haarNumNodes, haarNumFeatures;
+
+ ncv_return_status = ncvHaarGetClassifierSize(cascade_file_name_, haarNumStages, haarNumNodes, haarNumFeatures);
+ PCL_ASSERT_ERROR_PRINT_RETURN(ncv_return_status == NCV_SUCCESS, "[pcl::gpu::people::FaceDetector::FaceDetector] : Error reading classifier size (check the file)", -1);
+
+ haar_stages_host_ = new NCVVectorAlloc<HaarStage64>(*cpu_allocator_, haarNumStages);
+ PCL_ASSERT_ERROR_PRINT_RETURN(haar_stages_host_->isMemAllocated(), "[pcl::gpu::people::FaceDetector::FaceDetector] : Error in cascade CPU allocator", -1);
+
+ haar_nodes_host_ = new NCVVectorAlloc<HaarClassifierNode128>(*cpu_allocator_, haarNumNodes);
+ PCL_ASSERT_ERROR_PRINT_RETURN(haar_nodes_host_->isMemAllocated(), "[pcl::gpu::people::FaceDetector::FaceDetector] : Error in cascade CPU allocator", -1);
+
+ haar_features_host_ = new NCVVectorAlloc<HaarFeature64>(*cpu_allocator_, haarNumFeatures);
+ PCL_ASSERT_ERROR_PRINT_RETURN(haar_features_host_->isMemAllocated(), "[pcl::gpu::people::FaceDetector::FaceDetector] : Error in cascade CPU allocator", -1);
+
+ ncv_return_status = ncvHaarLoadFromFile_host(cascade_file_name_, haar_clas_casc_descr_, *haar_stages_host_, *haar_nodes_host_, *haar_features_host_);
+ PCL_ASSERT_ERROR_PRINT_RETURN(ncv_return_status == NCV_SUCCESS, "[pcl::gpu::people::FaceDetector::FaceDetector] : Error loading classifier", -1);
+
+ haar_stages_dev_ = new NCVVectorAlloc<HaarStage64>(*gpu_allocator_, haarNumStages);
+ PCL_ASSERT_ERROR_PRINT_RETURN(haar_stages_dev_->isMemAllocated(), "[pcl::gpu::people::FaceDetector::FaceDetector] : Error in cascade GPU allocator", -1);
+
+ haar_nodes_dev_ = new NCVVectorAlloc<HaarClassifierNode128>(*gpu_allocator_, haarNumNodes);
+ PCL_ASSERT_ERROR_PRINT_RETURN(haar_nodes_dev_->isMemAllocated(), "[pcl::gpu::people::FaceDetector::FaceDetector] : Error in cascade GPU allocator", -1);
+
+ haar_features_dev_ = new NCVVectorAlloc<HaarFeature64>(*gpu_allocator_, haarNumFeatures);
+ PCL_ASSERT_ERROR_PRINT_RETURN(haar_features_dev_->isMemAllocated(), "[pcl::gpu::people::FaceDetector::FaceDetector] : Error in cascade GPU allocator", -1);
+
+ ncv_return_status = haar_stages_host_->copySolid(*haar_stages_dev_, 0);
+ PCL_ASSERT_ERROR_PRINT_RETURN(ncv_return_status == NCV_SUCCESS, "[pcl::gpu::people::FaceDetector::FaceDetector] : Error copying cascade to GPU", -1);
+ ncv_return_status = haar_nodes_host_->copySolid(*haar_nodes_dev_, 0);
+ PCL_ASSERT_ERROR_PRINT_RETURN(ncv_return_status == NCV_SUCCESS, "[pcl::gpu::people::FaceDetector::FaceDetector] : Error copying cascade to GPU", -1);
+ ncv_return_status = haar_features_host_->copySolid(*haar_features_dev_, 0);
+ PCL_ASSERT_ERROR_PRINT_RETURN(ncv_return_status == NCV_SUCCESS, "[pcl::gpu::people::FaceDetector::FaceDetector] : Error copying cascade to GPU", -1);
+
+ // Calculate memory requirements and create real allocators
+ gpu_counter_ = new NCVMemStackAllocator(static_cast<Ncv32u>(cuda_dev_prop_.textureAlignment));
+ PCL_ASSERT_ERROR_PRINT_RETURN(gpu_counter_->isInitialized(), "[pcl::gpu::people::FaceDetector::FaceDetector] : Error creating GPU memory counter", -1);
+
+ cpu_counter_ = new NCVMemStackAllocator(static_cast<Ncv32u>(cuda_dev_prop_.textureAlignment));
+ PCL_ASSERT_ERROR_PRINT_RETURN(cpu_counter_->isInitialized(), "[pcl::gpu::people::FaceDetector::FaceDetector] : Error creating CPU memory counter", -1);
+
+ // TODO fix this fake call
+ /*
+ ncv_return_status = NCVprocess (NULL, cols_, rows_,
+ false, false, haar,
+ d_haar_stages, d_haar_nodes,
+ d_haar_features, h_haar_stages,
+ gpuCounter, cpuCounter, cuda_dev_prop_);
+ PCL_ASSERT_ERROR_PRINT_RETURN(ncv_return_status == NCV_SUCCESS, "[pcl::gpu::people::FaceDetector::FaceDetector] : Error in memory counting pass", -1);
+ */
+
+ gpu_stack_allocator_ = new NCVMemStackAllocator(NCVMemoryTypeDevice, gpu_counter_->maxSize(), static_cast<Ncv32u>(cuda_dev_prop_.textureAlignment));
+ PCL_ASSERT_ERROR_PRINT_RETURN(gpu_stack_allocator_->isInitialized(), "[pcl::gpu::people::FaceDetector::FaceDetector] : Error creating GPU memory allocator", -1);
+
+ cpu_stack_allocator_ = new NCVMemStackAllocator(NCVMemoryTypeHostPinned, cpu_counter_->maxSize(), static_cast<Ncv32u>(cuda_dev_prop_.textureAlignment));
+ PCL_ASSERT_ERROR_PRINT_RETURN(cpu_stack_allocator_->isInitialized(), "[pcl::gpu::people::FaceDetector::FaceDetector] : Error creating CPU memory allocator", -1);
+
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::FaceDetector] : (D) : Initialized for frame size [%dx%d]\n", cols_, rows_);
+
+ return 1;
+}
+
+void
+pcl::gpu::people::FaceDetector::setDeviceId( int id )
+{
+ cuda_dev_id_ = id;
+ cudaSafeCall ( cudaSetDevice (cuda_dev_id_));
+ cudaSafeCall ( cudaGetDeviceProperties (&cuda_dev_prop_, cuda_dev_id_));
+}
+
+void
+pcl::gpu::people::FaceDetector::process(pcl::PointCloud<pcl::RGB>& cloud_in,
+ pcl::PointCloud<pcl::Intensity32u>& cloud_out)
+{
+ PCL_DEBUG("[pcl::gpu::people::FaceDetector::process] : (D) : called\n");
+ cols_ = cloud_in.width; rows_ = cloud_in.height;
+
+ // TODO do something with the NCVStatus return value
+ NCVStatus status = NCVprocess(cloud_in,
+ cloud_out,
+ haar_clas_casc_descr_,
+ *haar_stages_dev_,
+ *haar_nodes_dev_,
+ *haar_features_dev_,
+ *haar_stages_host_,
+ *gpu_allocator_,
+ *cpu_allocator_,
+ cuda_dev_prop_,
+ cloud_in.width,
+ cloud_in.height,
+ filter_rects_,
+ largest_object_);
+
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * @authors: Cedric Cagniart, Koen Buys, Anatoly Baksheev
+ */
+
+#ifndef PCL_GPU_PEOPLE_INTERNAL_H_
+#define PCL_GPU_PEOPLE_INTERNAL_H_
+
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/utils/safe_call.hpp>
+#include <pcl/gpu/people/label_common.h>
+#include <pcl/gpu/people/tree.h>
+
+using pcl::gpu::people::NUM_PARTS;
+
+namespace pcl
+{
+ namespace device
+ {
+ typedef DeviceArray2D<float4> Cloud;
+ typedef DeviceArray2D<uchar4> Image;
+
+ typedef DeviceArray2D<unsigned short> Depth;
+ typedef DeviceArray2D<unsigned char> Labels;
+ typedef DeviceArray2D<float> HueImage;
+ typedef DeviceArray2D<unsigned char> Mask;
+
+ typedef DeviceArray2D<char4> MultiLabels;
+
+ /** \brief The intrinsic camera calibration **/
+ struct Intr
+ {
+ float fx, fy, cx, cy;
+ Intr () {}
+ Intr (float fx_, float fy_, float cx_, float cy_) : fx(fx_), fy(fy_), cx(cx_), cy(cy_) {}
+
+ void setDefaultPPIfIncorrect(int cols, int rows)
+ {
+ cx = cx > 0 ? cx : cols/2 - 0.5f;
+ cy = cy > 0 ? cy : rows/2 - 0.5f;
+ }
+ };
+
+ void smoothLabelImage(const Labels& src, const Depth& depth, Labels& dst, int num_parts, int patch_size, int depthThres);
+ void colorLMap(const Labels& labels, const DeviceArray<uchar4>& cmap, Image& rgb);
+ void mixedColorMap(const Labels& labels, const DeviceArray<uchar4>& map, const Image& rgba, Image& output);
+
+ ////////////// connected components ///////////////////
+
+ struct ConnectedComponents
+ {
+ static void initEdges(int rows, int cols, DeviceArray2D<unsigned char>& edges);
+ //static void computeEdges(const Labels& labels, const Cloud& cloud, int num_parts, float sq_radius, DeviceArray2D<unsigned char>& edges);
+ static void computeEdges(const Labels& labels, const Depth& depth, int num_parts, float sq_radius, DeviceArray2D<unsigned char>& edges);
+ static void labelComponents(const DeviceArray2D<unsigned char>& edges, DeviceArray2D<int>& comps);
+ };
+
+ void computeCloud(const Depth& depth, const Intr& intr, Cloud& cloud);
+
+ void setZero(Mask& mask);
+ void prepareForeGroundDepth(const Depth& depth1, Mask& inverse_mask, Depth& depth2);
+
+ float computeHue(int rgba);
+ void computeHueWithNans(const Image& image, const Depth& depth, HueImage& hue);
+
+ //void shs(const DeviceArray2D<float4> &cloud, float tolerance, const std::vector<int>& indices_in, float delta_hue, Mask& indices_out);
+
+ struct Dilatation
+ {
+ typedef DeviceArray<unsigned char> Kernel;
+ enum
+ {
+ KSIZE_X = 5,
+ KSIZE_Y = 5,
+ ANCH_X = KSIZE_X/2,
+ ANCH_Y = KSIZE_Y/2,
+ };
+
+ static void prepareRect5x5Kernel(Kernel& kernel);
+ static void invoke(const Mask& src, const Kernel& kernel, Mask& dst);
+ };
+
+ /** \brief Struct that holds a single RDF tree in GPU **/
+ struct CUDATree
+ {
+ typedef pcl::gpu::people::trees::Node Node;
+ typedef pcl::gpu::people::trees::Label Label;
+
+ int treeHeight;
+ int numNodes;
+
+ DeviceArray<Node> nodes_device;
+ DeviceArray<Label> leaves_device;
+
+ CUDATree (int treeHeight_, const std::vector<Node>& nodes, const std::vector<Label>& leaves);
+ };
+
+ /** \brief Processor using multiple trees */
+ class MultiTreeLiveProc
+ {
+ public:
+ /** \brief Constructor with default values, allocates multilmap device memory **/
+ MultiTreeLiveProc(int def_rows = 480, int def_cols = 640) : multilmap (def_rows, def_cols) {}
+ /** \brief Empty destructor **/
+ ~MultiTreeLiveProc() {}
+
+ void
+ process (const Depth& dmap, Labels& lmap);
+
+ /** \brief same as process, but runs the trick of declaring as background any neighbor that is more than FGThresh away.**/
+ void
+ process (const Depth& dmap, Labels& lmap, int FGThresh);
+
+ /** \brief output a probability map from the RDF.**/
+ void
+ processProb (const Depth& dmap, Labels& lmap, LabelProbability& prob, int FGThresh);
+
+ std::vector<CUDATree> trees;
+ MultiLabels multilmap;
+ };
+
+ /** \brief Implementation Class to process probability histograms on GPU **/
+ class ProbabilityProc
+ {
+ public:
+ /** \brief Default constructor **/
+ ProbabilityProc()
+ {
+ std::cout << "[pcl::device::ProbabilityProc:ProbabilityProc] : (D) : Constructor called" << std::endl;
+ //PCL_DEBUG("[pcl::device::ProbabilityProc:ProbabilityProc] : (D) : Constructor called");
+ }
+
+ /** \brief Default destructor **/
+ ~ProbabilityProc() {}
+
+ /** \brief This will merge the votes from the different trees into one final vote, including probabilistic's **/
+ void
+ CUDA_SelectLabel ( const Depth& depth, Labels& labels, LabelProbability& probabilities);
+
+ /** \brief This will combine two probabilities according their weight **/
+ void
+ CUDA_CombineProb ( const Depth& depth, LabelProbability& probIn1, float weight1,
+ LabelProbability& probIn2, float weight2, LabelProbability& probOut);
+
+ /** \brief This will sum a probability multiplied with it's weight **/
+ void
+ CUDA_WeightedSumProb ( const Depth& depth, LabelProbability& probIn, float weight, LabelProbability& probOut);
+
+ /** \brief This will blur the input labelprobability with the given kernel **/
+ int
+ CUDA_GaussianBlur( const Depth& depth,
+ LabelProbability& probIn,
+ DeviceArray<float>& kernel,
+ LabelProbability& probOut);
+ /** \brief This will blur the input labelprobability with the given kernel, this version avoids extended allocation **/
+ int
+ CUDA_GaussianBlur( const Depth& depth,
+ LabelProbability& probIn,
+ DeviceArray<float>& kernel,
+ LabelProbability& probTemp,
+ LabelProbability& probOut);
+ };
+ }
+}
+
+#endif /* PCL_GPU_PEOPLE_INTERNAL_H_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * @author: Koen Buys
+ */
+
+#include <pcl/gpu/people/organized_plane_detector.h>
+
+#include <pcl/console/print.h>
+
+#include <pcl/filters/voxel_grid.h>
+
+#include <pcl/features/integral_image_normal.h>
+
+#include <pcl/common/transforms.h>
+
+#include <pcl/segmentation/organized_multi_plane_segmentation.h>
+#include <pcl/segmentation/planar_polygon_fusion.h>
+#include <pcl/segmentation/plane_coefficient_comparator.h>
+#include <pcl/segmentation/euclidean_plane_coefficient_comparator.h>
+#include <pcl/segmentation/rgb_plane_coefficient_comparator.h>
+#include <pcl/segmentation/edge_aware_plane_comparator.h>
+#include <pcl/segmentation/euclidean_cluster_comparator.h>
+#include <pcl/segmentation/organized_connected_component_segmentation.h>
+
+
+pcl::gpu::people::OrganizedPlaneDetector::OrganizedPlaneDetector(int rows, int cols)
+{
+ PCL_DEBUG("[pcl::gpu::people::OrganizedPlaneDetector::OrganizedPlaneDetector] : (D) : Constructor called\n");
+
+ // Set NE defaults
+ ne_.setNormalEstimationMethod (ne_.COVARIANCE_MATRIX);
+ ne_.setMaxDepthChangeFactor (0.02f);
+ ne_.setNormalSmoothingSize (20.0f);
+
+ // Set MPS defaults
+ mps_MinInliers_ = 10000;
+ mps_AngularThreshold_ = pcl::deg2rad (3.0); //3 degrees
+ mps_DistanceThreshold_ = 0.02; //2cm
+ mps_use_planar_refinement_ = true;
+
+ mps_.setMinInliers (mps_MinInliers_);
+ mps_.setAngularThreshold (mps_AngularThreshold_);
+ mps_.setDistanceThreshold (mps_DistanceThreshold_);
+
+ allocate_buffers(rows, cols);
+}
+
+void
+pcl::gpu::people::OrganizedPlaneDetector::process(const PointCloud<PointTC>::ConstPtr &cloud)
+{
+ PCL_DEBUG("[pcl::gpu::people::OrganizedPlaneDetector::process] : (D) : Called\n");
+
+ // Estimate Normals
+ pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
+ ne_.setInputCloud (cloud);
+ ne_.compute (*normal_cloud);
+
+ // Segment Planes
+ std::vector<pcl::PlanarRegion<PointTC>, Eigen::aligned_allocator<pcl::PlanarRegion<PointTC> > > regions;
+ std::vector<pcl::ModelCoefficients> model_coefficients;
+ std::vector<pcl::PointIndices> inlier_indices;
+ pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
+
+ std::vector<pcl::PointIndices> label_indices;
+ std::vector<pcl::PointIndices> boundary_indices;
+
+ mps_.setInputNormals (normal_cloud);
+ mps_.setInputCloud (cloud);
+ if (mps_use_planar_refinement_)
+ {
+ mps_.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
+ }
+ else
+ {
+ //mps_.segment (regions);
+ mps_.segment (model_coefficients, inlier_indices);
+ }
+
+ // Fill in the probabilities
+ for(int plane = 0; plane < inlier_indices.size(); plane++) // iterate over all found planes
+ {
+ for(int idx = 0; idx < inlier_indices[plane].indices.size(); idx++) // iterate over all the indices in that plane
+ {
+ P_l_host_.points[inlier_indices[plane].indices[idx]].probs[pcl::gpu::people::Background] = 1.f; // set background at max
+ }
+ }
+}
+
+
+void
+pcl::gpu::people::OrganizedPlaneDetector::allocate_buffers(int rows, int cols)
+{
+ PCL_DEBUG("[pcl::gpu::people::OrganizedPlaneDetector::allocate_buffers] : (D) : Called\n");
+
+ // Create histogram on host
+ P_l_host_.points.resize(rows*cols);
+ P_l_host_.width = cols;
+ P_l_host_.height = rows;
+
+ P_l_host_prev_.points.resize(rows*cols);
+ P_l_host_prev_.width = cols;
+ P_l_host_prev_.height = rows;
+
+ // Create all the label probabilities on device
+ P_l_dev_.create(rows,cols);
+ P_l_dev_prev_.create(rows,cols);
+}
+
+void
+pcl::gpu::people::OrganizedPlaneDetector::emptyHostLabelProbability(HostLabelProbability& histogram)
+{
+ for(int hist = 0; hist < histogram.points.size(); hist++)
+ {
+ for(int label = 0; label < pcl::gpu::people::NUM_LABELS; label++)
+ {
+ histogram.points[hist].probs[label] = 0.f;
+ }
+ }
+}
+
+int
+pcl::gpu::people::OrganizedPlaneDetector::copyHostLabelProbability(HostLabelProbability& src,
+ HostLabelProbability& dst)
+{
+ if(src.points.size() != dst.points.size())
+ {
+ PCL_ERROR("[pcl::gpu::people::OrganizedPlaneDetector::copyHostLabelProbability] : (E) : Sizes don't match\n");
+ return -1;
+ }
+ for(int hist = 0; hist < src.points.size(); hist++)
+ {
+ for(int label = 0; label < pcl::gpu::people::NUM_LABELS; label++)
+ {
+ dst.points[hist].probs[label] = src.points[hist].probs[label];
+ }
+ }
+ return 1;
+}
+
+int
+pcl::gpu::people::OrganizedPlaneDetector::copyAndClearHostLabelProbability(HostLabelProbability& src,
+ HostLabelProbability& dst)
+{
+ if(src.points.size() != dst.points.size())
+ {
+ PCL_ERROR("[pcl::gpu::people::OrganizedPlaneDetector::copyHostLabelProbability] : (E) : Sizes don't match\n");
+ return -1;
+ }
+ for(int hist = 0; hist < src.points.size(); hist++)
+ {
+ for(int label = 0; label < pcl::gpu::people::NUM_LABELS; label++)
+ {
+ dst.points[hist].probs[label] = src.points[hist].probs[label];
+ src.points[hist].probs[label] = 0.f;
+ }
+ }
+ return 1;
+}
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* @author: Koen Buys, Anatoly Baksheev
+*/
+
+#include <pcl/gpu/people/people_detector.h>
+#include <pcl/gpu/people/label_common.h>
+
+//#include <pcl/gpu/people/conversions.h>
+//#include <pcl/gpu/people/label_conversion.h>
+//#include <pcl/gpu/people/label_segment.h>
+#include <pcl/gpu/people/label_tree.h>
+#include <pcl/gpu/people/probability_processor.h>
+#include <pcl/gpu/people/organized_plane_detector.h>
+#include <pcl/console/print.h>
+#include "internal.h"
+
+#include <pcl/common/time.h>
+
+#define AREA_THRES 200 // for euclidean clusterization 1
+#define AREA_THRES2 100 // for euclidean clusterization 2
+#define CLUST_TOL_SHS 0.05
+#define DELTA_HUE_SHS 5
+
+using namespace std;
+using namespace pcl;
+using namespace pcl::gpu::people;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+pcl::gpu::people::PeopleDetector::PeopleDetector()
+ : fx_(525.f), fy_(525.f), cx_(319.5f), cy_(239.5f), delta_hue_tolerance_(5)
+{
+ PCL_DEBUG ("[pcl::gpu::people::PeopleDetector] : (D) : Constructor called\n");
+
+ // Create a new organized plane detector
+ org_plane_detector_ = OrganizedPlaneDetector::Ptr (new OrganizedPlaneDetector());
+
+ // Create a new probability_processor
+ probability_processor_ = ProbabilityProcessor::Ptr (new ProbabilityProcessor());
+
+ // Create a new person attribs
+ person_attribs_ = PersonAttribs::Ptr (new PersonAttribs());
+
+ // Just created, indicates first time callback (allows for tracking features to start from second frame)
+ first_iteration_ = true;
+
+ // allocation buffers with default sizes
+ // if input size is other than the defaults,
+ // then the buffers will be reallocated at processing time.
+ // This cause only penalty for first frame ( one reallocation of each buffer )
+ allocate_buffers();
+}
+
+void
+pcl::gpu::people::PeopleDetector::setIntrinsics (float fx, float fy, float cx, float cy)
+{
+ fx_ = fx; fy_ = fy; cx_ = cx; cy_ = cy;
+}
+
+/** @brief This function prepares the needed buffers on both host and device **/
+void
+pcl::gpu::people::PeopleDetector::allocate_buffers(int rows, int cols)
+{
+ device::Dilatation::prepareRect5x5Kernel(kernelRect5x5_);
+
+ cloud_host_.width = cols;
+ cloud_host_.height = rows;
+ cloud_host_.points.resize(cols * rows);
+ cloud_host_.is_dense = false;
+
+ cloud_host_color_.width = cols;
+ cloud_host_color_.height = rows;
+ cloud_host_color_.resize(cols * rows);
+ cloud_host_color_.is_dense = false;
+
+ hue_host_.width = cols;
+ hue_host_.height = rows;
+ hue_host_.points.resize(cols * rows);
+ hue_host_.is_dense = false;
+
+ depth_host_.width = cols;
+ depth_host_.height = rows;
+ depth_host_.points.resize(cols * rows);
+ depth_host_.is_dense = false;
+
+ flowermat_host_.width = cols;
+ flowermat_host_.height = rows;
+ flowermat_host_.points.resize(cols * rows);
+ flowermat_host_.is_dense = false;
+
+ cloud_device_.create(rows, cols);
+ hue_device_.create(rows, cols);
+
+ depth_device1_.create(rows, cols);
+ depth_device2_.create(rows, cols);
+ fg_mask_.create(rows, cols);
+ fg_mask_grown_.create(rows, cols);
+}
+
+int
+pcl::gpu::people::PeopleDetector::process(const Depth& depth, const Image& rgba)
+{
+ int cols;
+ allocate_buffers(depth.rows(), depth.cols());
+
+ depth_device1_ = depth;
+
+ const device::Image& i = (const device::Image&)rgba;
+ device::computeHueWithNans(i, depth_device1_, hue_device_);
+ //TODO Hope this is temporary and after porting to GPU the download will be deleted
+ hue_device_.download(hue_host_.points, cols);
+
+ device::Intr intr(fx_, fy_, cx_, cy_);
+ intr.setDefaultPPIfIncorrect(depth.cols(), depth.rows());
+
+ device::Cloud& c = (device::Cloud&)cloud_device_;
+ device::computeCloud(depth, intr, c);
+ cloud_device_.download(cloud_host_.points, cols);
+
+ // uses cloud device, cloud host, depth device, hue device and other buffers
+ return process();
+}
+
+int
+pcl::gpu::people::PeopleDetector::process (const pcl::PointCloud<PointTC>::ConstPtr &cloud)
+{
+ allocate_buffers(cloud->height, cloud->width);
+
+ const float qnan = std::numeric_limits<float>::quiet_NaN();
+
+ for(size_t i = 0; i < cloud->points.size(); ++i)
+ {
+ cloud_host_.points[i].x = cloud->points[i].x;
+ cloud_host_.points[i].y = cloud->points[i].y;
+ cloud_host_.points[i].z = cloud->points[i].z;
+
+ bool valid = isFinite(cloud_host_.points[i]);
+
+ hue_host_.points[i] = !valid ? qnan : device::computeHue(cloud->points[i].rgba);
+ depth_host_.points[i] = !valid ? 0 : static_cast<unsigned short>(cloud_host_.points[i].z * 1000); //m -> mm
+ }
+ cloud_device_.upload(cloud_host_.points, cloud_host_.width);
+ hue_device_.upload(hue_host_.points, hue_host_.width);
+ depth_device1_.upload(depth_host_.points, depth_host_.width);
+
+ // uses cloud device, cloud host, depth device, hue device and other buffers
+ return process();
+}
+
+int
+pcl::gpu::people::PeopleDetector::process ()
+{
+ int cols = cloud_device_.cols();
+ int rows = cloud_device_.rows();
+
+ rdf_detector_->process(depth_device1_, cloud_host_, AREA_THRES);
+
+ const RDFBodyPartsDetector::BlobMatrix& sorted = rdf_detector_->getBlobMatrix();
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////
+ // if we found a neck display the tree, and continue with processing
+ if(sorted[Neck].size() != 0)
+ {
+ int c = 0;
+ Tree2 t;
+ buildTree(sorted, cloud_host_, Neck, c, t);
+
+ const std::vector<int>& seed = t.indices.indices;
+
+ std::fill(flowermat_host_.points.begin(), flowermat_host_.points.end(), 0);
+ {
+ //ScopeTime time("shs");
+ shs5(cloud_host_, seed, &flowermat_host_.points[0]);
+ }
+
+ fg_mask_.upload(flowermat_host_.points, cols);
+ device::Dilatation::invoke(fg_mask_, kernelRect5x5_, fg_mask_grown_);
+
+ device::prepareForeGroundDepth(depth_device1_, fg_mask_grown_, depth_device2_);
+
+ //// //////////////////////////////////////////////////////////////////////////////////////////////// //
+ //// The second label evaluation
+
+ rdf_detector_->process(depth_device2_, cloud_host_, AREA_THRES2);
+ const RDFBodyPartsDetector::BlobMatrix& sorted2 = rdf_detector_->getBlobMatrix();
+
+ //brief Test if the second tree is build up correctly
+ if(sorted2[Neck].size() != 0)
+ {
+ Tree2 t2;
+ buildTree(sorted2, cloud_host_, Neck, c, t2);
+ int par = 0;
+ for(int f = 0; f < NUM_PARTS; f++)
+ {
+ /* if(t2.parts_lid[f] == NO_CHILD)
+ {
+ cerr << "1;";
+ par++;
+ }
+ else
+ cerr << "0;";*/
+ }
+ static int counter = 0; // TODO move this logging to PeopleApp
+ //cerr << t2.nr_parts << ";" << par << ";" << t2.total_dist_error << ";" << t2.norm_dist_error << ";" << counter++ << ";" << endl;
+ return 2;
+ }
+ return 1;
+ //output: Tree2 and PointCloud<XYZRGBL>
+ }
+ return 0;
+}
+
+int
+pcl::gpu::people::PeopleDetector::processProb (const pcl::PointCloud<PointTC>::ConstPtr &cloud)
+{
+ allocate_buffers(cloud->height, cloud->width);
+
+ const float qnan = std::numeric_limits<float>::quiet_NaN();
+
+ for(size_t i = 0; i < cloud->points.size(); ++i)
+ {
+ cloud_host_color_.points[i].x = cloud_host_.points[i].x = cloud->points[i].x;
+ cloud_host_color_.points[i].y = cloud_host_.points[i].y = cloud->points[i].y;
+ cloud_host_color_.points[i].z = cloud_host_.points[i].z = cloud->points[i].z;
+ cloud_host_color_.points[i].rgba = cloud->points[i].rgba;
+
+ bool valid = isFinite(cloud_host_.points[i]);
+
+ hue_host_.points[i] = !valid ? qnan : device::computeHue(cloud->points[i].rgba);
+ depth_host_.points[i] = !valid ? 0 : static_cast<unsigned short>(cloud_host_.points[i].z * 1000); //m -> mm
+ }
+ cloud_device_.upload(cloud_host_.points, cloud_host_.width);
+ hue_device_.upload(hue_host_.points, hue_host_.width);
+ depth_device1_.upload(depth_host_.points, depth_host_.width);
+
+ // uses cloud device, cloud host, depth device, hue device and other buffers
+ return processProb();
+}
+
+int
+pcl::gpu::people::PeopleDetector::processProb ()
+{
+ int cols = cloud_device_.cols();
+ int rows = cloud_device_.rows();
+
+ PCL_DEBUG("[pcl::gpu::people::PeopleDetector::processProb] : (D) : called\n");
+
+ // First iteration no tracking can take place
+ if(first_iteration_)
+ {
+ //Process input pointcloud with RDF
+ rdf_detector_->processProb(depth_device1_);
+
+ probability_processor_->SelectLabel(depth_device1_, rdf_detector_->labels_, rdf_detector_->P_l_);
+ }
+ // Join probabilities from previous result
+ else
+ {
+ // Backup P_l_1_ value in P_l_prev_1_;
+ rdf_detector_->P_l_prev_1_.swap(rdf_detector_->P_l_1_);
+ // Backup P_l_2_ value in P_l_prev_2_;
+ rdf_detector_->P_l_prev_2_.swap(rdf_detector_->P_l_2_);
+
+ //Process input pointcloud with RDF
+ rdf_detector_->processProb(depth_device1_);
+
+ // Create Gaussian Kernel for this iteration, in order to smooth P_l_2_
+ float* kernel_ptr_host;
+ int kernel_size = 5;
+ float sigma = 1.0;
+ kernel_ptr_host = probability_processor_->CreateGaussianKernel(sigma, kernel_size);
+ DeviceArray<float> kernel_device(kernel_size * sizeof(float));
+ kernel_device.upload(kernel_ptr_host, kernel_size * sizeof(float));
+
+ // Output kernel for verification
+ PCL_DEBUG("[pcl::gpu::people::PeopleDetector::processProb] : (D) : kernel:\n");
+ for(int i = 0; i < kernel_size; i++)
+ PCL_DEBUG("\t Entry %d \t: %lf\n", i, kernel_ptr_host[i]);
+
+ if(probability_processor_->GaussianBlur(depth_device1_,rdf_detector_->P_l_2_, kernel_device, rdf_detector_->P_l_Gaus_Temp_ ,rdf_detector_->P_l_Gaus_) != 1)
+ PCL_ERROR("[pcl::gpu::people::PeopleDetector::processProb] : (E) : Gaussian Blur failed\n");
+
+ // merge with prior probabilities at this line
+ probability_processor_->CombineProb(depth_device1_, rdf_detector_->P_l_Gaus_, 0.5, rdf_detector_->P_l_, 0.5, rdf_detector_->P_l_Gaus_Temp_);
+ PCL_DEBUG("[pcl::gpu::people::PeopleDetector::processProb] : (D) : CombineProb called\n");
+
+ // get labels
+ probability_processor_->SelectLabel(depth_device1_, rdf_detector_->labels_, rdf_detector_->P_l_Gaus_Temp_);
+ }
+
+ // This executes the connected components
+ rdf_detector_->processSmooth(depth_device1_, cloud_host_, AREA_THRES);
+ // This creates the blobmatrix
+ rdf_detector_->processRelations(person_attribs_);
+
+ // Backup this value in P_l_1_;
+ rdf_detector_->P_l_1_.swap(rdf_detector_->P_l_);
+
+ const RDFBodyPartsDetector::BlobMatrix& sorted = rdf_detector_->getBlobMatrix();
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////
+ // if we found a neck display the tree, and continue with processing
+ if(sorted[Neck].size() != 0)
+ {
+ int c = 0;
+ Tree2 t;
+ buildTree(sorted, cloud_host_, Neck, c, t, person_attribs_);
+
+ const std::vector<int>& seed = t.indices.indices;
+
+ std::fill(flowermat_host_.points.begin(), flowermat_host_.points.end(), 0);
+ {
+ //ScopeTime time("shs");
+ shs5(cloud_host_, seed, &flowermat_host_.points[0]);
+ }
+
+ fg_mask_.upload(flowermat_host_.points, cols);
+ device::Dilatation::invoke(fg_mask_, kernelRect5x5_, fg_mask_grown_);
+
+ device::prepareForeGroundDepth(depth_device1_, fg_mask_grown_, depth_device2_);
+
+ //// //////////////////////////////////////////////////////////////////////////////////////////////// //
+ //// The second label evaluation
+
+ rdf_detector_->processProb(depth_device2_);
+ // TODO: merge with prior probabilities at this line
+
+ // get labels
+ probability_processor_->SelectLabel(depth_device1_, rdf_detector_->labels_, rdf_detector_->P_l_);
+ // This executes the connected components
+ rdf_detector_->processSmooth(depth_device2_, cloud_host_, AREA_THRES2);
+ // This creates the blobmatrix
+ rdf_detector_->processRelations(person_attribs_);
+
+ // Backup this value in P_l_2_;
+ rdf_detector_->P_l_2_.swap(rdf_detector_->P_l_);
+
+ const RDFBodyPartsDetector::BlobMatrix& sorted2 = rdf_detector_->getBlobMatrix();
+
+ //brief Test if the second tree is build up correctly
+ if(sorted2[Neck].size() != 0)
+ {
+ Tree2 t2;
+ buildTree(sorted2, cloud_host_, Neck, c, t2, person_attribs_);
+ int par = 0;
+ for(int f = 0; f < NUM_PARTS; f++)
+ {
+ if(t2.parts_lid[f] == NO_CHILD)
+ {
+ cerr << "1;";
+ par++;
+ }
+ else
+ cerr << "0;";
+ }
+ std::cerr << std::endl;
+ static int counter = 0; // TODO move this logging to PeopleApp
+
+ //cerr << t2.nr_parts << ";" << par << ";" << t2.total_dist_error << ";" << t2.norm_dist_error << ";" << counter++ << ";" << endl;
+ first_iteration_ = false;
+ return 2;
+ }
+ first_iteration_ = false;
+ return 1;
+ //output: Tree2 and PointCloud<XYZRGBL>
+ }
+ first_iteration_ = false;
+ return 0;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace
+{
+ void
+ getProjectedRadiusSearchBox (int rows, int cols, const pcl::device::Intr& intr, const pcl::PointXYZ& point, float squared_radius,
+ int &minX, int &maxX, int &minY, int &maxY)
+ {
+ int min, max;
+
+ float3 q;
+ q.x = intr.fx * point.x + intr.cx * point.z;
+ q.y = intr.fy * point.y + intr.cy * point.z;
+ q.z = point.z;
+
+ // http://www.wolframalpha.com/input/?i=%7B%7Ba%2C+0%2C+b%7D%2C+%7B0%2C+c%2C+d%7D%2C+%7B0%2C+0%2C+1%7D%7D+*+%7B%7Ba%2C+0%2C+0%7D%2C+%7B0%2C+c%2C+0%7D%2C+%7Bb%2C+d%2C+1%7D%7D
+
+ float coeff8 = 1; //K_KT_.coeff (8);
+ float coeff7 = intr.cy; //K_KT_.coeff (7);
+ float coeff4 = intr.fy * intr.fy + intr.cy*intr.cy; //K_KT_.coeff (4);
+
+ float coeff6 = intr.cx; //K_KT_.coeff (6);
+ float coeff0 = intr.fx * intr.fx + intr.cx*intr.cx; //K_KT_.coeff (0);
+
+ float a = squared_radius * coeff8 - q.z * q.z;
+ float b = squared_radius * coeff7 - q.y * q.z;
+ float c = squared_radius * coeff4 - q.y * q.y;
+
+ // a and c are multiplied by two already => - 4ac -> - ac
+ float det = b * b - a * c;
+
+ if (det < 0)
+ {
+ minY = 0;
+ maxY = rows - 1;
+ }
+ else
+ {
+ float y1 = (b - sqrt (det)) / a;
+ float y2 = (b + sqrt (det)) / a;
+
+ min = (int)std::min(floor(y1), floor(y2));
+ max = (int)std::max( ceil(y1), ceil(y2));
+ minY = std::min (rows - 1, std::max (0, min));
+ maxY = std::max (std::min (rows - 1, max), 0);
+ }
+
+ b = squared_radius * coeff6 - q.x * q.z;
+ c = squared_radius * coeff0 - q.x * q.x;
+
+ det = b * b - a * c;
+ if (det < 0)
+ {
+ minX = 0;
+ maxX = cols - 1;
+ }
+ else
+ {
+ float x1 = (b - sqrt (det)) / a;
+ float x2 = (b + sqrt (det)) / a;
+
+ min = (int)std::min (floor(x1), floor(x2));
+ max = (int)std::max ( ceil(x1), ceil(x2));
+ minX = std::min (cols- 1, std::max (0, min));
+ maxX = std::max (std::min (cols - 1, max), 0);
+ }
+ }
+
+ float
+ sqnorm(const pcl::PointXYZ& p1, const pcl::PointXYZ& p2)
+ {
+ float dx = (p1.x - p2.x);
+ float dy = (p1.y - p2.y);
+ float dz = (p1.z - p2.z);
+ return dx*dx + dy*dy + dz*dz;
+ }
+}
+
+void
+pcl::gpu::people::PeopleDetector::shs5(const pcl::PointCloud<PointT> &cloud, const std::vector<int>& indices, unsigned char *mask)
+{
+ pcl::device::Intr intr(fx_, fy_, cx_, cy_);
+ intr.setDefaultPPIfIncorrect(cloud.width, cloud.height);
+
+ const float *hue = &hue_host_.points[0];
+ double squared_radius = CLUST_TOL_SHS * CLUST_TOL_SHS;
+
+ std::vector< std::vector<int> > storage(100);
+
+ // Process all points in the indices vector
+ int total = static_cast<int> (indices.size ());
+#ifdef _OPENMP
+#pragma omp parallel for
+#endif
+ for (int k = 0; k < total; ++k)
+ {
+ int i = indices[k];
+ if (mask[i])
+ continue;
+
+ mask[i] = 255;
+
+ int id = 0;
+#ifdef _OPENMP
+ id = omp_get_thread_num();
+#endif
+ std::vector<int>& seed_queue = storage[id];
+ seed_queue.clear();
+ seed_queue.reserve(cloud.size());
+ int sq_idx = 0;
+ seed_queue.push_back (i);
+
+ PointT p = cloud.points[i];
+ float h = hue[i];
+
+ while (sq_idx < (int)seed_queue.size ())
+ {
+ int index = seed_queue[sq_idx];
+ const PointT& q = cloud.points[index];
+
+ if(!pcl::isFinite (q))
+ continue;
+
+ // search window
+ int left, right, top, bottom;
+ getProjectedRadiusSearchBox(cloud.height, cloud.width, intr, q, squared_radius, left, right, top, bottom);
+
+ int yEnd = (bottom + 1) * cloud.width + right + 1;
+ int idx = top * cloud.width + left;
+ int skip = cloud.width - right + left - 1;
+ int xEnd = idx - left + right + 1;
+
+ for (; xEnd < yEnd; idx += 2*skip, xEnd += 2*cloud.width)
+ for (; idx < xEnd; idx += 2)
+ {
+ if (mask[idx])
+ continue;
+
+ if (sqnorm(cloud.points[idx], q) <= squared_radius)
+ {
+ float h_l = hue[idx];
+
+ if (fabs(h_l - h) < DELTA_HUE_SHS)
+ {
+ seed_queue.push_back (idx);
+ mask[idx] = 255;
+ }
+ }
+ }
+
+ sq_idx++;
+ }
+ }
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * @author: Koen Buys, Anatoly Baksheev
+ */
+
+#include <pcl/gpu/people/person_attribs.h>
+#include <pcl/gpu/people/label_common.h>
+
+#include <boost/foreach.hpp>
+#include <boost/property_tree/xml_parser.hpp>
+#include <boost/property_tree/ptree.hpp>
+
+#include <pcl/console/print.h>
+
+#include <sstream>
+
+pcl::gpu::people::PersonAttribs::PersonAttribs()
+{
+ PCL_DEBUG("[pcl::gpu::people::PersonAttribs] : (D) : Constructor called\n");
+
+ // INIT
+ max_part_size_.resize(pcl::gpu::people::NUM_PARTS);
+
+ part_ideal_length_.resize(pcl::gpu::people::NUM_PARTS);
+ for(int i = 0; i < pcl::gpu::people::NUM_PARTS; i++)
+ part_ideal_length_[i].resize(pcl::gpu::people::MAX_CHILD);
+
+ max_length_offset_.resize(pcl::gpu::people::NUM_PARTS);
+ for(int i = 0; i < pcl::gpu::people::NUM_PARTS; i++)
+ max_length_offset_[i].resize(pcl::gpu::people::MAX_CHILD);
+
+ nr_of_children_.resize(pcl::gpu::people::NUM_PARTS);
+
+ name_ = "generic";
+}
+
+int
+pcl::gpu::people::PersonAttribs::readPersonXMLConfig (std::istream& is)
+{
+ PCL_DEBUG("[pcl::gpu::people::PersonAttribs::readPersonXMLConfig] : (D) : called\n");
+ // Read in the property tree
+ boost::property_tree::ptree pt;
+ read_xml(is,pt);
+
+ // Check file version
+ int version = pt.get<int>("version");
+ if(version != pcl::gpu::people::XML_VERSION)
+ {
+ PCL_ERROR("[pcl::gpu::people::PersonAttribs::readPersonXMLConfig] : (E) : Incompatible XML_VERSIONS\n");
+ return -1;
+ }
+
+ // Check num_parts
+ int num_parts = pt.get<int>("num_parts");
+ if(num_parts != pcl::gpu::people::NUM_PARTS)
+ {
+ PCL_ERROR("[pcl::gpu::people::PersonAttribs::readPersonXMLConfig] : (E) : num_parts doesn't match\n");
+ return -1;
+ }
+
+ // Check num_labels
+ int num_labels = pt.get<int>("num_labels");
+ if(num_labels != pcl::gpu::people::NUM_LABELS)
+ {
+ PCL_ERROR("[pcl::gpu::people::PersonAttribs::readPersonXMLConfig] : (E) : num_labels doesn't match\n");
+ return -1;
+ }
+
+ name_ = pt.get<std::string>("person.name");
+
+ PCL_DEBUG("[pcl::gpu::people::PersonAttribs::readPersonXMLConfig] : (D) : loaded %s\n", name_.c_str());
+
+ // Get max_part_size_
+ for(int i = 0; i < pcl::gpu::people::NUM_PARTS; i++)
+ {
+ std::stringstream path;
+ path << "person.max_part_size.value_" << i;
+ max_part_size_[i] = pt.get<float>(path.str());
+ }
+
+ // Get part_ideal_length
+ for(int i = 0; i < pcl::gpu::people::NUM_PARTS; i++)
+ {
+ for(int j = 0; j < pcl::gpu::people::MAX_CHILD; j++)
+ {
+ std::stringstream path;
+ path << "person.part_ideal_length.value_" << i << ".child_" << j;
+ part_ideal_length_[i][j] = pt.get<float>(path.str());
+ }
+ }
+
+ // Get max_length_offset_
+ for(int i = 0; i < pcl::gpu::people::NUM_PARTS; i++)
+ {
+ for(int j = 0; j < pcl::gpu::people::MAX_CHILD; j++)
+ {
+ std::stringstream path;
+ path << "person.max_length_offset.value_" << i << ".child_" << j;
+ max_length_offset_[i][j] = pt.get<float>(path.str());
+ }
+ }
+
+ // Get nr_of_children
+ for(int i = 0; i < pcl::gpu::people::NUM_PARTS; i++)
+ {
+ std::stringstream path;
+ path << "person.nr_of_children.value_" << i;
+ nr_of_children_[i] = pt.get<float>(path.str());
+ }
+
+ return 0;
+}
+
+void
+pcl::gpu::people::PersonAttribs::writePersonXMLConfig (std::ostream& os)
+{
+ PCL_DEBUG("[pcl::gpu::people::PersonAttribs::writePersonXMLConfig] : (D) : called\n");
+ boost::property_tree::ptree pt;
+
+ // Write global information which is not person specific
+ pt.add("version", static_cast<int> (pcl::gpu::people::XML_VERSION));
+ pt.add("num_parts", static_cast<int> (pcl::gpu::people::NUM_PARTS));
+ pt.add("num_labels", static_cast<int> (NUM_LABELS));
+
+// boost::property_tree::ptree& node = pt.add("person", "");
+// node.put("name", name_);
+
+ // FROM HERE PERSON SPECIFIC STUFF
+ pt.add("person.name", name_);
+
+ // Add max_part_size_
+ for(int i = 0; i < pcl::gpu::people::NUM_PARTS; i++)
+ {
+ std::stringstream path;
+ path << "person.max_part_size.value_" << i;
+ pt.add(path.str(), max_part_size_[i]);
+ }
+
+ // Add part_ideal_length
+ for(int i = 0; i < pcl::gpu::people::NUM_PARTS; i++)
+ {
+ for(int j = 0; j < pcl::gpu::people::MAX_CHILD; j++)
+ {
+ std::stringstream path;
+ path << "person.part_ideal_length.value_" << i << ".child_" << j;
+ pt.add(path.str(), part_ideal_length_[i][j]);
+ }
+ }
+
+ // Add max_length_offset_
+ for(int i = 0; i < pcl::gpu::people::NUM_PARTS; i++)
+ {
+ for(int j = 0; j < pcl::gpu::people::MAX_CHILD; j++)
+ {
+ std::stringstream path;
+ path << "person.max_length_offset.value_" << i << ".child_" << j;
+ pt.add(path.str(), max_length_offset_[i][j]);
+ }
+ }
+
+ // Add nr_of_children_
+ for(int i = 0; i < pcl::gpu::people::NUM_PARTS; i++)
+ {
+ std::stringstream path;
+ path << "person.nr_of_children.value_" << i;
+ pt.add(path.str(), nr_of_children_[i]);
+ }
+
+ write_xml(os,pt);
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * @author: Koen Buys
+ */
+
+#include <pcl/gpu/people/label_common.h>
+#include <pcl/gpu/people/probability_processor.h>
+#include <pcl/console/print.h>
+#include "internal.h"
+
+pcl::gpu::people::ProbabilityProcessor::ProbabilityProcessor()
+{
+ PCL_DEBUG("[pcl::gpu::people::ProbabilityProcessor] : (D) : Constructor called\n");
+ impl_.reset (new device::ProbabilityProc());
+}
+
+/** \brief This will merge the votes from the different trees into one final vote, including probabilistic's **/
+void
+pcl::gpu::people::ProbabilityProcessor::SelectLabel (const Depth& depth, Labels& labels, pcl::device::LabelProbability& probabilities)
+{
+ PCL_DEBUG("[pcl::gpu::people::ProbabilityProcessor::SelectLabel] : (D) : Called\n");
+ impl_->CUDA_SelectLabel(depth, labels, probabilities);
+}
+
+/** \brief This will combine two probabilities according their weight **/
+void
+pcl::gpu::people::ProbabilityProcessor::CombineProb ( const Depth& depth, pcl::device::LabelProbability& probIn1, float weight1,
+ pcl::device::LabelProbability& probIn2, float weight2, pcl::device::LabelProbability& probOut)
+{
+ impl_->CUDA_CombineProb(depth, probIn1, weight1, probIn2, weight2, probOut);
+}
+
+/** \brief This will sum a probability multiplied with it's weight **/
+void
+pcl::gpu::people::ProbabilityProcessor::WeightedSumProb ( const Depth& depth, pcl::device::LabelProbability& probIn, float weight, pcl::device::LabelProbability& probOut)
+{
+ impl_->CUDA_WeightedSumProb(depth, probIn, weight, probOut);
+}
+
+/** \brief This will do a GaussianBlur over the LabelProbability **/
+int
+pcl::gpu::people::ProbabilityProcessor::GaussianBlur( const Depth& depth,
+ pcl::device::LabelProbability& probIn,
+ DeviceArray<float>& kernel,
+ pcl::device::LabelProbability& probOut)
+{
+ return impl_->CUDA_GaussianBlur( depth, probIn, kernel, probOut);
+}
+
+/** \brief This will do a GaussianBlur over the LabelProbability **/
+int
+pcl::gpu::people::ProbabilityProcessor::GaussianBlur( const Depth& depth,
+ pcl::device::LabelProbability& probIn,
+ DeviceArray<float>& kernel,
+ pcl::device::LabelProbability& probTemp,
+ pcl::device::LabelProbability& probOut)
+{
+ return impl_->CUDA_GaussianBlur( depth, probIn, kernel, probTemp, probOut);
+}
+
+/** \brief This will create a Gaussian Kernel **/
+float*
+pcl::gpu::people::ProbabilityProcessor::CreateGaussianKernel ( float sigma,
+ int kernelSize)
+{
+ float* f;
+ f = static_cast<float*> (malloc(kernelSize * sizeof(float)));
+ float sigma_sq = static_cast<float> (pow (sigma,2.f));
+ float mult = static_cast<float> (1/sqrt (2*M_PI*sigma_sq));
+ int mid = static_cast<int> (floor (static_cast<float> (kernelSize)/2.f));
+
+ // Create a symmetric kernel, could also be solved in CUDA kernel but let's do it here :D
+ float sum = 0;
+ for(int i = 0; i < kernelSize; i++)
+ {
+ f[i] = static_cast<float> (mult * exp (-(pow (i-mid,2.f)/2*sigma_sq)));
+ sum += f[i];
+ }
+
+ // Normalize f
+ for(int i = 0; i < kernelSize; i++)
+ {
+ f[i] = f[i]/sum;
+ }
+
+ return f;
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * @authors: Koen Buys, Anatoly Baksheev
+ *
+ */
+
+#include <pcl/gpu/people/tree.h>
+#include <pcl/gpu/people/label_common.h>
+#include <pcl/console/print.h>
+
+#include <fstream>
+#include <string>
+#include <cassert>
+#include <stdexcept>
+#include <limits>
+#include <iostream>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ namespace people
+ {
+ namespace trees
+ {
+ /**
+ * \brief Simple helper structure to fetch the texture without bothering about limits
+ * This will be done by CUDA directly for the run time part of the stuff
+ */
+ struct Tex2Dfetcher
+ {
+ Tex2Dfetcher( const boost::uint16_t* dmap, int W, int H ) : m_dmap(dmap), m_W(W), m_H(H) {}
+
+ inline boost::uint16_t operator () ( float uf, float vf )
+ {
+ int u = static_cast<int>(uf);
+ int v = static_cast<int>(vf);
+ if( u < 0 ) u = 0;
+ if( v < 0 ) v = 0;
+ if( u >= m_W ) u = m_W-1;
+ if( v >= m_H ) v = m_H-1;
+
+ return m_dmap[u+v*m_W]; // this is going to be SLOOOWWW
+ }
+ const boost::uint16_t* m_dmap;
+ const int m_W;
+ const int m_H;
+ };
+ } // end namespace trees
+ } // end namespace people
+ } // end namespace gpu
+} // end namespace pcl
+
+using pcl::gpu::people::trees::NUM_ATTRIBS;
+
+//////////////////////////////////////////////////////////////////////////////////
+// tree_run
+
+int
+pcl::gpu::people::trees::loadTree ( std::istream& is,
+ std::vector<Node>& tree,
+ std::vector<Label>& leaves )
+{
+ // load the depth
+ int maxDepth;
+ is >> maxDepth;
+ int numNodes = (1 << maxDepth) - 1; //pow(2.0,maxDepth)-1;
+ int numLeaves = (1 << maxDepth); //pow(2.0,maxDepth);
+
+ // alloc
+ tree.resize(numNodes);
+ leaves.resize(numLeaves);
+
+ // read
+ for(int ni = 0; ni < numNodes; ++ni)
+ is >> tree[ni];
+
+ // the int is necessary.. otherwise it will format it as unsigned char
+ for(int li = 0; li < numLeaves; ++li)
+ {
+ int l; is >> l; leaves[li] = l;
+ }
+
+ // Check loading of the tree in terminal
+ PCL_DEBUG("[pcl::gpu::people::trees::loadTree] : (D) : loaded %d nodes, %d leaves and depth %d\n", numNodes, numLeaves, maxDepth);
+
+ if( is.fail() )
+ throw std::runtime_error(std::string("(E) malformed *.tree stream") );
+ return maxDepth;
+}
+
+int
+pcl::gpu::people::trees::loadTree ( const std::string& filename,
+ std::vector<Node>& tree,
+ std::vector<Label>& leaves )
+{
+ std::ifstream fin(filename.c_str() );
+ if( !fin.is_open() )
+ throw std::runtime_error(std::string("[pcl::gpu::people::trees::loadTree] : (E) could not open ") + filename );
+ return loadTree(fin, tree, leaves);
+}
+
+void
+pcl::gpu::people::trees::runThroughTree( int maxDepth,
+ const std::vector<Node>& tree,
+ const std::vector<Label>& leaves,
+ int W,
+ int H,
+ const uint16_t* dmap,
+ Label* lmap )
+{
+ Tex2Dfetcher tfetch( dmap, W, H ); // the tex fetcher
+
+ int numNodes = (1 << maxDepth) - 1; //pow(2.0,maxDepth) - 1;
+ for(int y = 0; y < H; ++y)
+ {
+ for(int x = 0; x < W; ++x)
+ {
+ uint16_t depth = tfetch((float)x,(float)y);
+ if(depth == std::numeric_limits<uint16_t>::max() )
+ {
+ lmap[x+W*y] = pcl::gpu::people::NOLABEL;
+ continue;
+ }
+
+ double scale = focal / depth;
+
+ // and now for the tree processing
+ int nid = 0;
+ for(int d = 0; d < maxDepth; ++d)
+ {
+ const Node& node = tree[nid];
+ const AttribLocation& loc = node.loc;
+ uint16_t d1 = tfetch((float)(x+loc.du1*scale), (float)(y+loc.dv1*scale));
+ uint16_t d2 = tfetch((float)(x+loc.du2*scale), (float)(y+loc.dv2*scale));
+ int32_t delta = int32_t(d1) - int32_t(d2);
+ bool test = delta > int32_t(node.thresh);
+
+ nid = test ? (nid*2+2) : (nid*2+1);
+ }
+ lmap[x+W*y] = leaves[nid-numNodes];
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////
+// tree_io
+
+void
+pcl::gpu::people::trees::writeAttribLocs( const std::string& filename,
+ const std::vector<AttribLocation>& alocs )
+{
+ std::ofstream fout(filename.c_str());
+ if(!fout.is_open())
+ throw std::runtime_error(std::string("(E) could not open ") + filename);
+
+ // first we write the number of attribs we intend to write so that we ll avoid mismatches
+ assert( alocs.size() == NUM_ATTRIBS );
+ fout << NUM_ATTRIBS << "\n";
+ for(int ai=0;ai<NUM_ATTRIBS;++ai)
+ fout << alocs[ai];
+}
+
+void
+pcl::gpu::people::trees::readAttribLocs( const std::string& filename,
+ std::vector<AttribLocation>& alocs )
+{
+ std::ifstream fin(filename.c_str() );
+ if(!fin.is_open())
+ throw std::runtime_error(std::string("(E) could not open ") + filename );
+
+ int numAttribs;
+ fin >> numAttribs;
+ if( numAttribs != NUM_ATTRIBS )
+ throw std::runtime_error(std::string("(E) the attribloc file has a wrong number of attribs ") + filename );
+
+ alocs.resize(NUM_ATTRIBS);
+ for(int ai = 0; ai < NUM_ATTRIBS; ++ai)
+ fin >> alocs[ai];
+}
+
+void
+pcl::gpu::people::trees::readThreshs( const std::string& filename,
+ std::vector<Attrib>& threshs )
+{
+ std::ifstream fin(filename.c_str() );
+ if(!fin.is_open())
+ throw std::runtime_error(std::string("(E) could not open " + filename) );
+
+ int numThreshs;
+ fin >> numThreshs;
+ threshs.resize(numThreshs);
+ for(int ti =0;ti<numThreshs;++ti)
+ fin >> threshs[ti];
+
+ if( fin.fail() )
+ throw std::runtime_error(std::string("(E) malformed thresh file: " + filename) );
+}
+
+void
+pcl::gpu::people::trees::writeThreshs( const std::string& filename,
+ const std::vector<Attrib>& threshs )
+{
+ std::ofstream fout(filename.c_str() );
+ if(!fout.is_open())
+ throw std::runtime_error(std::string("(E) could not open " + filename) );
+
+ int numThreshs = threshs.size();
+
+ fout << numThreshs << "\n";
+ for(int ti =0;ti<numThreshs;++ti)
+ fout << threshs[ti] << "\n";
+}
+
--- /dev/null
+if(NOT VTK_FOUND)
+ set(DEFAULT FALSE)
+ set(REASON "VTK was not found.")
+else(NOT VTK_FOUND)
+ set(DEFAULT TRUE)
+ set(REASON)
+ set(VTK_USE_FILE "${VTK_USE_FILE}" CACHE INTERNAL "VTK_USE_FILE")
+ include("${VTK_USE_FILE}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
+endif(NOT VTK_FOUND)
+
+set(the_target people_tracking)
+
+include_directories(${VTK_INCLUDE_DIRS})
+
+#PCL_ADD_EXECUTABLE("${the_target}" "${SUBSYS_NAME}" people_tracking.cpp)
+#target_link_libraries("${the_target}" pcl_common pcl_filters pcl_kdtree pcl_segmentation pcl_kdtree pcl_gpu_people pcl_filters pcl_io pcl_visualization)
+
+if (HAVE_OPENNI)
+ PCL_ADD_EXECUTABLE(pcl_people_app "${SUBSYS_NAME}" people_app.cpp)
+ target_link_libraries (pcl_people_app pcl_common pcl_filters pcl_kdtree pcl_segmentation pcl_kdtree pcl_gpu_people pcl_filters pcl_io pcl_visualization ${Boost_LIBRARIES})
+endif (HAVE_OPENNI)
+
+PCL_ADD_EXECUTABLE(pcl_people_pcd_prob "${SUBSYS_NAME}" people_pcd_prob.cpp)
+target_link_libraries (pcl_people_pcd_prob pcl_common pcl_filters pcl_kdtree pcl_segmentation pcl_kdtree pcl_gpu_people pcl_filters pcl_io pcl_visualization ${Boost_LIBRARIES})
+
+#PCL_ADD_EXECUTABLE(people_pcd_folder "${SUBSYS_NAME}" people_pcd_folder.cpp)
+#target_link_libraries (people_pcd_folder pcl_common pcl_filters pcl_kdtree pcl_segmentation pcl_kdtree pcl_gpu_people pcl_filters pcl_io pcl_visualization ${Boost_LIBRARIES})
+
+#PCL_ADD_EXECUTABLE(people_pcd_person "${SUBSYS_NAME}" people_pcd_person.cpp)
+#target_link_libraries (people_pcd_person pcl_common pcl_filters pcl_kdtree pcl_segmentation pcl_kdtree pcl_gpu_people pcl_filters pcl_io pcl_visualization)
+
+#if(VTK_FOUND)
+# if(QT4_FOUND AND VTK_USE_QVTK)
+# QT4_WRAP_UI(people_proc_ui people_proc/people_proc.ui)
+# QT4_WRAP_CPP(people_proc_moc people_proc/people_proc.h OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED)
+# PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_people_proc "${SUBSYS_NAME}" ${people_proc_ui} ${people_proc_moc} people_proc/people_proc.cpp)
+# target_link_libraries(pcl_people_proc pcl_common pcl_io pcl_visualization pcl_segmentation pcl_features pcl_surface pcl_gpu_people QVTK ${QT_LIBRARIES})
+#
+# endif()
+#endif()
--- /dev/null
+<?xml version="1.0" encoding="utf-8"?>
+<version>1</version>
+<num_parts>25</num_parts>
+<num_labels>32</num_labels>
+<person>
+ <name>generic</name>
+ <max_part_size>
+ <value_0>0.5</value_0>
+ <value_1>0.7</value_1>
+ <value_2>0.6</value_2>
+ <value_3>0.6</value_3>
+ <value_4>0.5</value_4>
+ <value_5>0.7</value_5>
+ <value_6>0.6</value_6>
+ <value_7>0.6</value_7>
+ <value_8>0.9</value_8>
+ <value_9>0.9</value_9>
+ <value_10>0.5</value_10>
+ <value_11>0.7</value_11>
+ <value_12>0.5</value_12>
+ <value_13>0.7</value_13>
+ <value_14>0.5</value_14>
+ <value_15>0.7</value_15>
+ <value_16>0.5</value_16>
+ <value_17>0.7</value_17>
+ <value_18>0.5</value_18>
+ <value_19>0.5</value_19>
+ <value_20>0.5</value_20>
+ <value_21>0.5</value_21>
+ <value_22>0.5</value_22>
+ <value_23>0.9</value_23>
+ <value_24>0.9</value_24>
+ </max_part_size>
+ <part_ideal_length>
+ <value_0>
+ <child_0>-1.0</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_0>
+ <value_1>
+ <child_0>0.2</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_1>
+ <value_2>
+ <child_0>0.2</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_2>
+ <value_3>
+ <child_0>0.3</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_3>
+ <value_4>
+ <child_0>-1.0</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_4>
+ <value_5>
+ <child_0>0.2</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_5>
+ <value_6>
+ <child_0>0.2</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_6>
+ <value_7>
+ <child_0>0.3</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_7>
+ <value_8>
+ <child_0>0.3</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_8>
+ <value_9>
+ <child_0>0.3</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_9>
+ <value_10>
+ <child_0>0.15</child_0>
+ <child_1>0.15</child_1>
+ <child_2>0.2</child_2>
+ <child_3>0.2</child_3>
+ </value_10>
+ <value_11>
+ <child_0>0.15</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_11>
+ <value_12>
+ <child_0>0.1</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_12>
+ <value_13>
+ <child_0>0.15</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_13>
+ <value_14>
+ <child_0>-1.0</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_14>
+ <value_15>
+ <child_0>0.15</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_15>
+ <value_16>
+ <child_0>0.1</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_16>
+ <value_17>
+ <child_0>0.15</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_17>
+ <value_18>
+ <child_0>-1.0</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_18>
+ <value_19>
+ <child_0>0.15</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_19>
+ <value_20>
+ <child_0>0.15</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_20>
+ <value_21>
+ <child_0>-1.0</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_21>
+ <value_22>
+ <child_0>-1.0</child_0>
+ <child_1>-1.0</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_22>
+ <value_23>
+ <child_0>0.3</child_0>
+ <child_1>0.3</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_23>
+ <value_24>
+ <child_0>0.3</child_0>
+ <child_1>0.3</child_1>
+ <child_2>-1.0</child_2>
+ <child_3>-1.0</child_3>
+ </value_24>
+ </part_ideal_length>
+ <max_length_offset>
+ <value_0><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_0>
+ <value_1><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_1>
+ <value_2><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_2>
+ <value_3><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_3>
+ <value_4><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_4>
+ <value_5><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_5>
+ <value_6><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_6>
+ <value_7><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_7>
+ <value_8><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_8>
+ <value_9><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_9>
+ <value_10><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_10>
+ <value_11><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_11>
+ <value_12><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_12>
+ <value_13><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_13>
+ <value_14><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_14>
+ <value_15><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_15>
+ <value_16><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_16>
+ <value_17><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_17>
+ <value_18><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_18>
+ <value_19><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_19>
+ <value_20><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_20>
+ <value_21><child_0>0.3</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_21>
+ <value_22><child_0>0.3</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_22>
+ <value_23><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_23>
+ <value_24><child_0>0.15</child_0><child_1>0.15</child_1><child_2>0.15</child_2><child_3>0.15</child_3></value_24>
+ </max_length_offset>
+ <nr_of_children>
+ <value_0>0</value_0>
+ <value_1>1</value_1>
+ <value_2>1</value_2>
+ <value_3>1</value_3>
+ <value_4>0</value_4>
+ <value_5>1</value_5>
+ <value_6>1</value_6>
+ <value_7>1</value_7>
+ <value_8>1</value_8>
+ <value_9>1</value_9>
+ <value_10>4</value_10>
+ <value_11>1</value_11>
+ <value_12>1</value_12>
+ <value_13>1</value_13>
+ <value_14>0</value_14>
+ <value_15>1</value_15>
+ <value_16>1</value_16>
+ <value_17>1</value_17>
+ <value_18>0</value_18>
+ <value_19>1</value_19>
+ <value_20>1</value_20>
+ <value_21>0</value_21>
+ <value_22>0</value_22>
+ <value_23>2</value_23>
+ <value_24>2</value_24>
+ </nr_of_children>
+</person>
--- /dev/null
+/**
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * @brief This file is the execution node of the Human Tracking
+ * @copyright Copyright (2011) Willow Garage
+ * @authors Koen Buys, Anatoly Baksheev
+ **/
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/common/time.h>
+#include <pcl/exceptions.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
+#include <pcl/gpu/containers/initialization.h>
+#include <pcl/gpu/people/people_detector.h>
+#include <pcl/gpu/people/colormap.h>
+#include <pcl/visualization/image_viewer.h>
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/oni_grabber.h>
+#include <pcl/io/pcd_grabber.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/png_io.h>
+#include <boost/filesystem.hpp>
+
+#include <iostream>
+
+namespace pc = pcl::console;
+using namespace pcl::visualization;
+using namespace pcl::gpu;
+using namespace pcl;
+using namespace std;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+vector<string> getPcdFilesInDir(const string& directory)
+{
+ namespace fs = boost::filesystem;
+ fs::path dir(directory);
+
+ if (!fs::exists(dir) || !fs::is_directory(dir))
+ PCL_THROW_EXCEPTION(pcl::IOException, "Wrong PCD directory");
+
+ vector<string> result;
+ fs::directory_iterator pos(dir);
+ fs::directory_iterator end;
+
+ for(; pos != end ; ++pos)
+ if (fs::is_regular_file(pos->status()) )
+ if (fs::extension(*pos) == ".pcd")
+ result.push_back(pos->path().string());
+
+ return result;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct SampledScopeTime : public StopWatch
+{
+ enum { EACH = 33 };
+ SampledScopeTime(int& time_ms) : time_ms_(time_ms) {}
+ ~SampledScopeTime()
+ {
+ static int i_ = 0;
+ time_ms_ += getTime ();
+ if (i_ % EACH == 0 && i_)
+ {
+ cout << "[~SampledScopeTime] : Average frame time = " << time_ms_ / EACH << "ms ( " << 1000.f * EACH / time_ms_ << "fps )" << endl;
+ time_ms_ = 0;
+ }
+ ++i_;
+ }
+ private:
+ int& time_ms_;
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+string
+make_name(int counter, const char* suffix)
+{
+ char buf[4096];
+ sprintf (buf, "./people%04d_%s.png", counter, suffix);
+ return buf;
+}
+
+template<typename T> void
+savePNGFile(const std::string& filename, const pcl::gpu::DeviceArray2D<T>& arr)
+{
+ int c;
+ pcl::PointCloud<T> cloud(arr.cols(), arr.rows());
+ arr.download(cloud.points, c);
+ pcl::io::savePNGFile(filename, cloud);
+}
+
+template <typename T> void
+savePNGFile (const std::string& filename, const pcl::PointCloud<T>& cloud)
+{
+ pcl::io::savePNGFile(filename, cloud);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+class PeoplePCDApp
+{
+ public:
+ typedef pcl::gpu::people::PeopleDetector PeopleDetector;
+
+ enum { COLS = 640, ROWS = 480 };
+
+ PeoplePCDApp (pcl::Grabber& capture, bool write)
+ : capture_(capture),
+ write_ (write),
+ exit_(false),
+ time_ms_(0),
+ cloud_cb_(true),
+ counter_(0),
+ final_view_("Final labeling"),
+ depth_view_("Depth")
+ {
+ final_view_.setSize (COLS, ROWS);
+ depth_view_.setSize (COLS, ROWS);
+
+ final_view_.setPosition (0, 0);
+ depth_view_.setPosition (650, 0);
+
+ cmap_device_.create(ROWS, COLS);
+ cmap_host_.points.resize(COLS * ROWS);
+ depth_device_.create(ROWS, COLS);
+ image_device_.create(ROWS, COLS);
+
+ depth_host_.points.resize(COLS * ROWS);
+
+ rgba_host_.points.resize(COLS * ROWS);
+ rgb_host_.resize(COLS * ROWS * 3);
+
+ people::uploadColorMap(color_map_);
+
+ }
+
+ void
+ visualizeAndWrite()
+ {
+ const PeopleDetector::Labels& labels = people_detector_.rdf_detector_->getLabels();
+ people::colorizeLabels(color_map_, labels, cmap_device_);
+ //people::colorizeMixedLabels(
+
+ int c;
+ cmap_host_.width = cmap_device_.cols();
+ cmap_host_.height = cmap_device_.rows();
+ cmap_host_.points.resize(cmap_host_.width * cmap_host_.height);
+ cmap_device_.download(cmap_host_.points, c);
+
+ final_view_.showRGBImage<pcl::RGB>(cmap_host_);
+ final_view_.spinOnce(1, true);
+
+ if (cloud_cb_)
+ {
+ depth_host_.width = people_detector_.depth_device1_.cols();
+ depth_host_.height = people_detector_.depth_device1_.rows();
+ depth_host_.points.resize(depth_host_.width * depth_host_.height);
+ people_detector_.depth_device1_.download(depth_host_.points, c);
+ }
+
+ depth_view_.showShortImage(&depth_host_.points[0], depth_host_.width, depth_host_.height, 0, 5000, true);
+ depth_view_.spinOnce(1, true);
+
+ if (write_)
+ {
+ PCL_VERBOSE("PeoplePCDApp::visualizeAndWrite : (I) : Writing to disk");
+ if (cloud_cb_)
+ savePNGFile(make_name(counter_, "ii"), cloud_host_);
+ else
+ savePNGFile(make_name(counter_, "ii"), rgba_host_);
+ savePNGFile(make_name(counter_, "c2"), cmap_host_);
+ savePNGFile(make_name(counter_, "s2"), labels);
+ savePNGFile(make_name(counter_, "d1"), people_detector_.depth_device1_);
+ savePNGFile(make_name(counter_, "d2"), people_detector_.depth_device2_);
+ }
+ }
+
+ void source_cb1(const boost::shared_ptr<const PointCloud<PointXYZRGBA> >& cloud)
+ {
+ {
+ boost::mutex::scoped_lock lock(data_ready_mutex_);
+ if (exit_)
+ return;
+
+ pcl::copyPointCloud(*cloud, cloud_host_);
+ }
+ data_ready_cond_.notify_one();
+ }
+
+ void source_cb2(const boost::shared_ptr<openni_wrapper::Image>& image_wrapper, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_wrapper, float)
+ {
+ {
+ boost::mutex::scoped_try_lock lock(data_ready_mutex_);
+
+ if (exit_ || !lock)
+ return;
+
+ //getting depth
+ int w = depth_wrapper->getWidth();
+ int h = depth_wrapper->getHeight();
+ int s = w * PeopleDetector::Depth::elem_size;
+ const unsigned short *data = depth_wrapper->getDepthMetaData().Data();
+ depth_device_.upload(data, s, h, w);
+
+ depth_host_.points.resize(w *h);
+ depth_host_.width = w;
+ depth_host_.height = h;
+ std::copy(data, data + w * h, &depth_host_.points[0]);
+
+ //getting image
+ w = image_wrapper->getWidth();
+ h = image_wrapper->getHeight();
+ s = w * PeopleDetector::Image::elem_size;
+
+ //fill rgb array
+ rgb_host_.resize(w * h * 3);
+ image_wrapper->fillRGB(w, h, (unsigned char*)&rgb_host_[0]);
+
+ // convert to rgba, TODO image_wrapper should be updated to support rgba directly
+ rgba_host_.points.resize(w * h);
+ rgba_host_.width = w;
+ rgba_host_.height = h;
+ for(int i = 0; i < rgba_host_.size(); ++i)
+ {
+ const unsigned char *pixel = &rgb_host_[i * 3];
+ RGB& rgba = rgba_host_.points[i];
+ rgba.r = pixel[0];
+ rgba.g = pixel[1];
+ rgba.b = pixel[2];
+ }
+ image_device_.upload(&rgba_host_.points[0], s, h, w);
+ }
+ data_ready_cond_.notify_one();
+ }
+
+ void
+ startMainLoop ()
+ {
+ cloud_cb_ = false;
+
+ PCDGrabberBase* ispcd = dynamic_cast<pcl::PCDGrabberBase*>(&capture_);
+ if (ispcd)
+ cloud_cb_= true;
+
+ typedef boost::shared_ptr<openni_wrapper::DepthImage> DepthImagePtr;
+ typedef boost::shared_ptr<openni_wrapper::Image> ImagePtr;
+
+ boost::function<void (const boost::shared_ptr<const PointCloud<PointXYZRGBA> >&)> func1 = boost::bind (&PeoplePCDApp::source_cb1, this, _1);
+ boost::function<void (const ImagePtr&, const DepthImagePtr&, float constant)> func2 = boost::bind (&PeoplePCDApp::source_cb2, this, _1, _2, _3);
+ boost::signals2::connection c = cloud_cb_ ? capture_.registerCallback (func1) : capture_.registerCallback (func2);
+
+ {
+ boost::unique_lock<boost::mutex> lock(data_ready_mutex_);
+
+ try
+ {
+ capture_.start ();
+ while (!exit_ && !final_view_.wasStopped())
+ {
+ bool has_data = data_ready_cond_.timed_wait(lock, boost::posix_time::millisec(100));
+ if(has_data)
+ {
+ SampledScopeTime fps(time_ms_);
+
+ if (cloud_cb_)
+ process_return_ = people_detector_.process(cloud_host_.makeShared());
+ else
+ process_return_ = people_detector_.process(depth_device_, image_device_);
+
+ ++counter_;
+ }
+
+ if(has_data && (process_return_ == 2))
+ visualizeAndWrite();
+ }
+ final_view_.spinOnce (3);
+ }
+ catch (const std::bad_alloc& /*e*/) { cout << "Bad alloc" << endl; }
+ catch (const std::exception& /*e*/) { cout << "Exception" << endl; }
+
+ capture_.stop ();
+ }
+ c.disconnect();
+ }
+
+ boost::mutex data_ready_mutex_;
+ boost::condition_variable data_ready_cond_;
+
+ pcl::Grabber& capture_;
+
+ bool cloud_cb_;
+ bool write_;
+ bool exit_;
+ int time_ms_;
+ int counter_;
+ int process_return_;
+ PeopleDetector people_detector_;
+ PeopleDetector::Image cmap_device_;
+ pcl::PointCloud<pcl::RGB> cmap_host_;
+
+ PeopleDetector::Depth depth_device_;
+ PeopleDetector::Image image_device_;
+
+ pcl::PointCloud<unsigned short> depth_host_;
+ pcl::PointCloud<pcl::RGB> rgba_host_;
+ std::vector<unsigned char> rgb_host_;
+
+ PointCloud<PointXYZRGBA> cloud_host_;
+
+ ImageViewer final_view_;
+ ImageViewer depth_view_;
+
+ DeviceArray<pcl::RGB> color_map_;
+};
+
+void print_help()
+{
+ cout << "\nPeople tracking app options (help):" << endl;
+ cout << "\t -numTrees \t<int> \tnumber of trees to load" << endl;
+ cout << "\t -tree0 \t<path_to_tree_file>" << endl;
+ cout << "\t -tree1 \t<path_to_tree_file>" << endl;
+ cout << "\t -tree2 \t<path_to_tree_file>" << endl;
+ cout << "\t -tree3 \t<path_to_tree_file>" << endl;
+ cout << "\t -gpu \t<GPU_device_id>" << endl;
+ cout << "\t -w \t<bool> \tWrite results to disk" << endl;
+ cout << "\t -h \tPrint this help" << endl;
+ cout << "\t -dev \t<Kinect_device_id>" << endl;
+ cout << "\t -pcd \t<path_to_pcd_file>" << endl;
+ cout << "\t -oni \t<path_to_oni_file>" << endl;
+ cout << "\t -pcd_folder \t<path_to_folder_with_pcd_files>" << endl;
+}
+
+int main(int argc, char** argv)
+{
+ // answering for help
+ PCL_INFO("People tracking App version 0.2\n");
+ if(pc::find_switch (argc, argv, "--help") || pc::find_switch (argc, argv, "-h"))
+ return print_help(), 0;
+
+ // selecting GPU and prining info
+ int device = 0;
+ pc::parse_argument (argc, argv, "-gpu", device);
+ pcl::gpu::setDevice (device);
+ pcl::gpu::printShortCudaDeviceInfo (device);
+
+ bool write = 0;
+ pc::parse_argument (argc, argv, "-w", write);
+
+ // selecting data source
+ boost::shared_ptr<pcl::Grabber> capture;
+ string openni_device, oni_file, pcd_file, pcd_folder;
+
+ try
+ {
+ if (pc::parse_argument (argc, argv, "-dev", openni_device) > 0)
+ {
+ capture.reset( new pcl::OpenNIGrabber(openni_device) );
+ }
+ else
+ if (pc::parse_argument (argc, argv, "-oni", oni_file) > 0)
+ {
+ capture.reset( new pcl::ONIGrabber(oni_file, true, true) );
+ }
+ else
+ if (pc::parse_argument (argc, argv, "-pcd", pcd_file) > 0)
+ {
+ capture.reset( new pcl::PCDGrabber<PointXYZRGBA>(vector<string>(31, pcd_file), 30, true) );
+ }
+ else
+ if (pc::parse_argument (argc, argv, "-pcd_folder", pcd_folder) > 0)
+ {
+ vector<string> pcd_files = getPcdFilesInDir(pcd_folder);
+ capture.reset( new pcl::PCDGrabber<PointXYZRGBA>(pcd_files, 30, true) );
+ }
+ else
+ {
+ capture.reset( new pcl::OpenNIGrabber() );
+ //capture.reset( new pcl::ONIGrabber("d:/onis/20111013-224932.oni", true, true) );
+
+ //vector<string> pcd_files(31, "d:/3/0008.pcd");
+ //vector<string> pcd_files(31, "d:/git/pcl/gpu/people/tools/test.pcd");
+ //vector<string> pcd_files = getPcdFilesInDir("d:/3/");
+ //capture.reset( new pcl::PCDGrabber<PointXYZRGBA>(pcd_files, 30, true) );
+ }
+ }
+ catch (const pcl::PCLException& /*e*/) { return cout << "Can't open depth source" << endl, -1; }
+
+ //selecting tree files
+ vector<string> tree_files;
+ tree_files.push_back("Data/forest1/tree_20.txt");
+ tree_files.push_back("Data/forest2/tree_20.txt");
+ tree_files.push_back("Data/forest3/tree_20.txt");
+ tree_files.push_back("Data/forest4/tree_20.txt");
+
+ pc::parse_argument (argc, argv, "-tree0", tree_files[0]);
+ pc::parse_argument (argc, argv, "-tree1", tree_files[1]);
+ pc::parse_argument (argc, argv, "-tree2", tree_files[2]);
+ pc::parse_argument (argc, argv, "-tree3", tree_files[3]);
+
+ int num_trees = (int)tree_files.size();
+ pc::parse_argument (argc, argv, "-numTrees", num_trees);
+
+ tree_files.resize(num_trees);
+ if (num_trees == 0 || num_trees > 4)
+ {
+ PCL_ERROR("[Main] : Invalid number of trees");
+ print_help();
+ return -1;
+ }
+
+ try
+ {
+ // loading trees
+ typedef pcl::gpu::people::RDFBodyPartsDetector RDFBodyPartsDetector;
+ RDFBodyPartsDetector::Ptr rdf(new RDFBodyPartsDetector(tree_files));
+ PCL_VERBOSE("[Main] : Loaded files into rdf");
+
+ // Create the app
+ PeoplePCDApp app(*capture, write);
+ app.people_detector_.rdf_detector_ = rdf;
+
+ // executing
+ app.startMainLoop ();
+ }
+ catch (const pcl::PCLException& e) { cout << "PCLException: " << e.detailedMessage() << endl; print_help();}
+ catch (const std::runtime_error& e) { cout << e.what() << endl; print_help(); }
+ catch (const std::bad_alloc& /*e*/) { cout << "Bad alloc" << endl; print_help(); }
+ catch (const std::exception& /*e*/) { cout << "Exception" << endl; print_help(); }
+
+ return 0;
+}
+
+
--- /dev/null
+/**
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: $
+ * @brief This file is the execution node of the Human Tracking
+ * @copyright Copyright (2011) Willow Garage
+ * @authors Koen Buys, Anatoly Baksheev
+ **/
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/common/time.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/gpu/people/people_detector.h>
+#include <pcl/gpu/people/colormap.h>
+#include <pcl/visualization/image_viewer.h>
+#include <pcl/search/pcl_search.h>
+#include <Eigen/Core>
+
+#include <pcl/io/png_io.h>
+
+#include <iostream>
+#include <fstream>
+
+using namespace pcl::visualization;
+using namespace pcl::console;
+using namespace pcl::gpu;
+using namespace std;
+
+typedef pcl::PointXYZRGBA PointT;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct ProjMatrix : public pcl::search::OrganizedNeighbor<PointT>
+{
+ using pcl::search::OrganizedNeighbor<PointT>::projection_matrix_;
+};
+
+float estimateFocalLength(const pcl::PointCloud<PointT>::ConstPtr &cloud)
+{
+ ProjMatrix proj_matrix;
+ proj_matrix.setInputCloud(cloud);
+ Eigen::Matrix3f KR = proj_matrix.projection_matrix_.topLeftCorner <3, 3> ();
+ return (KR(0,0) + KR(1,1))/KR(2,2)/2;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+string
+make_name(int counter, const char* suffix)
+{
+ char buf[4096];
+ sprintf (buf, "./people%04d_%s.png", counter, suffix);
+ return buf;
+}
+
+string
+make_ext_name(int counter1, int counter2, const char* suffix)
+{
+ char buf[4096];
+ sprintf (buf, "./people_%04d_%04d_%s.png", counter1, counter2, suffix);
+ return buf;
+}
+
+template<typename T> void
+savePNGFile(const std::string& filename, const pcl::gpu::DeviceArray2D<T>& arr)
+{
+ int c;
+ pcl::PointCloud<T> cloud(arr.cols(), arr.rows());
+ arr.download(cloud.points, c);
+ pcl::io::savePNGFile(filename, cloud);
+}
+
+template <typename T> void
+savePNGFile (const std::string& filename, const pcl::PointCloud<T>& cloud)
+{
+ pcl::io::savePNGFile(filename, cloud);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+class PeoplePCDApp
+{
+ public:
+ typedef pcl::gpu::people::PeopleDetector PeopleDetector;
+
+ enum { COLS = 640, ROWS = 480 };
+
+ PeoplePCDApp () : counter_(0), cmap_device_(ROWS, COLS)//, final_view_("Final labeling")//, image_view_("Input image")
+ {
+ //final_view_.setSize (COLS, ROWS);
+ //image_view_.setSize (COLS, ROWS);
+
+ //final_view_.setPosition (0, 0);
+ //image_view_.setPosition (650, 0);
+
+ people::uploadColorMap(color_map_);
+ }
+
+ void cloud_cb (const pcl::PointCloud<PointT>::ConstPtr &cloud)
+ {
+ PCL_DEBUG("[PeoplePCDApp::cloud_cb] : (D) : Cloud Callback\n");
+ processReturn = people_detector_.processProb(cloud);
+ ++counter_;
+ PCL_DEBUG("[PeoplePCDApp::cloud_cb] : (D) : Done\n");
+ }
+
+ void
+ writeXMLFile(std::string& filename)
+ {
+ filebuf fb;
+ fb.open (filename.c_str(), ios::out);
+ ostream os(&fb);
+ people_detector_.person_attribs_->writePersonXMLConfig(os);
+ fb.close();
+ }
+
+ void
+ readXMLFile(std::string& filename)
+ {
+ filebuf fb;
+ fb.open (filename.c_str(), ios::in);
+ istream is(&fb);
+ people_detector_.person_attribs_->readPersonXMLConfig(is);
+ fb.close();
+ }
+
+ void
+ visualizeAndWrite(const pcl::PointCloud<PointT>::ConstPtr &cloud)
+ {
+ PCL_DEBUG("[PeoplePCDApp::visualizeAndWrite] : (D) : called\n");
+ const PeopleDetector::Labels& labels = people_detector_.rdf_detector_->getLabels();
+ people::colorizeLabels(color_map_, labels, cmap_device_);
+
+ int c;
+ pcl::PointCloud<pcl::RGB> cmap(cmap_device_.cols(), cmap_device_.rows());
+ cmap_device_.download(cmap.points, c);
+
+ //final_view_.showRGBImage<pcl::RGB>(cmap);
+ //final_view_.spinOnce(1, true);
+
+ //image_view_.showRGBImage<PointT>(cloud);
+ //image_view_.spinOnce(1, true);
+
+ savePNGFile(make_name(counter_, "ii"), *cloud);
+ savePNGFile(make_name(counter_, "c2"), cmap);
+ savePNGFile(make_name(counter_, "s2"), labels);
+ savePNGFile(make_name(counter_, "d1"), people_detector_.depth_device1_);
+ savePNGFile(make_name(counter_, "d2"), people_detector_.depth_device2_);
+
+ PCL_DEBUG("[PeoplePCDApp::visualizeAndWrite] : (D) : Done writing files\n");
+ }
+
+ void
+ convertProbToRGB (pcl::PointCloud<pcl::device::prob_histogram>& histograms, int label, pcl::PointCloud<pcl::RGB>& rgb)
+ {
+ for(size_t t = 0; t < histograms.points.size(); t++)
+ {
+ float value = histograms.points[t].probs[label];
+ float value8 = value * 255;
+ char val = static_cast<char> (value8);
+ pcl::RGB p;
+ p.r = val; p.b = val; p.g = val;
+ rgb.points.push_back(p);
+ }
+ rgb.width = histograms.width;
+ rgb.height = histograms.height;
+ }
+
+ void
+ writeProb(const pcl::PointCloud<PointT>::ConstPtr &cloud)
+ {
+ PCL_DEBUG("[PeoplePCDApp::writeProb] : (D) : Called\n");
+ //const pcl::device::LabelProbability& prob = people_detector_.rdf_detector_->getProbability1();
+ int c;
+ // first write the first iteration
+
+ pcl::PointCloud<pcl::device::prob_histogram> prob_host(people_detector_.rdf_detector_->P_l_1_.cols(), people_detector_.rdf_detector_->P_l_1_.rows());
+ people_detector_.rdf_detector_->P_l_1_.download(prob_host.points, c);
+ prob_host.width = people_detector_.rdf_detector_->P_l_1_.cols();
+ prob_host.height = people_detector_.rdf_detector_->P_l_1_.rows();
+
+ PCL_DEBUG("[PeoplePCDApp::writeProb] : (D) : savePNGFile\n");
+ for(int i = 0; i < pcl::gpu::people::NUM_LABELS; i++)
+ {
+ pcl::PointCloud<pcl::RGB> rgb;
+ convertProbToRGB(prob_host, i, rgb);
+ savePNGFile(make_ext_name(counter_,i, "hist1"), rgb);
+ }
+
+ PCL_DEBUG("[PeoplePCDApp::writeProb] : (D) : cols1: %d\n", people_detector_.rdf_detector_->P_l_1_.cols ());
+ PCL_DEBUG("[PeoplePCDApp::writeProb] : (D) : rows1: %d\n", people_detector_.rdf_detector_->P_l_1_.rows ());
+
+ // and now again for the second iteration
+
+ pcl::PointCloud<pcl::device::prob_histogram> prob_host2(people_detector_.rdf_detector_->P_l_2_.cols(), people_detector_.rdf_detector_->P_l_2_.rows());
+ people_detector_.rdf_detector_->P_l_2_.download(prob_host2.points, c);
+ prob_host.width = people_detector_.rdf_detector_->P_l_2_.cols();
+ prob_host.height = people_detector_.rdf_detector_->P_l_2_.rows();
+
+ PCL_DEBUG("[PeoplePCDApp::writeProb] : (D) : savePNGFile\n");
+ for(int i = 0; i < pcl::gpu::people::NUM_LABELS; i++)
+ {
+ pcl::PointCloud<pcl::RGB> rgb;
+ convertProbToRGB(prob_host2, i, rgb);
+ savePNGFile(make_ext_name(counter_, i, "hist2"), rgb);
+ }
+ PCL_DEBUG("[PeoplePCDApp::writeProb] : (D) : cols2: %d\n", people_detector_.rdf_detector_->P_l_2_.cols());
+ PCL_DEBUG("[PeoplePCDApp::writeProb] : (D) : rows2: %d\n", people_detector_.rdf_detector_->P_l_2_.rows());
+
+ // and now again for the Gaus test
+
+ pcl::PointCloud<pcl::device::prob_histogram> prob_host3(people_detector_.rdf_detector_->P_l_Gaus_.cols(), people_detector_.rdf_detector_->P_l_Gaus_.rows());
+ people_detector_.rdf_detector_->P_l_Gaus_.download(prob_host3.points, c);
+ prob_host3.width = people_detector_.rdf_detector_->P_l_Gaus_.cols();
+ prob_host3.height = people_detector_.rdf_detector_->P_l_Gaus_.rows();
+ for(int i = 0; i < pcl::gpu::people::NUM_LABELS; i++)
+ {
+ pcl::PointCloud<pcl::RGB> rgb;
+ convertProbToRGB(prob_host3, i, rgb);
+ savePNGFile(make_ext_name(counter_, i, "gaus"), rgb);
+ }
+ }
+
+ int counter_;
+ int processReturn;
+ PeopleDetector people_detector_;
+ PeopleDetector::Image cmap_device_;
+
+ //ImageViewer final_view_;
+ //ImageViewer image_view_;
+
+ pcl::gpu::DeviceArray<pcl::RGB> color_map_;
+};
+
+void print_help()
+{
+ PCL_INFO("\nPeople tracking app options (help):\n");
+ PCL_INFO("\t -numTrees \t<int> \tnumber of trees to load\n");
+ PCL_INFO("\t -tree0 \t<path_to_tree_file>\n");
+ PCL_INFO("\t -tree1 \t<path_to_tree_file>\n");
+ PCL_INFO("\t -tree2 \t<path_to_tree_file>\n");
+ PCL_INFO("\t -tree3 \t<path_to_tree_file>\n");
+ PCL_INFO("\t -prob \t<bool> \tsave prob files or not, default 0\n");
+ PCL_INFO("\t -debug \t<bool> \tset debug output");
+ PCL_INFO("\t -pcd \t<path_to_pcd_file>\n");
+ PCL_INFO("\t -XML \t<path_to_XML_file> \tcontains person specifics, defaults to generic.xml\n");
+}
+
+int main(int argc, char** argv)
+{
+ PCL_INFO("[Main] : (I) : People tracking on PCD files version 0.2\n");
+ if(find_switch (argc, argv, "--help") || find_switch (argc, argv, "-h"))
+ return print_help(), 0;
+
+ bool saveProb = 1;
+ parse_argument (argc, argv, "-prob", saveProb);
+
+ bool debugOutput = 0;
+ parse_argument (argc, argv, "-debug", saveProb);
+ if(debugOutput)
+ setVerbosityLevel(L_DEBUG);
+
+ std::string treeFilenames[4] =
+ {
+ "d:/TreeData/results/forest1/tree_20.txt",
+ "d:/TreeData/results/forest2/tree_20.txt",
+ "d:/TreeData/results/forest3/tree_20.txt",
+ "d:/TreeData/results/forest3/tree_20.txt"
+ };
+ int numTrees = 4;
+ parse_argument (argc, argv, "-numTrees", numTrees);
+ parse_argument (argc, argv, "-tree0", treeFilenames[0]);
+ parse_argument (argc, argv, "-tree1", treeFilenames[1]);
+ parse_argument (argc, argv, "-tree2", treeFilenames[2]);
+ parse_argument (argc, argv, "-tree3", treeFilenames[3]);
+
+ std::string XMLfilename("generic.xml");
+ parse_argument (argc, argv, "-XML", XMLfilename);
+ PCL_DEBUG("[Main] : (D) : Will read XML %s\n", XMLfilename.c_str());
+
+ if (numTrees == 0 || numTrees > 4)
+ {
+ PCL_ERROR("[Main] : (E) : Main : Invalid number of trees\n");
+ return -1;
+ }
+ PCL_DEBUG("[Main] : (D) : Read %d Trees\n", numTrees);
+
+ std::string pcdname("/home/u0062536/Data/pcd/koen.pcd");
+ parse_argument (argc, argv, "-pcd", pcdname);
+
+ PCL_DEBUG("[Main] : (D) : Will read %s\n", pcdname.c_str());
+
+ // loading cloud file
+ pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
+
+ //int res = pcl::io::loadPCDFile<PointT> ("/home/u0062536/Data/pcd/koen.pcd", *cloud);
+ int res = pcl::io::loadPCDFile<PointT> (pcdname, *cloud);
+
+ if (res == -1) //* load the file
+ {
+ PCL_ERROR("[Main] : (E) : Couldn't read cloud file\n");
+ return res;
+ }
+
+ //PCL_INFO("(I) : Main : Loaded %d data points from %s",cloud->width * cloud->height, pcdname);
+
+ // loading trees
+ using pcl::gpu::people::RDFBodyPartsDetector;
+
+ vector<string> names_vector(treeFilenames, treeFilenames + numTrees);
+ PCL_DEBUG("[Main] : (D) : Trees collected\n");
+ RDFBodyPartsDetector::Ptr rdf(new RDFBodyPartsDetector(names_vector));
+ PCL_DEBUG("[Main] : (D) : Loaded files into rdf\n");
+
+ // Create the app
+ PeoplePCDApp app;
+ PCL_DEBUG("[Main] : (D) : App created");
+ app.people_detector_.rdf_detector_ = rdf;
+
+ // Read in person specific configuration
+ app.readXMLFile(XMLfilename);
+ PCL_DEBUG("[Main] : (D) : filename %s\n", XMLfilename.c_str());
+
+ /// Run the app
+ //{
+ //pcl::ScopeTime frame_time("[Main] : (I) : frame_time");
+ app.cloud_cb(cloud);
+ //}
+ if(app.processReturn == 1)
+ {
+ PCL_INFO("[Main] : (I) : calling visualisation and write return 1\n");
+ app.visualizeAndWrite(cloud);
+ if(saveProb)
+ app.writeProb(cloud);
+ }
+ else if(app.processReturn == 2)
+ {
+ PCL_INFO("[Main] : (I) : calling visualisation and write return 2\n");
+ app.visualizeAndWrite(cloud);
+ if(saveProb)
+ app.writeProb(cloud);
+ }
+ else
+ {
+ PCL_INFO("[Main] : (I) : no good person found\n");
+ }
+ //app.final_view_.spin();
+
+ return 0;
+}
--- /dev/null
+/**
+ * @brief This file is the execution node of the Human Tracking
+ * @copyright Copyright (2011) Willow Garage
+ * @authors Koen Buys, Anatoly Baksheev
+ **/
+
+////// ALL INCLUDES /////////////
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/kdtree/kdtree.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/segmentation/extract_labeled_clusters.h>
+#include <pcl/segmentation/seeded_hue_segmentation.h>
+#include <pcl/people/conversion/conversions.h>
+#include <opencv2/opencv.hpp>
+#include <pcl/people/label_skeleton/conversion.h>
+#include <pcl/people/trees/tree_live.h>
+//#include <pcl/filters/passthrough.h>
+//#include <pcl/common/time.h>
+#include <pcl/io/openni_grabber.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/console/parse.h>
+
+class PeopleTrackingApp
+{
+ public:
+ PeopleTrackingApp () : viewer ("PCL People Tracking App") {}
+
+ void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud)
+ {
+ if (!viewer.wasStopped())
+ viewer.showCloud (cloud);
+ ////////////////////CALLBACK IMPL/////////////////////
+
+ /// @todo rewrite this to a pointcloud::Ptr
+ pcl::PointCloud<pcl::PointXYZRGB> cloud_in;
+ pcl::PointCloud<pcl::PointXYZRGB> cloud_in_filt;
+ cloud_in = *cloud;
+
+ cv::Mat dmat(cloud_in.height, cloud_in.width, CV_16U);
+
+ // Project pointcloud back into the imageplane
+ // TODO: do this directly in GPU?
+ pcl::people::label_skeleton::makeDepthImage16FromPointCloud(dmat, cloud_in);
+
+ // Process the depthimage (CUDA)
+ m_proc->process(dmat, m_lmap);
+
+ ////////////////////END CALLBACK IMPL/////////////////
+ }
+
+ void run ()
+ {
+ pcl::Grabber* interface = new pcl::OpenNIGrabber();
+
+ boost::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f =
+ boost::bind (&PeopleTrackingApp::cloud_cb_, this, _1);
+
+ interface->registerCallback (f);
+ interface->start ();
+
+ while (!viewer.wasStopped())
+ {
+ boost::this_thread::sleep (boost::posix_time::seconds (1));
+ }
+ interface->stop ();
+ }
+
+ pcl::visualization::CloudViewer viewer;
+ pcl::people::trees::MultiTreeLiveProc* m_proc;
+ cv::Mat m_lmap;
+ cv::Mat m_cmap;
+ cv::Mat cmap;
+ cv::Mat m_bmap;
+};
+
+int print_help()
+{
+ cout << "\nPeople tracking app options:" << std::endl;
+ cout << "\t -numTrees \t<int> \tnumber of trees to load" << std::endl;
+ cout << "\t -tree0 \t<path_to_tree_file>" << std::endl;
+ cout << "\t -tree1 \t<path_to_tree_file>" << std::endl;
+ cout << "\t -tree2 \t<path_to_tree_file>" << std::endl;
+ cout << "\t -tree3 \t<path_to_tree_file>" << std::endl;
+ return 0;
+}
+
+int main(int argc, char** argv)
+{
+ if(pcl::console::find_switch (argc, argv, "--help") || pcl::console::find_switch (argc, argv, "-h"))
+ return print_help();
+
+ std::string treeFilenames[4];
+ int numTrees;
+ pcl::console::parse_argument (argc, argv, "-numTrees", numTrees);
+ pcl::console::parse_argument (argc, argv, "-tree0", treeFilenames[0]);
+ pcl::console::parse_argument (argc, argv, "-tree1", treeFilenames[1]);
+ pcl::console::parse_argument (argc, argv, "-tree2", treeFilenames[2]);
+ pcl::console::parse_argument (argc, argv, "-tree3", treeFilenames[3]);
+ //Don't know if this assert is still needed with pcl::console?
+ assert(numTrees > 0 );
+ assert(numTrees <= 4 );
+
+ /// Create the app
+ PeopleTrackingApp app;
+
+ /// Load the first tree
+ std::ifstream fin0(treeFilenames[0].c_str() );
+ assert(fin0.is_open() );
+ app.m_proc = new pcl::people::trees::MultiTreeLiveProc(fin0);
+ fin0.close();
+
+ /// Load the other tree files
+ for(int ti=1;ti<numTrees;++ti) {
+ std::ifstream fin(treeFilenames[ti].c_str() );
+ assert(fin.is_open() );
+ app.m_proc->addTree(fin);
+ fin.close();
+ }
+ /// Run the app
+ app.run();
+ return 0;
+}
--- /dev/null
+#!/bin/bash
+/data/svn/pcl/trunk/build/bin/pcl_people_pcd_prob -numTrees 3 -tree0 /data/results/forest1/tree_20.txt -tree1 /data/results/forest2/tree_20.txt -tree2 /data/results/forest3/tree_20.txt -tree3 /data/results/forest3/tree_20.txt -mask 0 -FG 0 -pcd $1
+
--- /dev/null
+#!/bin/bash
+../../../build/bin/people_app -numTrees 3 -tree0 ../data/results/forest1/tree_20.txt -tree1 ../data/results/forest2/tree_20.txt -tree2 ../data/results/forest3/tree_20.txt -tree3 ../data/results/forest3/tree_20.txt -mask 0 -FG 0 $1
+
--- /dev/null
+set(SUBSYS_NAME gpu_segmentation)
+set(SUBSYS_PATH gpu/segmentation)
+set(SUBSYS_DESC "Point cloud GPU segmentation library")
+set(SUBSYS_DEPS common gpu_containers gpu_utils gpu_octree)
+
+set(build TRUE)
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS})
+mark_as_advanced("BUILD_${SUBSYS_NAME}")
+
+PCL_ADD_DOC("${SUBSYS_NAME}")
+
+if(build)
+ set(srcs src/extract_clusters.cpp
+ )
+
+ set(incs include/pcl/gpu/segmentation/gpu_extract_clusters.h
+ include/pcl/gpu/segmentation/gpu_extract_labeled_clusters.h
+ include/pcl/gpu/segmentation/gpu_seeded_hue_segmentation.h
+ )
+
+ set(impl_incs include/pcl/gpu/segmentation/impl/gpu_extract_clusters.hpp
+ include/pcl/gpu/segmentation/impl/gpu_extract_labeled_clusters.hpp
+ include/pcl/gpu/segmentation/impl/gpu_seeded_hue_segmentation.hpp
+ )
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
+ PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs})
+ target_link_libraries("${LIB_NAME}" pcl_gpu_octree pcl_gpu_utils pcl_gpu_containers)
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}"
+ "${SUBSYS_DEPS}" "" "" "" "")
+
+ # Install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}" ${incs})
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}/impl" ${impl_incs})
+endif(build)
+
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id:$
+ * @author: Koen Buys
+ */
+
+#ifndef PCL_GPU_EXTRACT_CLUSTERS_H_
+#define PCL_GPU_EXTRACT_CLUSTERS_H_
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/PointIndices.h>
+#include <pcl/pcl_macros.h>
+#include <pcl/gpu/octree/octree.hpp>
+#include <pcl/gpu/containers/device_array.h>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ void
+ extractEuclideanClusters (const boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > &host_cloud_,
+ const pcl::gpu::Octree::Ptr &tree,
+ float tolerance,
+ std::vector<PointIndices> &clusters,
+ unsigned int min_pts_per_cluster,
+ unsigned int max_pts_per_cluster);
+
+ /** \brief @b EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense, depending on pcl::gpu::octree
+ * \author Koen Buys, Radu Bogdan Rusu
+ * \ingroup segmentation
+ */
+ class EuclideanClusterExtraction
+ {
+ public:
+ typedef pcl::PointXYZ PointType;
+ typedef pcl::PointCloud<pcl::PointXYZ> PointCloudHost;
+ typedef PointCloudHost::Ptr PointCloudHostPtr;
+ typedef PointCloudHost::ConstPtr PointCloudHostConstPtr;
+
+ typedef PointIndices::Ptr PointIndicesPtr;
+ typedef PointIndices::ConstPtr PointIndicesConstPtr;
+
+ typedef pcl::gpu::Octree GPUTree;
+ typedef pcl::gpu::Octree::Ptr GPUTreePtr;
+
+ typedef pcl::gpu::Octree::PointCloud CloudDevice;
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Empty constructor. */
+ EuclideanClusterExtraction () : min_pts_per_cluster_ (1), max_pts_per_cluster_ (std::numeric_limits<int>::max ())
+ {};
+
+ /** \brief the destructor */
+/* ~EuclideanClusterExtraction ()
+ {
+ tree_.clear();
+ };
+*/
+ /** \brief Provide a pointer to the search object.
+ * \param tree a pointer to the spatial search object.
+ */
+ inline void setSearchMethod (GPUTreePtr &tree) { tree_ = tree; }
+
+ /** \brief Get a pointer to the search method used.
+ * @todo fix this for a generic search tree
+ */
+ inline GPUTreePtr getSearchMethod () { return (tree_); }
+
+ /** \brief Set the spatial cluster tolerance as a measure in the L2 Euclidean space
+ * \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space
+ */
+ inline void setClusterTolerance (double tolerance) { cluster_tolerance_ = tolerance; }
+
+ /** \brief Get the spatial cluster tolerance as a measure in the L2 Euclidean space. */
+ inline double getClusterTolerance () { return (cluster_tolerance_); }
+
+ /** \brief Set the minimum number of points that a cluster needs to contain in order to be considered valid.
+ * \param min_cluster_size the minimum cluster size
+ */
+ inline void setMinClusterSize (int min_cluster_size) { min_pts_per_cluster_ = min_cluster_size; }
+
+ /** \brief Get the minimum number of points that a cluster needs to contain in order to be considered valid. */
+ inline int getMinClusterSize () { return (min_pts_per_cluster_); }
+
+ /** \brief Set the maximum number of points that a cluster needs to contain in order to be considered valid.
+ * \param max_cluster_size the maximum cluster size
+ */
+ inline void setMaxClusterSize (int max_cluster_size) { max_pts_per_cluster_ = max_cluster_size; }
+
+ /** \brief Get the maximum number of points that a cluster needs to contain in order to be considered valid. */
+ inline int getMaxClusterSize () { return (max_pts_per_cluster_); }
+
+ inline void setInput (CloudDevice input) {input_ = input;}
+
+ inline void setHostCloud (PointCloudHostPtr host_cloud) {host_cloud_ = host_cloud;}
+
+ /** \brief Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
+ * \param clusters the resultant point clusters
+ */
+ void extract (std::vector<pcl::PointIndices> &clusters);
+
+ protected:
+ /** \brief the input cloud on the GPU */
+ CloudDevice input_;
+
+ /** \brief the original cloud the Host */
+ PointCloudHostPtr host_cloud_;
+
+ /** \brief A pointer to the spatial search object. */
+ GPUTreePtr tree_;
+
+ /** \brief The spatial cluster tolerance as a measure in the L2 Euclidean space. */
+ double cluster_tolerance_;
+
+ /** \brief The minimum number of points that a cluster needs to contain in order to be considered valid (default = 1). */
+ int min_pts_per_cluster_;
+
+ /** \brief The maximum number of points that a cluster needs to contain in order to be considered valid (default = MAXINT). */
+ int max_pts_per_cluster_;
+
+ /** \brief Class getName method. */
+ virtual std::string getClassName () const { return ("gpu::EuclideanClusterExtraction"); }
+ };
+ /** \brief Sort clusters method (for std::sort).
+ * \ingroup segmentation
+ */
+ inline bool
+ comparePointClusters (const pcl::PointIndices &a, const pcl::PointIndices &b)
+ {
+ return (a.indices.size () < b.indices.size ());
+ }
+ }
+}
+
+#endif //PCL_GPU_EXTRACT_CLUSTERS_H_
+
+
+
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id:$
+ *
+ */
+
+#ifndef PCL_GPU_EXTRACT_LABELED_CLUSTERS_H_
+#define PCL_GPU_EXTRACT_LABELED_CLUSTERS_H_
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/PointIndices.h>
+#include <pcl/pcl_macros.h>
+#include <pcl/gpu/octree/octree.hpp>
+#include <pcl/gpu/containers/impl/device_array.hpp>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ template <typename PointT> void
+ extractLabeledEuclideanClusters (const boost::shared_ptr<pcl::PointCloud<PointT> > &host_cloud_,
+ const pcl::gpu::Octree::Ptr &tree,
+ float tolerance,
+ std::vector<PointIndices> &clusters,
+ unsigned int min_pts_per_cluster,
+ unsigned int max_pts_per_cluster);
+
+ /** \brief @b EuclideanLabeledClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense, depending on pcl::gpu::octree
+ * \author Koen Buys, Radu Bogdan Rusu
+ * \ingroup segmentation
+ */
+ template <typename PointT>
+ class EuclideanLabeledClusterExtraction
+ {
+ public:
+ typedef pcl::PointXYZ PointType;
+ typedef pcl::PointCloud<PointT> PointCloudHost;
+ typedef typename PointCloudHost::Ptr PointCloudHostPtr;
+ typedef typename PointCloudHost::ConstPtr PointCloudHostConstPtr;
+
+ typedef PointIndices::Ptr PointIndicesPtr;
+ typedef PointIndices::ConstPtr PointIndicesConstPtr;
+
+ typedef pcl::gpu::Octree GPUTree;
+ typedef pcl::gpu::Octree::Ptr GPUTreePtr;
+
+ typedef pcl::gpu::Octree::PointCloud CloudDevice;
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Empty constructor. */
+ EuclideanLabeledClusterExtraction () : min_pts_per_cluster_ (1), max_pts_per_cluster_ (std::numeric_limits<int>::max ())
+ {};
+
+ /** \brief Provide a pointer to the search object.
+ * \param tree a pointer to the spatial search object.
+ */
+ inline void setSearchMethod (const GPUTreePtr &tree) { tree_ = tree; }
+
+ /** \brief Get a pointer to the search method used.
+ * @todo fix this for a generic search tree
+ */
+ inline GPUTreePtr getSearchMethod () { return (tree_); }
+
+ /** \brief Set the spatial cluster tolerance as a measure in the L2 Euclidean space
+ * \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space
+ */
+ inline void setClusterTolerance (double tolerance) { cluster_tolerance_ = tolerance; }
+
+ /** \brief Get the spatial cluster tolerance as a measure in the L2 Euclidean space. */
+ inline double getClusterTolerance () { return (cluster_tolerance_); }
+
+ /** \brief Set the minimum number of points that a cluster needs to contain in order to be considered valid.
+ * \param min_cluster_size the minimum cluster size
+ */
+ inline void setMinClusterSize (int min_cluster_size) { min_pts_per_cluster_ = min_cluster_size; }
+
+ /** \brief Get the minimum number of points that a cluster needs to contain in order to be considered valid. */
+ inline int getMinClusterSize () { return (min_pts_per_cluster_); }
+
+ /** \brief Set the maximum number of points that a cluster needs to contain in order to be considered valid.
+ * \param max_cluster_size the maximum cluster size
+ */
+ inline void setMaxClusterSize (int max_cluster_size) { max_pts_per_cluster_ = max_cluster_size; }
+
+ /** \brief Get the maximum number of points that a cluster needs to contain in order to be considered valid. */
+ inline int getMaxClusterSize () { return (max_pts_per_cluster_); }
+
+ inline void setInput (CloudDevice input) {input_ = input;}
+
+ inline void setHostCloud (PointCloudHostPtr host_cloud) {host_cloud_ = host_cloud;}
+
+ /** \brief Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
+ * \param clusters the resultant point clusters
+ */
+ void extract (std::vector<PointIndices> &clusters);
+
+ protected:
+ /** \brief the input cloud on the GPU */
+ CloudDevice input_;
+
+ /** \brief the original cloud the Host */
+ PointCloudHostPtr host_cloud_;
+
+ /** \brief A pointer to the spatial search object. */
+ GPUTreePtr tree_;
+
+ /** \brief The spatial cluster tolerance as a measure in the L2 Euclidean space. */
+ double cluster_tolerance_;
+
+ /** \brief The minimum number of points that a cluster needs to contain in order to be considered valid (default = 1). */
+ int min_pts_per_cluster_;
+
+ /** \brief The maximum number of points that a cluster needs to contain in order to be considered valid (default = MAXINT). */
+ int max_pts_per_cluster_;
+
+ /** \brief Class getName method. */
+ virtual std::string getClassName () const { return ("gpu::EuclideanLabeledClusterExtraction"); }
+ };
+ /** \brief Sort clusters method (for std::sort).
+ * \ingroup segmentation
+ */
+ inline bool
+ compareLabeledPointClusters (const pcl::PointIndices &a, const pcl::PointIndices &b)
+ {
+ return (a.indices.size () < b.indices.size ());
+ }
+ }
+}
+
+#endif //PCL_GPU_EXTRACT_LABELED_CLUSTERS_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id:$
+ *
+ */
+
+#ifndef PCL_GPU_SEEDED_HUE_SEGMENTATION_H_
+#define PCL_GPU_SEEDED_HUE_SEGMENTATION_H_
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/PointIndices.h>
+#include <pcl/pcl_macros.h>
+#include <pcl/gpu/octree/octree.hpp>
+#include <pcl/gpu/containers/device_array.hpp>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ void
+ seededHueSegmentation (const boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > &host_cloud_,
+ const pcl::gpu::Octree::Ptr &tree,
+ float tolerance,
+ PointIndices &clusters_in,
+ PointIndices &clusters_out,
+ float delta_hue = 0.0);
+
+ class SeededHueSegmentation
+ {
+ public:
+ typedef pcl::PointXYZ PointType;
+ typedef pcl::PointCloud<pcl::PointXYZ> PointCloudHost;
+ typedef PointCloudHost::Ptr PointCloudHostPtr;
+ typedef PointCloudHost::ConstPtr PointCloudHostConstPtr;
+
+ typedef PointIndices::Ptr PointIndicesPtr;
+ typedef PointIndices::ConstPtr PointIndicesConstPtr;
+
+ typedef pcl::gpu::Octree GPUTree;
+ typedef pcl::gpu::Octree::Ptr GPUTreePtr;
+
+ typedef pcl::gpu::Octree::PointCloud CloudDevice;
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Empty constructor. */
+ SeededHueSegmentation () : delta_hue_ (0.0)
+ {};
+
+ /** \brief Provide a pointer to the search object.
+ * \param tree a pointer to the spatial search object.
+ */
+ inline void setSearchMethod (const GPUTreePtr &tree) { tree_ = tree; }
+
+ /** \brief Get a pointer to the search method used.
+ * @todo fix this for a generic search tree
+ */
+ inline GPUTreePtr getSearchMethod () { return (tree_); }
+
+ /** \brief Set the spatial cluster tolerance as a measure in the L2 Euclidean space
+ * \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space
+ */
+ inline void setClusterTolerance (double tolerance) { cluster_tolerance_ = tolerance; }
+
+ /** \brief Get the spatial cluster tolerance as a measure in the L2 Euclidean space. */
+ inline double getClusterTolerance () { return (cluster_tolerance_); }
+
+ inline void setInput (CloudDevice input) {input_ = input;}
+
+ inline void setHostCloud (PointCloudHostPtr host_cloud) {host_cloud_ = host_cloud;}
+
+ /** \brief Set the tollerance on the hue
+ * \param[in] delta_hue the new delta hue
+ */
+ inline void
+ setDeltaHue (float delta_hue) { delta_hue_ = delta_hue; }
+
+ /** \brief Get the tolerance on the hue */
+ inline float
+ getDeltaHue () { return (delta_hue_); }
+
+ /** \brief Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
+ * \param indices_in
+ * \param indices_out
+ */
+ void segment (PointIndices &indices_in, PointIndices &indices_out);
+
+ protected:
+ /** \brief the input cloud on the GPU */
+ CloudDevice input_;
+
+ /** \brief the original cloud the Host */
+ PointCloudHostPtr host_cloud_;
+
+ /** \brief A pointer to the spatial search object. */
+ GPUTreePtr tree_;
+
+ /** \brief The spatial cluster tolerance as a measure in the L2 Euclidean space. */
+ double cluster_tolerance_;
+
+ /** \brief The allowed difference on the hue*/
+ float delta_hue_;
+
+ /** \brief Class getName method. */
+ virtual std::string getClassName () const { return ("gpu::SeededHueSegmentation"); }
+ };
+ /** \brief Sort clusters method (for std::sort).
+ * \ingroup segmentation
+ */
+ inline bool
+ comparePointClusters (const pcl::PointIndices &a, const pcl::PointIndices &b)
+ {
+ return (a.indices.size () < b.indices.size ());
+ }
+ }
+}
+
+#endif //PCL_GPU_SEEDED_HUE_SEGMENTATION_H_
+
+
+
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id:$
+ * @author: Koen Buys
+ */
+
+#ifndef PCL_GPU_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_
+#define PCL_GPU_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_
+
+#include <pcl/gpu/segmentation/gpu_extract_clusters.h>
+
+void
+pcl::gpu::extractEuclideanClusters (const boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > &host_cloud_,
+ const pcl::gpu::Octree::Ptr &tree,
+ float tolerance,
+ std::vector<PointIndices> &clusters,
+ unsigned int min_pts_per_cluster,
+ unsigned int max_pts_per_cluster)
+{
+
+ // Create a bool vector of processed point indices, and initialize it to false
+ // cloud is a DeviceArray<PointType>
+ std::vector<bool> processed (host_cloud_->points.size (), false);
+
+ int max_answers;
+
+ if(max_pts_per_cluster > host_cloud_->points.size())
+ max_answers = host_cloud_->points.size();
+ else
+ max_answers = max_pts_per_cluster;
+ std::cout << "Max_answers: " << max_answers << std::endl;
+
+ // to store the current cluster
+ pcl::PointIndices r;
+
+ DeviceArray<PointXYZ> queries_device_buffer;
+ queries_device_buffer.create(max_answers);
+
+ // Process all points in the cloud
+ for (size_t i = 0; i < host_cloud_->points.size (); ++i)
+ {
+ // if we already processed this point continue with the next one
+ if (processed[i])
+ continue;
+ // now we will process this point
+ processed[i] = true;
+
+ // Create the query queue on the device, point based not indices
+ pcl::gpu::Octree::Queries queries_device;
+ // Create the query queue on the host
+ pcl::PointCloud<pcl::PointXYZ>::VectorType queries_host;
+ // Push the starting point in the vector
+ queries_host.push_back (host_cloud_->points[i]);
+ // Clear vector
+ r.indices.clear();
+ // Push the starting point in
+ r.indices.push_back(i);
+
+ unsigned int found_points = queries_host.size ();
+ unsigned int previous_found_points = 0;
+
+ pcl::gpu::NeighborIndices result_device;
+
+ // once the area stop growing, stop also iterating.
+ do
+ {
+ // Host buffer for results
+ std::vector<int> sizes, data;
+
+ // if the number of queries is not high enough implement search on Host here
+ if(queries_host.size () <= 10) ///@todo: adjust this to a variable number settable with method
+ {
+ std::cout << " CPU: ";
+ for(size_t p = 0; p < queries_host.size (); p++)
+ {
+ // Execute the radiusSearch on the host
+ tree->radiusSearchHost(queries_host[p], tolerance, data, max_answers);
+ }
+ // Store the previously found number of points
+ previous_found_points = found_points;
+ // Clear queries list
+ queries_host.clear();
+
+ //std::unique(data.begin(), data.end());
+ if(data.size () == 1)
+ continue;
+
+ // Process the results
+ for(size_t i = 0; i < data.size (); i++)
+ {
+ if(processed[data[i]])
+ continue;
+ processed[data[i]] = true;
+ queries_host.push_back (host_cloud_->points[data[i]]);
+ found_points++;
+ r.indices.push_back(data[i]);
+ }
+ }
+
+ // If number of queries is high enough do it here
+ else
+ {
+ std::cout << " GPU: ";
+ // Copy buffer
+ queries_device = DeviceArray<PointXYZ>(queries_device_buffer.ptr(),queries_host.size());
+ // Move queries to GPU
+ queries_device.upload(queries_host);
+ // Execute search
+ tree->radiusSearch(queries_device, tolerance, max_answers, result_device);
+ // Copy results from GPU to Host
+ result_device.sizes.download (sizes);
+ result_device.data.download (data);
+ // Store the previously found number of points
+ previous_found_points = found_points;
+ // Clear queries list
+ queries_host.clear();
+ for(size_t qp = 0; qp < sizes.size (); qp++)
+ {
+ for(int qp_r = 0; qp_r < sizes[qp]; qp_r++)
+ {
+ if(processed[data[qp_r + qp * max_answers]])
+ continue;
+ processed[data[qp_r + qp * max_answers]] = true;
+ queries_host.push_back (host_cloud_->points[data[qp_r + qp * max_answers]]);
+ found_points++;
+ r.indices.push_back(data[qp_r + qp * max_answers]);
+ }
+ }
+ }
+ std::cout << " data.size: " << data.size() << " foundpoints: " << found_points << " previous: " << previous_found_points;
+ std::cout << " new points: " << found_points - previous_found_points << " next queries size: " << queries_host.size() << std::endl;
+ }
+ while (previous_found_points < found_points);
+ // If this queue is satisfactory, add to the clusters
+ if (found_points >= min_pts_per_cluster && found_points <= max_pts_per_cluster)
+ {
+ std::sort (r.indices.begin (), r.indices.end ());
+ // @todo: check if the following is actually still needed
+ //r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
+
+ r.header = host_cloud_->header;
+ clusters.push_back (r); // We could avoid a copy by working directly in the vector
+ }
+ }
+}
+
+void
+pcl::gpu::EuclideanClusterExtraction::extract (std::vector<pcl::PointIndices> &clusters)
+{
+/*
+ // Initialize the GPU search tree
+ if (!tree_)
+ {
+ tree_.reset (new pcl::gpu::Octree());
+ ///@todo what do we do if input isn't a PointXYZ cloud?
+ tree_.setCloud(input_);
+ }
+*/
+ if (!tree_->isBuilt())
+ {
+ tree_->build();
+ }
+/*
+ if(tree_->cloud_.size() != host_cloud.points.size ())
+ {
+ PCL_ERROR("[pcl::gpu::EuclideanClusterExtraction] size of host cloud and device cloud don't match!\n");
+ return;
+ }
+*/
+ // Extract the actual clusters
+ extractEuclideanClusters (host_cloud_, tree_, cluster_tolerance_, clusters, min_pts_per_cluster_, max_pts_per_cluster_);
+ std::cout << "INFO: end of extractEuclideanClusters " << std::endl;
+ // Sort the clusters based on their size (largest one first)
+ //std::sort (clusters.rbegin (), clusters.rend (), comparePointClusters);
+}
+
+#endif //PCL_GPU_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id:$
+ *
+ */
+
+#ifndef PCL_GPU_SEGMENTATION_IMPL_EXTRACT_LABELED_CLUSTERS_H_
+#define PCL_GPU_SEGMENTATION_IMPL_EXTRACT_LABELED_CLUSTERS_H_
+
+#include <pcl/gpu/segmentation/gpu_extract_labeled_clusters.h>
+
+template <typename PointT> void
+pcl::gpu::extractLabeledEuclideanClusters (const boost::shared_ptr<pcl::PointCloud<PointT> > &host_cloud_,
+ const pcl::gpu::Octree::Ptr &tree,
+ float tolerance,
+ std::vector<PointIndices> &clusters,
+ unsigned int min_pts_per_cluster,
+ unsigned int max_pts_per_cluster)
+{
+
+ // Create a bool vector of processed point indices, and initialize it to false
+ // cloud is a DeviceArray<PointType>
+ std::vector<bool> processed (host_cloud_->points.size (), false);
+
+ int max_answers;
+
+ if(max_pts_per_cluster > host_cloud_->points.size ())
+ max_answers = static_cast<int> (host_cloud_->points.size ());
+ else
+ max_answers = max_pts_per_cluster;
+
+ // to store the current cluster
+ pcl::PointIndices r;
+
+ // Process all points in the cloud
+ for (size_t i = 0; i < host_cloud_->points.size (); ++i)
+ {
+ // if we already processed this point continue with the next one
+ if (processed[i])
+ continue;
+ // now we will process this point
+ processed[i] = true;
+
+ // Create the query queue on the device, point based not indices
+ pcl::gpu::Octree::Queries queries_device;
+ // Create the query queue on the host
+ pcl::PointCloud<pcl::PointXYZ>::VectorType queries_host;
+
+ // Buffer in a new PointXYZ type
+ PointT t = host_cloud_->points[i];
+ PointXYZ p;
+ p.x = t.x; p.y = t.y; p.z = t.z;
+
+ // Push the starting point in the vector
+ queries_host.push_back (p);
+ // Clear vector
+ r.indices.clear ();
+ // Push the starting point in
+ r.indices.push_back (static_cast<int> (i));
+
+ unsigned int found_points = static_cast<unsigned int> (queries_host.size ());
+ unsigned int previous_found_points = 0;
+
+ pcl::gpu::NeighborIndices result_device;
+
+ // once the area stop growing, stop also iterating.
+ while (previous_found_points < found_points)
+ {
+ // Move queries to GPU
+ queries_device.upload(queries_host);
+ // Execute search
+ tree->radiusSearch(queries_device, tolerance, max_answers, result_device);
+
+ // Store the previously found number of points
+ previous_found_points = found_points;
+
+ // Host buffer for results
+ std::vector<int> sizes, data;
+
+ // Copy results from GPU to Host
+ result_device.sizes.download (sizes);
+ result_device.data.download (data);
+
+ for(size_t qp = 0; qp < sizes.size (); qp++)
+ {
+ for(int qp_r = 0; qp_r < sizes[qp]; qp_r++)
+ {
+ if(processed[data[qp_r + qp * max_answers]])
+ continue;
+ // Only add if label matches the original label
+ if(host_cloud_->points[i].label == host_cloud_->points[data[qp_r + qp * max_answers]].label)
+ {
+ processed[data[qp_r + qp * max_answers]] = true;
+ PointT t_l = host_cloud_->points[data[qp_r + qp * max_answers]];
+ PointXYZ p_l;
+ p_l.x = t_l.x; p_l.y = t_l.y; p_l.z = t_l.z;
+ queries_host.push_back (p_l);
+ found_points++;
+ r.indices.push_back(data[qp_r + qp * max_answers]);
+ }
+ }
+ }
+ }
+ // If this queue is satisfactory, add to the clusters
+ if (found_points >= min_pts_per_cluster && found_points <= max_pts_per_cluster)
+ {
+ std::sort (r.indices.begin (), r.indices.end ());
+ // @todo: check if the following is actually still needed
+ //r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
+
+ r.header = host_cloud_->header;
+ clusters.push_back (r); // We could avoid a copy by working directly in the vector
+ }
+ }
+}
+
+template <typename PointT> void
+pcl::gpu::EuclideanLabeledClusterExtraction<PointT>::extract (std::vector<PointIndices> &clusters)
+{
+ // Initialize the GPU search tree
+ if (!tree_)
+ {
+ tree_.reset (new pcl::gpu::Octree());
+ ///@todo what do we do if input isn't a PointXYZ cloud?
+ tree_->setCloud(input_);
+ }
+ if (!tree_->isBuilt())
+ {
+ tree_->build();
+ }
+/*
+ if(tree_->cloud_.size() != host_cloud.points.size ())
+ {
+ PCL_ERROR("[pcl::gpu::EuclideanClusterExtraction] size of host cloud and device cloud don't match!\n");
+ return;
+ }
+*/
+ // Extract the actual clusters
+ extractLabeledEuclideanClusters (host_cloud_, tree_, cluster_tolerance_, clusters, min_pts_per_cluster_, max_pts_per_cluster_);
+
+ // Sort the clusters based on their size (largest one first)
+ std::sort (clusters.rbegin (), clusters.rend (), compareLabeledPointClusters);
+}
+
+#define PCL_INSTANTIATE_extractLabeledEuclideanClusters(T) template void PCL_EXPORTS pcl::gpu::extractLabeledEuclideanClusters (const boost::shared_ptr<pcl::PointCloud<T> > &, const pcl::gpu::Octree::Ptr &,float, std::vector<PointIndices> &, unsigned int, unsigned int);
+#define PCL_INSTANTIATE_EuclideanLabeledClusterExtraction(T) template class PCL_EXPORTS pcl::gpu::EuclideanLabeledClusterExtraction<T>;
+/*
+#define PCL_INSTANTIATE_extractLabeledEuclideanClusters(T) template void PCL_EXPORTS pcl::gpu::extractLabeledEuclideanClusters (const boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGBL> > &, const pcl::gpu::Octree::Ptr &,float, std::vector<PointIndices> &, unsigned int, unsigned int);
+#define PCL_INSTANTIATE_EuclideanLabeledClusterExtraction(T) template class PCL_EXPORTS pcl::gpu::EuclideanLabeledClusterExtraction<T>;
+*/
+#endif //PCL_GPU_SEGMENTATION_IMPL_EXTRACT_LABELED_CLUSTERS_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id:$
+ *
+ */
+
+#ifndef PCL_GPU_SEGMENTATION_IMPL_SEEDED_HUE_SEGMENTATION_H_
+#define PCL_GPU_SEGMENTATION_IMPL_SEEDED_HUE_SEGMENTATION_H_
+
+#include <pcl/gpu/segmentation/gpu_seeded_hue_segmentation.h>
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+seededHueSegmentation (const boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > &host_cloud_,
+ const pcl::gpu::Octree::Ptr &tree,
+ float tolerance,
+ PointIndices &indices_in,
+ PointIndices &indices_out,
+ float delta_hue)
+{
+
+ // Create a bool vector of processed point indices, and initialize it to false
+ // cloud is a DeviceArray<PointType>
+ std::vector<bool> processed (host_cloud_->points.size (), false);
+
+ int max_answers = host_cloud_->points.size();
+
+ // Process all points in the indices vector
+ for (size_t k = 0; k < indices_in.indices.size (); ++k)
+ {
+ int i = indices_in.indices[k];
+ // if we already processed this point continue with the next one
+ if (processed[i])
+ continue;
+ // now we will process this point
+ processed[i] = true;
+
+ PointXYZRGB p;
+ p = host_cloud_->points[i];
+ PointXYZHSV h;
+ PointXYZRGBtoXYZHSV(p, h);
+
+ // Create the query queue on the device, point based not indices
+ pcl::gpu::Octree::Queries queries_device;
+ // Create the query queue on the host
+ pcl::PointCloud<pcl::PointXYZ>::VectorType queries_host;
+ // Push the starting point in the vector
+ queries_host.push_back (host_cloud_->points[i]);
+
+ unsigned int found_points = queries_host.size ();
+ unsigned int previous_found_points = 0;
+
+ pcl::gpu::NeighborIndices result_device;
+
+ // Host buffer for results
+ std::vector<int> sizes, data;
+
+ // once the area stop growing, stop also iterating.
+ while (previous_found_points < found_points)
+ {
+ // Move queries to GPU
+ queries_device.upload(queries_host);
+ // Execute search
+ tree->radiusSearch(queries_device, tolerance, max_answers, result_device);
+
+ // Store the previously found number of points
+ previous_found_points = found_points;
+
+ // Clear the Host vectors
+ sizes.clear (); data.clear ();
+
+ // Copy results from GPU to Host
+ result_device.sizes.download (sizes);
+ result_device.data.download (data);
+
+ for(size_t qp = 0; qp < sizes.size (); qp++)
+ {
+ for(int qp_r = 0; qp_r < sizes[qp]; qp_r++)
+ {
+ if(processed[data[qp_r + qp * max_answers]])
+ continue;
+
+ PointXYZRGB p_l;
+ p_l = host_cloud_->points[data[qp_r + qp * max_answers]];
+ PointXYZHSV h_l;
+ PointXYZRGBtoXYZHSV(p_l, h_l);
+
+ if (fabs(h_l.h - h.h) < delta_hue)
+ {
+ processed[data[qp_r + qp * max_answers]] = true;
+ queries_host.push_back (host_cloud_->points[data[qp_r + qp * max_answers]]);
+ found_points++;
+ }
+ }
+ }
+ }
+ for(size_t qp = 0; qp < sizes.size (); qp++)
+ {
+ for(int qp_r = 0; qp_r < sizes[qp]; qp_r++)
+ {
+ indices_out.indices.push_back(data[qp_r + qp * max_answers]);
+ }
+ }
+ }
+ // @todo: do we need to sort here and remove double points?
+}
+
+void
+pcl::gpu::SeededHueSegmentation::segment (PointIndices &indices_in, PointIndices &indices_out)
+{
+ // Initialize the GPU search tree
+ if (!tree_)
+ {
+ tree_.reset (new pcl::gpu::Octree());
+ ///@todo what do we do if input isn't a PointXYZ cloud?
+ tree_->setCloud(input_);
+ }
+ if (!tree_->isBuild())
+ {
+ tree_->build();
+ }
+/*
+ if(tree_->cloud_.size() != host_cloud.points.size ())
+ {
+ PCL_ERROR("[pcl::gpu::SeededHueSegmentation] size of host cloud and device cloud don't match!\n");
+ return;
+ }
+*/
+ // Extract the actual clusters
+ seededHueSegmentation (host_cloud_, tree_, cluster_tolerance_, indices_in, indices_out, delta_hue_);
+}
+
+#endif //PCL_GPU_SEGMENTATION_IMPL_SEEDED_HUE_SEGMENTATION_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: extract_clusters.cpp 2993 2011-10-31 17:17:53Z koenbuys $
+ *
+ */
+
+#include <pcl/impl/instantiate.hpp>
+#include <pcl/point_types.h>
+//#include <pcl/gpu/segmentation/gpu_extract_labeled_clusters.h>
+#include <pcl/gpu/segmentation/impl/gpu_extract_labeled_clusters.hpp>
+
+// Instantiations of specific point types
+PCL_INSTANTIATE(extractLabeledEuclideanClusters, PCL_XYZL_POINT_TYPES);
+PCL_INSTANTIATE(EuclideanLabeledClusterExtraction, PCL_XYZL_POINT_TYPES);
--- /dev/null
+set(SUBSYS_NAME gpu_surface)
+set(SUBSYS_PATH gpu/surface)
+set(SUBSYS_DESC "Surface algorithms with GPU")
+set(SUBSYS_DEPS common gpu_containers gpu_utils visualization geometry)
+
+set(build FALSE)
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" OFF)
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS})
+PCL_SET_SUBSYS_INCLUDE_DIR("${SUBSYS_NAME}" "${SUBSYS_PATH}")
+mark_as_advanced("BUILD_${SUBSYS_NAME}")
+
+if(build)
+ FILE(GLOB incs include/pcl/gpu/surface/*.h)
+ FILE(GLOB srcs src/*.cpp src/*.h*)
+ source_group("Source Files" FILES ${srcs})
+
+ FILE(GLOB utils src/utils/*.hpp)
+ source_group("Source Files\\utils" FILES ${utils})
+
+ FILE(GLOB cuda src/cuda/*.h* src/cuda/*.cu)
+ source_group("Source Files\\cuda" FILES ${cuda})
+
+ LIST(APPEND srcs ${utils} ${cuda})
+
+
+ include_directories(${VTK_INCLUDE_DIRS})
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include" "${CMAKE_CURRENT_SOURCE_DIR}/src" "${CMAKE_CURRENT_SOURCE_DIR}/src/cuda")
+ PCL_CUDA_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs})
+ target_link_libraries("${LIB_NAME}" pcl_gpu_containers pcl_visualization)
+
+ set(EXT_DEPS "")
+ #set(EXT_DEPS CUDA)
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "")
+
+ # Install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}" ${incs})
+
+ add_subdirectory(test)
+endif()
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+
+#ifndef PCL_GPU_SURFACE_CONVEX_HULL_HPP_
+#define PCL_GPU_SURFACE_CONVEX_HULL_HPP_
+
+#include <pcl/pcl_macros.h>
+#include <pcl/point_types.h>
+#include <pcl/gpu/containers/device_array.h>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ class PCL_EXPORTS PseudoConvexHull3D
+ {
+ public:
+
+ typedef pcl::PointXYZ PointType;
+ typedef pcl::gpu::DeviceArray<PointType> Cloud;
+
+
+ PseudoConvexHull3D(size_t buffer_size);
+ ~PseudoConvexHull3D();
+
+ void
+ reconstruct (const Cloud &points, Cloud &output);
+
+ void
+ reduce(const Cloud &points, Cloud &output);
+
+ private:
+
+ struct Impl;
+ boost::shared_ptr<Impl> impl_;
+
+ void
+ reconstruct (const Cloud &points, DeviceArray2D<int>& vertexes);
+ };
+ }
+}
+
+#endif /* PCL_GPU_SURFACE_CONVEX_HULL_HPP_*/
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTR
+ IBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/gpu/surface/convex_hull.h>
+#include <pcl/gpu/utils/device/static_check.hpp>
+#include "internal.h"
+#include <pcl/exceptions.h>
+
+pcl::device::FacetStream::FacetStream(size_t buffer_size)
+{
+ verts_inds.create(3, buffer_size);
+ head_points.create(buffer_size);
+
+ scan_buffer.create(buffer_size);
+ facet_count = 0;
+
+ verts_inds2.create(3,buffer_size);
+ head_points2.create(buffer_size);
+
+ empty_facets.create(3, buffer_size);
+
+ int zero = 0;
+ empty_count.upload(&zero, 1);
+}
+
+bool
+pcl::device::FacetStream::canSplit()
+{
+ return facet_count * 3 < verts_inds.cols();
+}
+
+struct pcl::gpu::PseudoConvexHull3D::Impl
+{
+ Impl(size_t buffer_size) : fs(buffer_size) {}
+ ~Impl() {};
+
+ device::FacetStream fs;
+};
+
+pcl::gpu::PseudoConvexHull3D::PseudoConvexHull3D(size_t bsize)
+{
+ pcl::gpu::Static<sizeof(pcl::device::uint64_type) == 8>::check();
+
+ impl_.reset( new Impl(bsize) );
+}
+pcl::gpu::PseudoConvexHull3D::~PseudoConvexHull3D() {}
+
+
+void
+pcl::gpu::PseudoConvexHull3D::reconstruct (const Cloud &cloud, DeviceArray2D<int>& vertexes)
+{
+ const device::Cloud& c = (const device::Cloud&)cloud;
+
+ device::FacetStream& fs = impl_->fs;
+ device::PointStream ps(c);
+
+ ps.computeInitalSimplex();
+
+ device::InitalSimplex simplex = ps.simplex;
+
+ fs.setInitialFacets(ps.simplex);
+ ps.initalClassify();
+
+ for(;;)
+ {
+ //new external points number
+ ps.cloud_size = ps.searchFacetHeads(fs.facet_count, fs.head_points);
+ if (ps.cloud_size == 0)
+ break;
+
+ fs.compactFacets();
+ ps.classify(fs);
+
+ if (!fs.canSplit())
+ throw PCLException("Can't split facets, please enlarge default buffer", __FILE__, "", __LINE__);
+
+ fs.splitFacets();
+ }
+
+ int ecount;
+ int fcount = fs.facet_count;
+ fs.empty_count.download(&ecount);
+
+ vertexes.create(3, fcount + ecount);
+ DeviceArray2D<int> subf(3, fcount, vertexes.ptr(), vertexes.step());
+ DeviceArray2D<int> sube(3, ecount, vertexes.ptr()+fcount, vertexes.step());
+
+ DeviceArray2D<int>(3, fcount, fs.verts_inds.ptr(), fs.verts_inds.step()).copyTo(subf);
+ DeviceArray2D<int>(3, ecount, fs.empty_facets.ptr(), fs.empty_facets.step()).copyTo(sube);
+}
+
+void
+pcl::gpu::PseudoConvexHull3D::reconstruct (const Cloud &points, Cloud &output)
+{
+ DeviceArray2D<int> vertexes;
+ reconstruct(points, vertexes);
+
+ DeviceArray<int> cont(vertexes.cols() * vertexes.rows());
+ DeviceArray2D<int> buf(3, vertexes.cols(), cont.ptr(), vertexes.cols() * sizeof(int));
+ vertexes.copyTo(buf);
+
+
+ size_t new_size = device::remove_duplicates(cont);
+ DeviceArray<int> new_cont(cont.ptr(), new_size);
+ output.create(new_size);
+
+ const device::Cloud& c = (const device::Cloud&)points;
+ device::Cloud& o = (device::Cloud&)output;
+
+ device::pack_hull(c, new_cont, o);
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "internal.h"
+#include "device.h"
+#include <limits>
+
+#include <pcl/gpu/utils/device/limits.hpp>
+#include <pcl/gpu/utils/device/algorithm.hpp>
+#include <pcl/gpu/utils/device/warp.hpp>
+#include <pcl/gpu/utils/device/static_check.hpp>
+//#include <pcl/gpu/utils/device/funcattrib.hpp>
+#include <pcl/gpu/utils/safe_call.hpp>
+
+#include <thrust/tuple.h>
+#include <thrust/iterator/counting_iterator.h>
+#include <thrust/iterator/zip_iterator.h>
+#include <thrust/transform_reduce.h>
+#include <thrust/functional.h>
+#include <thrust/sequence.h>
+#include "thrust/device_ptr.h"
+#include <thrust/transform.h>
+#include <thrust/sort.h>
+#include <thrust/transform_scan.h>
+#include <thrust/iterator/counting_iterator.h>
+#include <thrust/unique.h>
+#include <thrust/gather.h>
+
+using namespace thrust;
+using namespace std;
+
+namespace pcl
+{
+ namespace device
+ {
+ __global__ void size_check() { Static<sizeof(uint64_type) == 8>::check(); };
+
+ template<bool use_max>
+ struct IndOp
+ {
+ __device__ __forceinline__ thrust::tuple<float, int> operator()(const thrust::tuple<float, int>& e1, const thrust::tuple<float, int>& e2) const
+ {
+ thrust::tuple<float, int> res;
+
+ if (use_max)
+ res.get<0>() = fmax(e1.get<0>(), e2.get<0>());
+ else
+ res.get<0>() = fmin(e1.get<0>(), e2.get<0>());
+
+ res.get<1>() = (res.get<0>() == e1.get<0>()) ? e1.get<1>() : e2.get<1>();
+ return res;
+ }
+ };
+
+ struct X
+ {
+ __device__ __forceinline__
+ thrust::tuple<float, int>
+ operator()(const thrust::tuple<PointType, int>& in) const
+ {
+ return thrust::tuple<float, int>(in.get<0>().x, in.get<1>());
+ }
+ };
+
+ struct Y
+ {
+ __device__ __forceinline__ float operator()(const PointType& in) const { return in.y; }
+ };
+
+ struct Z
+ {
+ __device__ __forceinline__ float operator()(const PointType& in) const { return in.z; }
+ };
+
+ struct LineDist
+ {
+ float3 x1, x2;
+ LineDist(const PointType& p1, const PointType& p2) : x1(tr(p1)), x2(tr(p2)) {}
+
+ __device__ __forceinline__
+ thrust::tuple<float, int> operator()(const thrust::tuple<PointType, int>& in) const
+ {
+ float3 x0 = tr(in.get<0>());
+
+ float dist = norm(cross(x0 - x1, x0 - x2))/norm(x1 - x2);
+ return thrust::tuple<float, int>(dist, in.get<1>());
+ }
+ };
+
+ struct PlaneDist
+ {
+ float3 x1, n;
+ PlaneDist(const PointType& p1, const PointType& p2, const PointType& p3) : x1(tr(p1))
+ {
+ float3 x2 = tr(p2), x3 = tr(p3);
+ n = normalized(cross(x2 - x1, x3 - x1));
+ }
+
+ __device__ __forceinline__
+ thrust::tuple<float, int> operator()(const thrust::tuple<PointType, int>& in) const
+ {
+ float3 x0 = tr(in.get<0>());
+ float dist = fabs(dot(n, x0 - x1));
+ return thrust::tuple<float, int>(dist, in.get<1>());
+ }
+ };
+
+ template<typename It, typename Unary, typename Init, typename Binary>
+ int transform_reduce_index(It beg, It end, Unary unop, Init init, Binary binary)
+ {
+ counting_iterator<int> cbeg(0);
+ counting_iterator<int> cend = cbeg + thrust::distance(beg, end);
+
+ thrust::tuple<float, int> t = transform_reduce(
+ make_zip_iterator(thrust::make_tuple(beg, cbeg)),
+ make_zip_iterator(thrust::make_tuple(end, cend)),
+ unop, init, binary);
+
+ return t.get<1>();
+ }
+
+ template<typename It, typename Unary>
+ int transform_reduce_min_index(It beg, It end, Unary unop)
+ {
+ thrust::tuple<float, int> min_tuple(std::numeric_limits<float>::max(), 0);
+ return transform_reduce_index(beg, end, unop, min_tuple, IndOp<false>());
+ }
+
+ template<typename It, typename Unary>
+ int transform_reduce_max_index(It beg, It end, Unary unop)
+ {
+ thrust::tuple<float, int> max_tuple(std::numeric_limits<float>::min(), 0);
+ return transform_reduce_index(beg, end, unop, max_tuple, IndOp<true>());
+ }
+ }
+}
+
+pcl::device::PointStream::PointStream(const Cloud& cloud_) : cloud(cloud_)
+{
+ cloud_size = cloud.size();
+ facets_dists.create(cloud_size);
+ perm.create(cloud_size);
+
+ device_ptr<int> pbeg(perm.ptr());
+ thrust::sequence(pbeg, pbeg + cloud_size);
+}
+
+void pcl::device::PointStream::computeInitalSimplex()
+{
+ device_ptr<const PointType> beg(cloud.ptr());
+ device_ptr<const PointType> end = beg + cloud_size;
+
+ int minx = transform_reduce_min_index(beg, end, X());
+ int maxx = transform_reduce_max_index(beg, end, X());
+
+ PointType p1 = *(beg + minx);
+ PointType p2 = *(beg + maxx);
+
+ int maxl = transform_reduce_max_index(beg, end, LineDist(p1, p2));
+
+ PointType p3 = *(beg + maxl);
+
+ int maxp = transform_reduce_max_index(beg, end, PlaneDist(p1, p2, p3));
+
+ PointType p4 = *(beg + maxp);
+
+ simplex.x1 = tr(p1); simplex.x2 = tr(p2); simplex.x3 = tr(p3); simplex.x4 = tr(p4);
+ simplex.i1 = minx; simplex.i2 = maxx; simplex.i3 = maxl; simplex.i4 = maxp;
+
+ float maxy = transform_reduce(beg, end, Y(), std::numeric_limits<float>::min(), maximum<float>());
+ float miny = transform_reduce(beg, end, Y(), std::numeric_limits<float>::max(), minimum<float>());
+
+ float maxz = transform_reduce(beg, end, Z(), std::numeric_limits<float>::min(), maximum<float>());
+ float minz = transform_reduce(beg, end, Z(), std::numeric_limits<float>::max(), minimum<float>());
+
+ float dx = (p2.x - p1.x);
+ float dy = (maxy - miny);
+ float dz = (maxz - minz);
+
+ cloud_diag = sqrt(dx*dx + dy*dy + dz*dz);
+
+ simplex.p1 = compute_plane(simplex.x4, simplex.x2, simplex.x3, simplex.x1);
+ simplex.p2 = compute_plane(simplex.x3, simplex.x1, simplex.x4, simplex.x2);
+ simplex.p3 = compute_plane(simplex.x2, simplex.x1, simplex.x4, simplex.x3);
+ simplex.p4 = compute_plane(simplex.x1, simplex.x2, simplex.x3, simplex.x4);
+}
+
+namespace pcl
+{
+ namespace device
+ {
+ __global__ void init_fs(int i1, int i2, int i3, int i4, PtrStep<int> verts_inds)
+ {
+ *(int4*)verts_inds.ptr(0) = make_int4(i2, i1, i1, i1);
+ *(int4*)verts_inds.ptr(1) = make_int4(i3, i3, i2, i2);
+ *(int4*)verts_inds.ptr(2) = make_int4(i4, i4, i4, i3);
+ }
+
+ }
+}
+
+void pcl::device::FacetStream::setInitialFacets(const InitalSimplex& s)
+{
+ init_fs<<<1, 1>>>(s.i1, s.i2, s.i3, s.i4, verts_inds);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+ facet_count = 4;
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+ namespace device
+ {
+ struct InitalClassify
+ {
+ float diag;
+ float4 pl1, pl2, pl3, pl4;
+
+ InitalClassify(const float4& p1, const float4& p2, const float4& p3, const float4& p4, float diagonal)
+ : diag(diagonal), pl1(p1), pl2(p2), pl3(p3), pl4(p4)
+ {
+ pl1 *= compue_inv_normal_norm(pl1);
+ pl2 *= compue_inv_normal_norm(pl2);
+ pl3 *= compue_inv_normal_norm(pl3);
+ pl4 *= compue_inv_normal_norm(pl4);
+ }
+
+ __device__ __forceinline__
+ uint64_type
+ operator()(const PointType& p) const
+ {
+ float4 x = p;
+ x.w = 1;
+
+ float d0 = dot(pl1, x);
+ float d1 = dot(pl2, x);
+ float d2 = dot(pl3, x);
+ float d3 = dot(pl4, x);
+
+ float dists[] = { d0, d1, d2, d3 };
+ int negs_inds[4];
+ int neg_count = 0;
+
+ int idx = numeric_limits<int>::max();
+ float dist = 0;
+
+ #pragma unroll
+ for(int i = 0; i < 4; ++i)
+ if (dists[i] < 0)
+ negs_inds[neg_count++] = i;
+
+ if (neg_count == 3)
+ {
+ int i1 = negs_inds[1];
+ int i2 = negs_inds[2];
+
+ int ir = fabs(dists[i1]) < fabs(dists[i2]) ? i2 : i1;
+ negs_inds[1] = ir;
+ --neg_count;
+ }
+
+ if (neg_count == 2)
+ {
+ int i1 = negs_inds[0];
+ int i2 = negs_inds[1];
+
+ int ir = fabs(dists[i1]) < fabs(dists[i2]) ? i2 : i1;
+ negs_inds[0] = ir;
+ --neg_count;
+ }
+
+ if (neg_count == 1)
+ {
+ idx = negs_inds[0];
+ dist = diag - fabs(dists[idx]); // to ensure that sorting order is inverse, i.e. distant points go first
+ }
+
+ //if (neg_count == 0)
+ // then internal point ==>> idx = INT_MAX
+
+ uint64_type res = idx;
+ res <<= 32;
+ return res + *reinterpret_cast<unsigned int*>(&dist);
+ }
+ };
+
+ __global__ void initalClassifyKernel(const InitalClassify ic, const PointType* points, int cloud_size, uint64_type* output)
+ {
+ int index = threadIdx.x + blockIdx.x * blockDim.x;
+
+ if (index < cloud_size)
+ output[index] = ic(points[index]);
+ }
+ }
+}
+
+void pcl::device::PointStream::initalClassify()
+{
+ //thrust::device_ptr<const PointType> beg(cloud.ptr());
+ //thrust::device_ptr<const PointType> end = beg + cloud_size;
+ thrust::device_ptr<uint64_type> out(facets_dists.ptr());
+
+ InitalClassify ic(simplex.p1, simplex.p2, simplex.p3, simplex.p4, cloud_diag);
+ //thrust::transform(beg, end, out, ic);
+
+ //printFuncAttrib(initalClassifyKernel);
+
+ initalClassifyKernel<<<divUp(cloud_size, 256), 256>>>(ic, cloud, cloud_size, facets_dists);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+
+ thrust::device_ptr<int> pbeg(perm.ptr());
+ thrust::sort_by_key(out, out + cloud_size, pbeg);
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+ namespace device
+ {
+ __device__ int new_cloud_size;
+ struct SearchFacetHeads
+ {
+ uint64_type *facets_dists;
+ int cloud_size;
+ int facet_count;
+ int *perm;
+ const PointType* points;
+
+ mutable int* head_points;
+ //bool logger;
+
+ __device__ __forceinline__
+ void operator()(int facet) const
+ {
+ const uint64_type* b = facets_dists;
+ const uint64_type* e = b + cloud_size;
+
+ bool last_thread = facet == facet_count;
+
+ int search_value = !last_thread ? facet : numeric_limits<int>::max();
+ int index = lower_bound(b, e, search_value, LessThanByFacet()) - b;
+
+ if (last_thread)
+ new_cloud_size = index;
+ else
+ {
+ bool not_found = index == cloud_size || (facet != (facets_dists[index] >> 32));
+
+ head_points[facet] = not_found ? -1 : perm[index];
+ }
+ }
+ };
+
+ __global__ void searchFacetHeadsKernel(const SearchFacetHeads sfh)
+ {
+ int facet = threadIdx.x + blockDim.x * blockIdx.x;
+
+ if (facet <= sfh.facet_count)
+ sfh(facet);
+ }
+ }
+}
+
+int pcl::device::PointStream::searchFacetHeads(size_t facet_count, DeviceArray<int>& head_points)
+{
+ SearchFacetHeads sfh;
+
+ sfh.facets_dists = facets_dists;
+ sfh.cloud_size = (int)cloud_size;
+ sfh.facet_count = (int)facet_count;
+ sfh.perm = perm;
+ sfh.points = cloud.ptr();
+ sfh.head_points = head_points;
+
+ //thrust::counting_iterator<int> b(0);
+ //thrust::counting_iterator<int> e = b + facet_count + 1;
+ //thrust::for_each(b, e, sfh);
+
+ searchFacetHeadsKernel<<<divUp(facet_count+1, 256), 256>>>(sfh);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+
+ int new_size;
+ cudaSafeCall( cudaMemcpyFromSymbol( (void*)&new_size, pcl::device::new_cloud_size, sizeof(new_size)) );
+ return new_size;
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+ namespace device
+ {
+ struct NotMinus1
+ {
+ __device__ __forceinline__
+ int operator()(const int& v) const { return (v == -1) ? 0 : 1; }
+ };
+
+
+ struct Compaction
+ {
+ enum
+ {
+ CTA_SIZE = 256,
+
+ WARPS = CTA_SIZE/ Warp::WARP_SIZE
+ };
+
+ int* head_points_in;
+ PtrStep<int> verts_inds_in;
+
+
+ int *scan_buffer;
+ int facet_count;
+
+ mutable int* head_points_out;
+ mutable PtrStep<int> verts_inds_out;
+
+
+ mutable PtrStep<int> empty_facets;
+ mutable int *empty_count;
+
+ __device__ __forceinline__
+ void operator()() const
+ {
+ int idx = threadIdx.x + blockIdx.x * blockDim.x;
+
+ if (__all(idx >= facet_count))
+ return;
+
+ int empty = 0;
+
+ if(idx < facet_count)
+ {
+ int head_idx = head_points_in[idx];
+ if (head_idx != -1)
+ {
+ int offset = scan_buffer[idx];
+
+ head_points_out[offset] = head_idx;
+
+ verts_inds_out.ptr(0)[offset] = verts_inds_in.ptr(0)[idx];
+ verts_inds_out.ptr(1)[offset] = verts_inds_in.ptr(1)[idx];
+ verts_inds_out.ptr(2)[offset] = verts_inds_in.ptr(2)[idx];
+
+
+
+ }
+ else
+ empty = 1;
+ }
+
+ int total = __popc(__ballot(empty));
+ if (total > 0)
+ {
+ int offset = Warp::binaryExclScan(__ballot(empty));
+
+ volatile __shared__ int wapr_buffer[WARPS];
+
+ int laneid = Warp::laneId();
+ int warpid = Warp::id();
+ if (laneid == 0)
+ {
+ int old = atomicAdd(empty_count, total);
+ wapr_buffer[warpid] = old;
+ }
+ int old = wapr_buffer[warpid];
+
+ if (empty)
+ {
+ empty_facets.ptr(0)[old + offset] = verts_inds_in.ptr(0)[idx];
+ empty_facets.ptr(1)[old + offset] = verts_inds_in.ptr(1)[idx];
+ empty_facets.ptr(2)[old + offset] = verts_inds_in.ptr(2)[idx];
+
+ int a1 = verts_inds_in.ptr(0)[idx], a2 = verts_inds_in.ptr(1)[idx], a3 = verts_inds_in.ptr(2)[idx];
+ }
+ }
+ }
+ };
+
+ __global__ void compactionKernel( const Compaction c ) { c(); }
+ }
+}
+
+
+void pcl::device::FacetStream::compactFacets()
+{
+ int old_empty_count;
+ empty_count.download(&old_empty_count);
+
+ thrust::device_ptr<int> b(head_points.ptr());
+ thrust::device_ptr<int> e = b + facet_count;
+ thrust::device_ptr<int> o(scan_buffer.ptr());
+
+ thrust::transform_exclusive_scan(b, e, o, NotMinus1(), 0, thrust::plus<int>());
+
+ Compaction c;
+
+ c.verts_inds_in = verts_inds;
+ c.head_points_in = head_points;
+
+ c.scan_buffer = scan_buffer;
+ c.facet_count = facet_count;
+
+ c.head_points_out = head_points2;
+ c.verts_inds_out = verts_inds2;
+
+ c.empty_facets = empty_facets;
+ c.empty_count = empty_count;
+
+ int block = Compaction::CTA_SIZE;
+ int grid = divUp(facet_count, block);
+
+ compactionKernel<<<grid, block>>>(c);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+
+ verts_inds.swap(verts_inds2);
+ head_points.swap(head_points2);
+
+ int new_empty_count;
+ empty_count.download(&new_empty_count);
+
+ facet_count -= new_empty_count - old_empty_count;
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////
+
+namespace pcl
+{
+ namespace device
+ {
+ struct Classify
+ {
+ uint64_type* facets_dists;
+ int* scan_buffer;
+
+ int* head_points;
+ int* perm;
+ PtrStep<int> verts_inds;
+
+ const PointType *points;
+
+ float diag;
+
+ int facet_count;
+
+ __device__ __forceinline__
+ void operator()(int point_idx) const
+ {
+ int perm_index = perm[point_idx];
+
+ int facet = facets_dists[point_idx] >> 32;
+ facet = scan_buffer[facet];
+
+ int hi = head_points[facet];
+
+ if (hi == perm_index)
+ {
+ uint64_type res = numeric_limits<int>::max();
+ res <<= 32;
+ facets_dists[point_idx] = res;
+ }
+ else
+ {
+
+ int i1 = verts_inds.ptr(0)[facet];
+ int i2 = verts_inds.ptr(1)[facet];
+ int i3 = verts_inds.ptr(2)[facet];
+
+ float3 hp = tr( points[ hi ] );
+ float3 v1 = tr( points[ i1 ] );
+ float3 v2 = tr( points[ i2 ] );
+ float3 v3 = tr( points[ i3 ] );
+
+ float4 p0 = compute_plane(hp, v1, v2, /*opposite*/v3); // j
+ float4 p1 = compute_plane(hp, v2, v3, /*opposite*/v1); // facet_count + j
+ float4 p2 = compute_plane(hp, v3, v1, /*opposite*/v2); // facet_count + j*2
+
+ p0 *= compue_inv_normal_norm(p0);
+ p1 *= compue_inv_normal_norm(p1);
+ p2 *= compue_inv_normal_norm(p2);
+
+
+ float4 p = points[perm_index];
+ p.w = 1;
+
+ float d0 = dot(p, p0);
+ float d1 = dot(p, p1);
+ float d2 = dot(p, p2);
+
+ float dists[] = { d0, d1, d2 };
+ int negs_inds[3];
+ int neg_count = 0;
+
+ int new_idx = numeric_limits<int>::max();
+ float dist = 0;
+
+ int indeces[] = { facet, facet + facet_count, facet + facet_count * 2 };
+
+ #pragma unroll
+ for(int i = 0; i < 3; ++i)
+ if (dists[i] < 0)
+ negs_inds[neg_count++] = i;
+
+ if (neg_count == 3)
+ {
+ int i1 = negs_inds[1];
+ int i2 = negs_inds[2];
+
+ int ir = fabs(dists[i1]) < fabs(dists[i2]) ? i2 : i1;
+ negs_inds[1] = ir;
+ --neg_count;
+ }
+
+ if (neg_count == 2)
+ {
+ int i1 = negs_inds[0];
+ int i2 = negs_inds[1];
+
+ int ir = fabs(dists[i1]) < fabs(dists[i2]) ? i2 : i1;
+ negs_inds[0] = ir;
+ --neg_count;
+ }
+
+ if (neg_count == 1)
+ {
+ new_idx = negs_inds[0];
+ dist = diag - fabs(dists[new_idx]); // to ensure that sorting order is inverse, i.e. distant points go first
+ new_idx = indeces[new_idx];
+ }
+
+ // if (neg_count == 0)
+ // new_idx = INT_MAX ==>> internal point
+
+ uint64_type res = new_idx;
+ res <<= 32;
+ res += *reinterpret_cast<unsigned int*>(&dist);
+
+ facets_dists[point_idx] = res;
+
+ } /* if (hi == perm_index) */
+ }
+ };
+
+ __global__ void classifyKernel(const Classify c, int cloud_size)
+ {
+ int point_idx = threadIdx.x + blockIdx.x * blockDim.x;
+
+ if ( point_idx < cloud_size )
+ c(point_idx);
+ }
+ }
+}
+
+void pcl::device::PointStream::classify(FacetStream& fs)
+{
+ Classify c;
+
+ c.facets_dists = facets_dists;
+ c.scan_buffer = fs.scan_buffer;
+ c.head_points = fs.head_points;
+ c.perm = perm;
+
+ c.verts_inds = fs.verts_inds;
+ c.points = cloud;
+
+ c.diag = cloud_diag;
+ c.facet_count = fs.facet_count;
+
+ //thrust::counting_iterator<int> b(0);
+ //thrust::for_each(b, b + cloud_size, c);
+
+ classifyKernel<<<divUp(cloud_size, 256), 256>>>(c, cloud_size);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+
+ thrust::device_ptr<uint64_type> beg(facets_dists.ptr());
+ thrust::device_ptr<uint64_type> end = beg + cloud_size;
+
+ thrust::device_ptr<int> pbeg(perm.ptr());
+ thrust::sort_by_key(beg, end, pbeg);
+}
+
+namespace pcl
+{
+ namespace device
+ {
+ struct SplitFacets
+ {
+ int* head_points;
+ int facet_count;
+
+ mutable PtrStep<int> verts_inds;
+
+ __device__ __forceinline__
+ void operator()(int facet) const
+ {
+ int hi = head_points[facet];
+ int i1 = verts_inds.ptr(0)[facet];
+ int i2 = verts_inds.ptr(1)[facet];
+ int i3 = verts_inds.ptr(2)[facet];
+
+ make_facet(hi, i1, i2, facet);
+ make_facet(hi, i2, i3, facet + facet_count);
+ make_facet(hi, i3, i1, facet + facet_count * 2);
+ }
+
+ __device__ __forceinline__
+ void make_facet(int i1, int i2, int i3, int out_idx) const
+ {
+ verts_inds.ptr(0)[out_idx] = i1;
+ verts_inds.ptr(1)[out_idx] = i2;
+ verts_inds.ptr(2)[out_idx] = i3;
+ }
+ };
+
+ __global__ void splitFacetsKernel(const SplitFacets sf)
+ {
+ int facet = threadIdx.x + blockIdx.x * blockDim.x;
+
+ if (facet < sf.facet_count)
+ sf(facet);
+ }
+ }
+}
+
+void pcl::device::FacetStream::splitFacets()
+{
+ SplitFacets sf;
+ sf.head_points = head_points;
+ sf.verts_inds = verts_inds;
+ sf.facet_count = facet_count;
+
+
+ //thrust::counting_iterator<int> b(0);
+ //thrust::for_each(b, b + facet_count, sf);
+
+ splitFacetsKernel<<<divUp(facet_count, 256), 256>>>(sf);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+
+ facet_count *= 3;
+}
+
+size_t pcl::device::remove_duplicates(DeviceArray<int>& indeces)
+{
+ thrust::device_ptr<int> beg(indeces.ptr());
+ thrust::device_ptr<int> end = beg + indeces.size();
+
+ thrust::sort(beg, end);
+ return (size_t)(thrust::unique(beg, end) - beg);
+}
+
+
+namespace pcl
+{
+ namespace device
+ {
+ __global__ void gatherKernel(const PtrSz<int> indeces, const PointType* src, PointType* dst)
+ {
+ int idx = blockIdx.x * blockDim.x + threadIdx.x;
+
+ if (idx < indeces.size)
+ dst[idx] = src[indeces.data[idx]];
+ }
+ }
+}
+
+
+void pcl::device::pack_hull(const DeviceArray<PointType>& points, const DeviceArray<int>& indeces, DeviceArray<PointType>& output)
+{
+ output.create(indeces.size());
+
+ //device_ptr<const PointType> in(points.ptr());
+
+ //thrust::device_ptr<const int> mb(indeces.ptr());
+ //thrust::device_ptr<const int> me = mb + indeces.size();
+
+ //device_ptr<PointType> out(output.ptr());
+
+ //thrust::gather(mb, me, in, out);
+
+ gatherKernel<<<divUp(indeces.size(), 256), 256>>>(indeces, points, output);
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_GPU_SURFACE_DEVICE_H_
+#define PCL_GPU_SURFACE_DEVICE_H_
+
+#include "internal.h"
+#include <pcl/gpu/utils/device/vector_math.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+ /** \brief Computers plane from 3 points (v, v1, v2), ensures that point P lies positive subspace.
+ * \param[in] v 3D point volume tsdf volume container
+ * \param[in] v1 3D point volume tsdf volume container
+ * \param[in] v2 3D point volume tsdf volume container
+ * \param[in] p point for sign check of plane coefs (should lie in positive subspace)
+ * \return a,b,c,d coefs vector
+ */
+ __device__ __host__ __forceinline__
+ float4 compute_plane(const float3& v, const float3& v1, const float3& v2, const float3& p)
+ {
+ float3 n = cross(v1 - v, v2 - v);
+
+ float d = -dot(n, v);
+
+ if (dot(n, p) + d < 0)
+ {
+ n*=-1.f;
+ d*=-1.f;
+ }
+ return make_float4(n.x, n.y, n.z, d);
+ }
+
+ __device__ __host__ __forceinline__ float3 tr(const PointType& p) { return make_float3(p.x, p.y, p.z); }
+
+ struct LessThanByFacet
+ {
+ __device__ __forceinline__
+ bool operator()(const uint64_type& e1, const int& e2) const
+ {
+ int i1 = (int)(e1 >> 32);
+ return i1 < e2;
+ }
+ };
+
+ __device__ __host__ __forceinline__ float compue_inv_normal_norm(const float4& p) { return 1.f/sqrt(p.x*p.x + p.y*p.y + p.z*p.z); }
+
+
+ __device__ __host__ __forceinline__ float4& operator*=(float4& p, float v) { p.x*=v; p.y*=v; p.z*=v; p.w*=v; return p; }
+
+ }
+};
+
+#endif /* PCL_GPU_SURFACE_DEVICE_H_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_GPU_SURFACE_INTERNAL_H_
+#define PCL_GPU_SURFACE_INTERNAL_H_
+
+#include <pcl/gpu/containers/device_array.h>
+#include <cuda_runtime.h>
+
+namespace pcl
+{
+ namespace device
+ {
+ typedef unsigned long long uint64_type;
+
+ typedef float4 PointType;
+ typedef pcl::gpu::DeviceArray<PointType> Cloud;
+
+ typedef DeviceArray<uint64_type> FacetsDists;
+ typedef DeviceArray<int> Perm;
+
+ struct InitalSimplex
+ {
+ float3 x1, x2, x3, x4;
+ int i1, i2, i3, i4;
+
+ float4 p1, p2, p3, p4;
+ };
+
+ struct FacetStream
+ {
+ public:
+ FacetStream(size_t buffer_size);
+
+ // indeces: in each col indeces of vertexes for sigle facet
+ DeviceArray2D<int> verts_inds;
+
+ DeviceArray<int> head_points;
+ size_t facet_count;
+
+ DeviceArray2D<int> empty_facets;
+ DeviceArray<int> empty_count;
+
+ DeviceArray<int> scan_buffer;
+
+ void setInitialFacets(const InitalSimplex& simplex);
+
+ void compactFacets();
+
+ bool canSplit();
+ void splitFacets();
+ private:
+
+ //for compation (double buffering)
+ DeviceArray2D<int> verts_inds2;
+ DeviceArray<float4> facet_planes2;
+ DeviceArray<int> head_points2;
+ };
+
+ struct PointStream
+ {
+ public:
+ PointStream(const Cloud& cloud);
+
+ const Cloud cloud;
+ FacetsDists facets_dists;
+ Perm perm;
+
+ size_t cloud_size;
+
+ InitalSimplex simplex;
+ float cloud_diag;
+
+ void computeInitalSimplex();
+
+ void initalClassify();
+
+
+ int searchFacetHeads(size_t facet_count, DeviceArray<int>& head_points);
+
+ void classify(FacetStream& fs);
+ };
+
+
+ size_t remove_duplicates(DeviceArray<int>& indeces);
+ void pack_hull(const DeviceArray<PointType>& points, const DeviceArray<int>& indeces, DeviceArray<PointType>& output);
+ }
+}
+
+#endif /* PCL_GPU_SURFACE_INTERNAL_H_ */
--- /dev/null
+if(BUILD_TESTS)
+
+ set(the_test_target test_gpu_surface)
+
+ get_filename_component(DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
+ get_filename_component(INC_SURF "${DIR}/../../surface/include" REALPATH)
+ get_filename_component(INC_IO "${DIR}/../../../io/include" REALPATH)
+ get_filename_component(INC_VIZ "${DIR}/../../../visualization/include" REALPATH)
+ get_filename_component(INC_GEO "${DIR}/../../../geometry/include" REALPATH)
+ get_filename_component(INC_SURF_CPU "${DIR}/../../../surface/include" REALPATH)
+ get_filename_component(INC_SEA "${DIR}/../../../search/include" REALPATH)
+ get_filename_component(INC_KD "${DIR}/../../../kdtree/include" REALPATH)
+ get_filename_component(INC_OCT "${DIR}/../../../octree/include" REALPATH)
+ include_directories("${INC_SURF}" "${INC_IO}" "${INC_VIZ}" "${INC_GEO}" "${INC_SURF_CPU}" "${INC_SEA}" "${INC_KD}" "${INC_OCT}" ${VTK_INCLUDE_DIRS})
+
+ FILE(GLOB test_src *.cpp *.hpp)
+ #PCL_ADD_TEST(a_gpu_surface_test ${the_test_target} FILES ${test_src} LINK_WITH pcl_io pcl_gpu_containers pcl_gpu_surface pcl_visualization pcl_surface pcl_octree pcl_kdtree pcl_search)
+ #add_dependencies(${the_test_target} pcl_io pcl_gpu_containes pcl_gpu_surface pcl_visualization)
+
+endif(BUILD_TESTS)
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#if defined _MSC_VER
+ #pragma warning (disable : 4996 4530)
+#endif
+
+#include <gtest/gtest.h>
+
+#include<iostream>
+#include<fstream>
+#include<algorithm>
+
+#if defined _MSC_VER
+ #pragma warning (disable: 4521)
+#endif
+
+#include <pcl/point_cloud.h>
+
+#if defined _MSC_VER
+ #pragma warning (default: 4521)
+#endif
+
+
+#include <pcl/gpu/surface/convex_hull.h>
+#include <pcl/surface/convex_hull.h>
+#include <pcl/PolygonMesh.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/common/time.h>
+
+using namespace pcl::gpu;
+using namespace std;
+
+
+int loadCloud(const std::string& file, pcl::PointCloud<pcl::PointXYZ>& cloud)
+{
+ int result = pcl::io::loadPCDFile(file, cloud);
+ if (result != -1)
+ return result;
+
+ string name = file.substr(0, file.find_last_of("."));
+
+ result = pcl::io::loadPCDFile(name + ".pcd", cloud);
+ if (result != -1)
+ return result;
+
+ result = pcl::io::loadPLYFile(name + ".ply", cloud);
+ if (result != -1)
+ pcl::io::savePCDFile(name + ".pcd", cloud, true);
+ return result;
+};
+
+//TEST(PCL_SurfaceGPU, DISABGLED_pseudoConvexHull)
+TEST(PCL_SurfaceGPU, pseudoConvexHull)
+{
+ PseudoConvexHull3D pch(1e5);
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr( new pcl::PointCloud<pcl::PointXYZ>() );
+ //ASSERT_TRUE(loadCloud("d:/dragon.ply", *cloud_ptr) != -1);
+ //ASSERT_TRUE(loadCloud("d:/horse.ply", *cloud_ptr) != -1);
+ ASSERT_TRUE(loadCloud("d:/Ramesses.ply", *cloud_ptr) != -1);
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr convex_ptr;
+
+
+ PseudoConvexHull3D::Cloud cloud_device;
+ PseudoConvexHull3D::Cloud convex_device;
+ cloud_device.upload(cloud_ptr->points);
+
+ pcl::PolygonMesh mesh;
+
+ {
+ pcl::ScopeTime time("cpu");
+ pcl::ConvexHull<pcl::PointXYZ> ch;
+ ch.setInputCloud(cloud_ptr);
+ ch.reconstruct(mesh);
+ }
+
+ {
+ pcl::ScopeTime time("gpu+cpu");
+ {
+ pcl::ScopeTime time("gpu");
+ pch.reconstruct(cloud_device, convex_device);
+ }
+
+ convex_ptr.reset(new pcl::PointCloud<pcl::PointXYZ>((int)convex_device.size(), 1));
+ convex_device.download(convex_ptr->points);
+
+ pcl::ConvexHull<pcl::PointXYZ> ch;
+ ch.setInputCloud(convex_ptr);
+ ch.reconstruct(mesh);
+ }
+
+ pcl::visualization::PCLVisualizer viewer("Viewer");
+ viewer.addPointCloud<pcl::PointXYZ> (cloud_ptr, "sample cloud");
+
+ viewer.spinOnce(1, true);
+
+ pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(convex_ptr, 0, 255, 0);
+
+ viewer.addPointCloud<pcl::PointXYZ> (convex_ptr, single_color, "convex");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "convex");
+
+
+ viewer.addPolylineFromPolygonMesh(mesh);
+
+ viewer.spin();
+}
--- /dev/null
+set(SUBSYS_NAME gpu_tracking)
+set(SUBSYS_PATH gpu/tracking)
+set(SUBSYS_DESC "Tracking with GPU")
+set(SUBSYS_DEPS common gpu_containers gpu_utils gpu_octree tracking search kdtree filters octree)
+
+set(build FALSE)
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" OFF)
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS})
+PCL_SET_SUBSYS_INCLUDE_DIR("${SUBSYS_NAME}" "${SUBSYS_PATH}")
+mark_as_advanced("BUILD_${SUBSYS_NAME}")
+
+if(build)
+ FILE(GLOB incs include/pcl/gpu/tracking/*.h)
+ FILE(GLOB srcs src/*.cpp src/*.h*)
+ source_group("Source Files" FILES ${srcs})
+
+ FILE(GLOB utils src/utils/*.hpp)
+ source_group("Source Files\\utils" FILES ${utils})
+
+ FILE(GLOB cuda src/cuda/*.h* src/cuda/*.cu)
+ source_group("Source Files\\cuda" FILES ${cuda})
+
+ LIST(APPEND srcs ${utils} ${cuda})
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include" "${CMAKE_CURRENT_SOURCE_DIR}/src" "${CMAKE_CURRENT_SOURCE_DIR}/src/cuda")
+ PCL_CUDA_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs})
+ target_link_libraries("${LIB_NAME}" pcl_gpu_containers)
+
+ set(EXT_DEPS "")
+ #set(EXT_DEPS CUDA)
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "")
+
+ # Install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}" ${incs})
+
+endif()
--- /dev/null
+#ifndef PCL_GPU_TRACKING_PARTICLE_FILTER_H_
+#define PCL_GPU_TRACKING_PARTICLE_FILTER_H_
+
+#include <pcl/pcl_macros.h>
+#include <pcl/gpu/containers/device_array.h>
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/PointIndices.h>
+#include <pcl/pcl_macros.h>
+
+#include <pcl/gpu/kinfu/pixel_rgb.h>
+#include <pcl/tracking/particle_filter.h>
+
+#include <Eigen/Dense>
+
+#include "internal.h"
+
+namespace pcl
+{
+ namespace gpu
+ {
+ class ParticleFilterGPUTracker
+ {
+ public:
+ /** \brief Point type supported */
+ typedef pcl::PointXYZ PointType;
+ //typedef pcl::Normal NormalType;
+ typedef pcl::RGB PixelRGB;
+
+ typedef pcl::PointXYZ StateXYZ;
+ typedef pcl::PointXYZ StateRPY;
+
+ typedef pcl::tracking::ParticleXYZRPY StateType;
+
+ /** \brief Empty constructor. */
+ ParticleFilterGPUTracker ()
+ //: ParticleFilterTracker<PointInT, StateT> ()
+ {
+ tracker_name_ = "ParticleFilterGPUTracker";
+ }
+
+ /** \brief set the number of the particles.
+ * \param particle_num the number of the particles.
+ */
+ inline void
+ setParticleNum (const int particle_num) { particle_num_ = particle_num; }
+
+ /** \brief get the number of the particles. */
+ inline int
+ getParticleNum () const { return particle_num_; }
+
+ /** \brief set a pointer to a reference dataset to be tracked.
+ * \param ref a pointer to a PointCloud message
+ */
+ inline void
+ setReferenceCloud (const DeviceArray2D<PointType> &ref) { ref_ = ref; }
+
+ /** \brief get a pointer to a reference dataset to be tracked. */
+ inline DeviceArray2D<PointType> const
+ getReferenceCloud () { return ref_; }
+
+ int
+ cols ();
+
+ int
+ rows ();
+
+ virtual bool
+ operator() (const DeviceArray2D<PointType>& input, const DeviceArray2D<PixelRGB>& input_colors)
+ {
+
+ }
+
+ virtual void
+ setMotion (StateType motion)
+ { motion_ = motion; }
+
+ virtual StateType
+ getResult();
+
+ protected:
+ std::string tracker_name_;
+
+ virtual bool
+ initCompute()
+ {
+
+ //pcl::device::initParticles(particle_num_, particle_xyz_, particle_rpy_, particle_weight_ );
+ }
+
+ virtual void
+ computeTracking()
+ {
+
+ }
+
+ virtual void
+ allocateBuffers()
+ {
+ particles_.create( particle_num_ );
+
+ random_number_generator_.create( particle_num_ );
+
+ }
+
+ // reference point cloud
+ DeviceArray2D<PointType> ref_;
+
+ DeviceArray2D<PixelRGB> ref_colors_;
+
+ //DeviceArray2D<NormalType> ref_normals_;
+
+ // input point cloud
+ DeviceArray2D<PointType> input_;
+
+ DeviceArray2D<PixelRGB> input_colors_;
+
+ //DeviceArray2D<NormalType> input_normals_;
+
+ //StateCloud particles_;
+ DeviceArray<StateType> particles_;
+
+ // random number generate state
+ DeviceArray<curandState> rng_states;
+
+ int particle_num_;
+
+ std::vector<float> step_noise_covariance_;
+
+ std::vector<float> initial_noise_covariance_;
+
+ std::vector<float> initial_noise_mean_;
+
+ StateType motion_;
+
+ float motion_ratio_;
+
+ bool use_colors_;
+
+ StateType representative_state_;
+
+ /** \brief Height of input depth image. */
+ int rows_;
+ /** \brief Width of input depth image. */
+ int cols_;
+
+ };
+ }
+}
+
+#endif // PCL_GPU_TRACKING_PARTICLE_FILTER_H_
--- /dev/null
+#ifndef PCL_GPU_TRACKING_DEVICE_HPP_
+#define PCL_GPU_TRACKING_DEVICE_HPP_
+
+#include "internal.h"
+
+namespace pcl
+{
+ namespace device
+ {
+ __device__ __forceinline__ float
+ getSampleNormal (const float mean, const float cov, curandState* rng_state)
+ {
+ float rn = curand_normal(rng_state);
+ return (rn*cov + mean);
+ }
+ }
+}
+
+#endif // PCL_GPU_TRACKING_DEVICE_HPP_
\ No newline at end of file
--- /dev/null
+#include "device.hpp"
+#include <ctime>
+
+namespace pcl
+{
+ namespace device
+ {
+ struct KernelPolicy
+ {
+ enum
+ {
+ CTA_SIZE = 256,
+ WARP_SIZE = 32,
+ WARPS_COUNT = CTA_SIZE/WARP_SIZE,
+ };
+ };
+
+ struct ParticleInitializer
+ {
+ PtrSz<float> mean_;
+ PtrSz<float> cov_;
+
+ float8 representative_state_;
+
+ mutable PtrSz<StateType> particles_;
+ int num_particles_;
+
+ mutable PtrSz<curandState> rng_states_;
+ unsigned long int rng_seed;
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ unsigned int tid = threadIdx.x + blockDim.x;
+
+ curandState* rng_state = &rng_states_[tid];
+ curand_init ( rng_seed, tid, 0, rng_state );
+
+ StateType* p = &particles_[tid];
+ p->x = getSampleNormal(mean_[0], cov_[0], rng_state);
+ p->y = getSampleNormal(mean_[1], cov_[1], rng_state);
+ p->z = getSampleNormal(mean_[2], cov_[2], rng_state);
+
+ p->roll = getSampleNormal(mean_[3], cov_[3], rng_state);
+ p->pitch= getSampleNormal(mean_[4], cov_[4], rng_state);
+ p->yaw = getSampleNormal(mean_[5], cov_[5], rng_state);
+
+ p->x += representative_state_.x;
+ p->y += representative_state_.y;
+ p->z += representative_state_.z;
+
+ p->roll += representative_state_.roll;
+ p->pitch += representative_state_.pitch;
+ p->yaw += representative_state_.yaw;
+
+ p->weight= 1.0f / static_cast<float>(num_particles_);
+ }
+ };
+
+ struct ParticleFilter
+ {
+ PtrStepSz<float4> ref_;
+ PtrStepSz<uchar4> ref_color_;
+
+ PtrStepSz<float4> input_;
+ PtrStepSz<uchar4> input_color_;
+
+ PtrSz<curandState> rng_states_;
+
+ PtrSz<float> step_noise_covariance_;
+
+ PtrSz<float8> particles_;
+
+ float8 representative_state_;
+
+ float8 motion_;
+
+ float motion_ratio_;
+
+
+ // previous implementation
+ // 1. resampling
+ // 2. weight
+ // 3. weight normalization
+ // 4. update
+ // now we want
+ // 1. sampling
+ // 2. weight
+ // 3. weight normalization
+ // 4. resampling
+ // 5. update
+ //
+ // consideration
+ // 1. weight
+ // - find nearest point
+ // 2. resampling
+ // - build CDF
+
+ __device__ __forceinline__ void
+ operator () () const
+ {
+ // 1. resampling
+
+ __syncthreads();
+
+ // 2. weight
+ // transform ref
+ // find nearest correspondences
+ // coherence
+
+ // 3. weight normalization
+ // 3.1. find min, max - reduction
+ __syncthreads();
+ // 3.2. normalize weight using min, max
+ __syncthreads();
+ // 3.3. sum - reduction
+ __syncthreads();
+ // 3.4 normalize consider sum
+ __syncthreads();
+
+ // 4. update
+ // reduction
+
+ }
+ };
+
+ __global__ void
+ ParticleInitializerKernel(ParticleInitializer pi)
+ {
+ pi();
+ }
+
+ }
+}
+
+void
+ pcl::device::initParticles ( PtrSz<curandState> rng_states,
+ DeviceArray<float>& initial_noise_mean, DeviceArray<float>& initial_noise_covariance,
+ const StateType& representative_state,
+ DeviceArray<StateType>& particles )
+{
+ const int num_particles = particles.size();
+
+ ParticleInitializer pi;
+
+ pi.rng_seed = time(NULL);
+ pi.rng_states_ = rng_states;
+
+ pi.mean_ = initial_noise_mean;
+ pi.cov_ = initial_noise_covariance;
+
+ pi.representative_state_ = representative_state;
+
+ pi.particles_ = particles;
+ pi.num_particles_ = particles.size();
+
+ int block = pcl::device::KernelPolicy::WARP_SIZE;
+ int grid = divUp (particles.size(), block);
+
+ ParticleInitializerKernel<<<grid, block>>>(pi);
+
+ cudaSafeCall( cudaGetLastError() );
+ cudaSafeCall( cudaDeviceSynchronize() );
+}
+
+void
+ pcl::device::computeTracking ( const DeviceArray2D<PointType>& ref, const DeviceArray2D<PixelRGB>& ref_color,
+ const DeviceArray2D<PointType>& input, const DeviceArray2D<PixelRGB>& input_color,
+ PtrSz<curandState> rng_states, const DeviceArray<float>& step_noise_covariance,
+ DeviceArray<StateType>& particles,
+ StateType& representative_state, StateType& motion, float motion_ratio )
+{
+
+}
--- /dev/null
+#ifndef PCL_TRACKING_INTERNAL_H_
+#define PCL_TRACKING_INTERNAL_H_
+
+#include <pcl/gpu/containers/device_array.h>
+#include <pcl/gpu/utils/safe_call.hpp>
+
+#include <curand.h>
+#include <curand_kernel.h>
+
+namespace pcl
+{
+ namespace device
+ {
+
+ struct float8 { float x, y, z, w, roll, pitch, yaw, weight; };
+ typedef float8 StateType;
+
+ typedef float4 PointType;
+ typedef uchar4 PixelRGB;
+ typedef float4 NormalType;
+
+
+ void
+ initParticles (PtrSz<curandState> rng_states,
+ DeviceArray<float>& initial_noise_mean, DeviceArray<float>& initial_noise_covariance,
+ const StateType& representative_state,
+ DeviceArray<StateType>& particles);
+
+ void
+ computeTracking ( const DeviceArray2D<PointType>& ref, const DeviceArray2D<PixelRGB>& ref_color,
+ const DeviceArray2D<PointType>& input, const DeviceArray2D<PixelRGB>& input_color,
+ PtrSz<curandState> rng_states, const DeviceArray<float>& step_noise_covariance,
+ DeviceArray<StateType>& particles,
+ StateType& representative_state, StateType& motion, float motion_ratio );
+
+ /*
+ void
+ resample (StateXYZ& motion_xyz, StateRPY& motion_rpy, float motion_ratio,
+ int num_particles, PtrSz<StateXYZ>& particles_xyz_, PtrSz<StateRPY>& particles_rpy_, PtrSz<float>& particles_weight_);
+ void
+ weight (const PtrSz<PointType>& input, const PtrSz<PixelRGB>& input_color,
+ const PtrSz<PointType>& ref, const PtrSz<PixelRGB>& ref_color,
+ int num_particles, PtrSz<StateXYZ>& particles_xyz_, PtrSz<StateRPY>& particles_rpy_, PtrSz<float>& particles_weight_);
+ void
+ update (int num_particles, PtrSz<StateXYZ>& particles_xyz_, PtrSz<StateRPY>& particles_rpy_, PtrSz<float>& particles_weight_,
+ StateXYZ& representative_state_xyz, StateRPY& representative_state_rpy,
+ StateXYZ& motion_xyz, StateRPY& motion_rpy);
+ */
+ }
+}
+
+
+#endif // PCL_TRACKING_INTERNAL_H_
--- /dev/null
+set(SUBSYS_NAME gpu_utils)
+set(SUBSYS_PATH gpu/utils)
+set(SUBSYS_DESC "Device layer functions.")
+set(SUBSYS_DEPS common gpu_containers)
+
+set(build TRUE)
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS})
+PCL_SET_SUBSYS_INCLUDE_DIR("${SUBSYS_NAME}" "${SUBSYS_PATH}")
+mark_as_advanced("BUILD_${SUBSYS_NAME}")
+
+if(build)
+ FILE(GLOB incs include/pcl/gpu/utils/*.h*)
+ FILE(GLOB srcs src/*.cpp src/*.cu src/*.h*)
+ FILE(GLOB dev_incs include/pcl/gpu/utils/device/*.h*)
+ source_group("Header Files\\device" FILES ${dev_incs})
+ source_group("Source Files" FILES ${srcs})
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
+ PCL_CUDA_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${dev_incs} ${incs} ${srcs})
+ target_link_libraries("${LIB_NAME}" pcl_gpu_containers)
+
+ set(EXT_DEPS "")
+ #set(EXT_DEPS CUDA)
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "")
+
+ # Install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}" ${dev_incs} ${incs} ${srcs})
+endif()
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_PGU_DEVICE_ALGORITHM_HPP_
+#define PCL_PGU_DEVICE_ALGORITHM_HPP_
+
+namespace pcl
+{
+ namespace device
+ {
+ template <class T>
+ __device__ __host__ __forceinline__ void swap ( T& a, T& b )
+ {
+ T c(a); a=b; b=c;
+ }
+
+ template<typename Iterator, typename T, typename BinaryPredicate>
+ __host__ __device__ Iterator lower_bound(Iterator first, Iterator last, const T &val, BinaryPredicate comp)
+ {
+ int len = last - first;
+
+ while(len > 0)
+ {
+ int half = len >> 1;
+ Iterator middle = first;
+
+ middle += half;
+
+ if(comp(*middle, val))
+ {
+ first = middle;
+ ++first;
+ len = len - half - 1;
+ }
+ else
+ {
+ len = half;
+ }
+ }
+ return first;
+ }
+ }
+}
+#endif /* PCL_PGU_DEVICE_ALGORITHM_HPP_ */
\ No newline at end of file
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_DEVICE_ASM_HPP_
+#define PCL_DEVICE_ASM_HPP_
+
+namespace pcl
+{
+ namespace device
+ {
+
+ }
+}
+
+#endif /* PCL_DEVICE_ASM_HPP_ */
\ No newline at end of file
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_DEVICE_UTILS_BLOCK_HPP_
+#define PCL_DEVICE_UTILS_BLOCK_HPP_
+
+namespace pcl
+{
+ namespace device
+ {
+ struct Block
+ {
+ static __device__ __forceinline__ unsigned int id()
+ {
+ return blockIdx.x;
+ }
+
+ static __device__ __forceinline__ unsigned int stride()
+ {
+ return blockDim.x * blockDim.y * blockDim.z;
+ }
+
+ static __device__ __forceinline__ void sync()
+ {
+ __syncthreads();
+ }
+
+ static __device__ __forceinline__ int flattenedThreadId()
+ {
+ return threadIdx.z * blockDim.x * blockDim.y + threadIdx.y * blockDim.x + threadIdx.x;
+ }
+
+ template<typename It, typename T>
+ static __device__ __forceinline__ void fill(It beg, It end, const T& value)
+ {
+ int STRIDE = stride();
+ It t = beg + flattenedThreadId();
+
+ for(; t < end; t += STRIDE)
+ *t = value;
+ }
+
+ template<typename OutIt, typename T>
+ static __device__ __forceinline__ void yota(OutIt beg, OutIt end, T value)
+ {
+ int STRIDE = stride();
+ int tid = flattenedThreadId();
+ value += tid;
+
+ for(OutIt t = beg + tid; t < end; t += STRIDE, value += STRIDE)
+ *t = value;
+ }
+
+ template<typename InIt, typename OutIt>
+ static __device__ __forceinline__ void copy(InIt beg, InIt end, OutIt out)
+ {
+ int STRIDE = stride();
+ InIt t = beg + flattenedThreadId();
+ OutIt o = out + (t - beg);
+
+ for(; t < end; t += STRIDE, o += STRIDE)
+ *o = *t;
+ }
+
+ template<typename InIt, typename OutIt, class UnOp>
+ static __device__ __forceinline__ void transfrom(InIt beg, InIt end, OutIt out, UnOp op)
+ {
+ int STRIDE = stride();
+ InIt t = beg + flattenedThreadId();
+ OutIt o = out + (t - beg);
+
+ for(; t < end; t += STRIDE, o += STRIDE)
+ *o = op(*t);
+ }
+
+ template<typename InIt1, typename InIt2, typename OutIt, class BinOp>
+ static __device__ __forceinline__ void transfrom(InIt1 beg1, InIt1 end1, InIt2 beg2, OutIt out, BinOp op)
+ {
+ int STRIDE = stride();
+ InIt1 t1 = beg1 + flattenedThreadId();
+ InIt2 t2 = beg2 + flattenedThreadId();
+ OutIt o = out + (t1 - beg1);
+
+ for(; t1 < end1; t1 += STRIDE, t2 += STRIDE, o += STRIDE)
+ *o = op(*t1, *t2);
+ }
+
+ template<int CTA_SIZE, typename T, class BinOp>
+ static __device__ __forceinline__ void reduce(volatile T* buffer, BinOp op)
+ {
+ int tid = flattenedThreadId();
+ T val = buffer[tid];
+
+ if (CTA_SIZE >= 1024) { if (tid < 512) buffer[tid] = val = op(val, buffer[tid + 512]); __syncthreads(); }
+ if (CTA_SIZE >= 512) { if (tid < 256) buffer[tid] = val = op(val, buffer[tid + 256]); __syncthreads(); }
+ if (CTA_SIZE >= 256) { if (tid < 128) buffer[tid] = val = op(val, buffer[tid + 128]); __syncthreads(); }
+ if (CTA_SIZE >= 128) { if (tid < 64) buffer[tid] = val = op(val, buffer[tid + 64]); __syncthreads(); }
+
+ if (tid < 32)
+ {
+ if (CTA_SIZE >= 64) { buffer[tid] = val = op(val, buffer[tid + 32]); }
+ if (CTA_SIZE >= 32) { buffer[tid] = val = op(val, buffer[tid + 16]); }
+ if (CTA_SIZE >= 16) { buffer[tid] = val = op(val, buffer[tid + 8]); }
+ if (CTA_SIZE >= 8) { buffer[tid] = val = op(val, buffer[tid + 4]); }
+ if (CTA_SIZE >= 4) { buffer[tid] = val = op(val, buffer[tid + 2]); }
+ if (CTA_SIZE >= 2) { buffer[tid] = val = op(val, buffer[tid + 1]); }
+ }
+ }
+
+ template<int CTA_SIZE, typename T, class BinOp>
+ static __device__ __forceinline__ T reduce(volatile T* buffer, T init, BinOp op)
+ {
+ int tid = flattenedThreadId();
+ T val = buffer[tid] = init;
+ __syncthreads();
+
+ if (CTA_SIZE >= 1024) { if (tid < 512) buffer[tid] = val = op(val, buffer[tid + 512]); __syncthreads(); }
+ if (CTA_SIZE >= 512) { if (tid < 256) buffer[tid] = val = op(val, buffer[tid + 256]); __syncthreads(); }
+ if (CTA_SIZE >= 256) { if (tid < 128) buffer[tid] = val = op(val, buffer[tid + 128]); __syncthreads(); }
+ if (CTA_SIZE >= 128) { if (tid < 64) buffer[tid] = val = op(val, buffer[tid + 64]); __syncthreads(); }
+
+ if (tid < 32)
+ {
+ if (CTA_SIZE >= 64) { buffer[tid] = val = op(val, buffer[tid + 32]); }
+ if (CTA_SIZE >= 32) { buffer[tid] = val = op(val, buffer[tid + 16]); }
+ if (CTA_SIZE >= 16) { buffer[tid] = val = op(val, buffer[tid + 8]); }
+ if (CTA_SIZE >= 8) { buffer[tid] = val = op(val, buffer[tid + 4]); }
+ if (CTA_SIZE >= 4) { buffer[tid] = val = op(val, buffer[tid + 2]); }
+ if (CTA_SIZE >= 2) { buffer[tid] = val = op(val, buffer[tid + 1]); }
+ }
+ __syncthreads();
+ return buffer[0];
+ }
+
+ template <typename T, class BinOp>
+ static __device__ __forceinline__ void reduce_n(T* data, unsigned int n, BinOp op)
+ {
+ int ftid = flattenedThreadId();
+ int sft = stride();
+
+ if (sft < n)
+ {
+ for (unsigned int i = sft + ftid; i < n; i += sft)
+ data[ftid] = op(data[ftid], data[i]);
+
+ __syncthreads();
+
+ n = sft;
+ }
+
+ while (n > 1)
+ {
+ unsigned int half = n/2;
+
+ if (ftid < half)
+ data[ftid] = op(data[ftid], data[n - ftid - 1]);
+
+ __syncthreads();
+
+ n = n - half;
+ }
+ }
+ };
+ }
+}
+
+#endif /* PCL_DEVICE_UTILS_BLOCK_HPP_ */
+
--- /dev/null
+
+
+
+//will contain a lot of load/store utils
+
+namespace pcl
+{
+ namespace device
+ {
+ template<class T>
+ struct NonCachedLoad
+ {
+ __device__ static T Invoke(const T* ptr)
+ {
+
+#if (__CUDA_ARCH__ < 200)
+ return *ptr;
+#else
+ //asm code insertion
+ asm(...);
+#endif
+ }
+ };
+
+ }
+
+}
\ No newline at end of file
--- /dev/null
+/*
+ * Copyright 1993-2010 NVIDIA Corporation. All rights reserved.
+ *
+ * Please refer to the NVIDIA end user license agreement (EULA) associated
+ * with this source code for terms and conditions that govern your use of
+ * this software. Any use, reproduction, disclosure, or distribution of
+ * this software and related documentation outside the terms of the EULA
+ * is strictly prohibited.
+ *
+ */
+
+/*
+ This file implements common mathematical operations on vector types
+ (float3, float4 etc.) since these are not provided as standard by CUDA.
+
+ The syntax is modelled on the Cg standard library.
+
+ This is part of the CUTIL library and is not supported by NVIDIA.
+
+ Thanks to Linh Hah for additions and fixes.
+*/
+
+#ifndef CUTIL_MATH_H
+#define CUTIL_MATH_H
+
+#include "cuda_runtime.h"
+
+typedef unsigned int uint;
+typedef unsigned short ushort;
+
+#ifndef __CUDACC__
+#include <math.h>
+
+////////////////////////////////////////////////////////////////////////////////
+// host implementations of CUDA functions
+////////////////////////////////////////////////////////////////////////////////
+
+inline float fminf(float a, float b)
+{
+ return a < b ? a : b;
+}
+
+inline float fmaxf(float a, float b)
+{
+ return a > b ? a : b;
+}
+
+inline int max(int a, int b)
+{
+ return a > b ? a : b;
+}
+
+inline int min(int a, int b)
+{
+ return a < b ? a : b;
+}
+
+inline float rsqrtf(float x)
+{
+ return 1.0f / sqrtf(x);
+}
+#endif
+
+////////////////////////////////////////////////////////////////////////////////
+// constructors
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 make_float2(float s)
+{
+ return make_float2(s, s);
+}
+inline __host__ __device__ float2 make_float2(float3 a)
+{
+ return make_float2(a.x, a.y);
+}
+inline __host__ __device__ float2 make_float2(int2 a)
+{
+ return make_float2(float(a.x), float(a.y));
+}
+inline __host__ __device__ float2 make_float2(uint2 a)
+{
+ return make_float2(float(a.x), float(a.y));
+}
+
+inline __host__ __device__ int2 make_int2(int s)
+{
+ return make_int2(s, s);
+}
+inline __host__ __device__ int2 make_int2(int3 a)
+{
+ return make_int2(a.x, a.y);
+}
+inline __host__ __device__ int2 make_int2(uint2 a)
+{
+ return make_int2(int(a.x), int(a.y));
+}
+inline __host__ __device__ int2 make_int2(float2 a)
+{
+ return make_int2(int(a.x), int(a.y));
+}
+
+inline __host__ __device__ uint2 make_uint2(uint s)
+{
+ return make_uint2(s, s);
+}
+inline __host__ __device__ uint2 make_uint2(uint3 a)
+{
+ return make_uint2(a.x, a.y);
+}
+inline __host__ __device__ uint2 make_uint2(int2 a)
+{
+ return make_uint2(uint(a.x), uint(a.y));
+}
+
+inline __host__ __device__ float3 make_float3(float s)
+{
+ return make_float3(s, s, s);
+}
+inline __host__ __device__ float3 make_float3(float2 a)
+{
+ return make_float3(a.x, a.y, 0.0f);
+}
+inline __host__ __device__ float3 make_float3(float2 a, float s)
+{
+ return make_float3(a.x, a.y, s);
+}
+inline __host__ __device__ float3 make_float3(float4 a)
+{
+ return make_float3(a.x, a.y, a.z);
+}
+inline __host__ __device__ float3 make_float3(int3 a)
+{
+ return make_float3(float(a.x), float(a.y), float(a.z));
+}
+inline __host__ __device__ float3 make_float3(uint3 a)
+{
+ return make_float3(float(a.x), float(a.y), float(a.z));
+}
+
+inline __host__ __device__ int3 make_int3(int s)
+{
+ return make_int3(s, s, s);
+}
+inline __host__ __device__ int3 make_int3(int2 a)
+{
+ return make_int3(a.x, a.y, 0);
+}
+inline __host__ __device__ int3 make_int3(int2 a, int s)
+{
+ return make_int3(a.x, a.y, s);
+}
+inline __host__ __device__ int3 make_int3(uint3 a)
+{
+ return make_int3(int(a.x), int(a.y), int(a.z));
+}
+inline __host__ __device__ int3 make_int3(float3 a)
+{
+ return make_int3(int(a.x), int(a.y), int(a.z));
+}
+
+inline __host__ __device__ uint3 make_uint3(uint s)
+{
+ return make_uint3(s, s, s);
+}
+inline __host__ __device__ uint3 make_uint3(uint2 a)
+{
+ return make_uint3(a.x, a.y, 0);
+}
+inline __host__ __device__ uint3 make_uint3(uint2 a, uint s)
+{
+ return make_uint3(a.x, a.y, s);
+}
+inline __host__ __device__ uint3 make_uint3(uint4 a)
+{
+ return make_uint3(a.x, a.y, a.z);
+}
+inline __host__ __device__ uint3 make_uint3(int3 a)
+{
+ return make_uint3(uint(a.x), uint(a.y), uint(a.z));
+}
+
+inline __host__ __device__ float4 make_float4(float s)
+{
+ return make_float4(s, s, s, s);
+}
+inline __host__ __device__ float4 make_float4(float3 a)
+{
+ return make_float4(a.x, a.y, a.z, 0.0f);
+}
+inline __host__ __device__ float4 make_float4(float3 a, float w)
+{
+ return make_float4(a.x, a.y, a.z, w);
+}
+inline __host__ __device__ float4 make_float4(int4 a)
+{
+ return make_float4(float(a.x), float(a.y), float(a.z), float(a.w));
+}
+inline __host__ __device__ float4 make_float4(uint4 a)
+{
+ return make_float4(float(a.x), float(a.y), float(a.z), float(a.w));
+}
+
+inline __host__ __device__ int4 make_int4(int s)
+{
+ return make_int4(s, s, s, s);
+}
+inline __host__ __device__ int4 make_int4(int3 a)
+{
+ return make_int4(a.x, a.y, a.z, 0);
+}
+inline __host__ __device__ int4 make_int4(int3 a, int w)
+{
+ return make_int4(a.x, a.y, a.z, w);
+}
+inline __host__ __device__ int4 make_int4(uint4 a)
+{
+ return make_int4(int(a.x), int(a.y), int(a.z), int(a.w));
+}
+inline __host__ __device__ int4 make_int4(float4 a)
+{
+ return make_int4(int(a.x), int(a.y), int(a.z), int(a.w));
+}
+
+
+inline __host__ __device__ uint4 make_uint4(uint s)
+{
+ return make_uint4(s, s, s, s);
+}
+inline __host__ __device__ uint4 make_uint4(uint3 a)
+{
+ return make_uint4(a.x, a.y, a.z, 0);
+}
+inline __host__ __device__ uint4 make_uint4(uint3 a, uint w)
+{
+ return make_uint4(a.x, a.y, a.z, w);
+}
+inline __host__ __device__ uint4 make_uint4(int4 a)
+{
+ return make_uint4(uint(a.x), uint(a.y), uint(a.z), uint(a.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// negate
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator-(float2 &a)
+{
+ return make_float2(-a.x, -a.y);
+}
+inline __host__ __device__ int2 operator-(int2 &a)
+{
+ return make_int2(-a.x, -a.y);
+}
+inline __host__ __device__ float3 operator-(float3 &a)
+{
+ return make_float3(-a.x, -a.y, -a.z);
+}
+inline __host__ __device__ int3 operator-(int3 &a)
+{
+ return make_int3(-a.x, -a.y, -a.z);
+}
+inline __host__ __device__ float4 operator-(float4 &a)
+{
+ return make_float4(-a.x, -a.y, -a.z, -a.w);
+}
+inline __host__ __device__ int4 operator-(int4 &a)
+{
+ return make_int4(-a.x, -a.y, -a.z, -a.w);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// addition
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator+(float2 a, float2 b)
+{
+ return make_float2(a.x + b.x, a.y + b.y);
+}
+inline __host__ __device__ void operator+=(float2 &a, float2 b)
+{
+ a.x += b.x; a.y += b.y;
+}
+inline __host__ __device__ float2 operator+(float2 a, float b)
+{
+ return make_float2(a.x + b, a.y + b);
+}
+inline __host__ __device__ float2 operator+(float b, float2 a)
+{
+ return make_float2(a.x + b, a.y + b);
+}
+inline __host__ __device__ void operator+=(float2 &a, float b)
+{
+ a.x += b; a.y += b;
+}
+
+inline __host__ __device__ int2 operator+(int2 a, int2 b)
+{
+ return make_int2(a.x + b.x, a.y + b.y);
+}
+inline __host__ __device__ void operator+=(int2 &a, int2 b)
+{
+ a.x += b.x; a.y += b.y;
+}
+inline __host__ __device__ int2 operator+(int2 a, int b)
+{
+ return make_int2(a.x + b, a.y + b);
+}
+inline __host__ __device__ int2 operator+(int b, int2 a)
+{
+ return make_int2(a.x + b, a.y + b);
+}
+inline __host__ __device__ void operator+=(int2 &a, int b)
+{
+ a.x += b; a.y += b;
+}
+
+inline __host__ __device__ uint2 operator+(uint2 a, uint2 b)
+{
+ return make_uint2(a.x + b.x, a.y + b.y);
+}
+inline __host__ __device__ void operator+=(uint2 &a, uint2 b)
+{
+ a.x += b.x; a.y += b.y;
+}
+inline __host__ __device__ uint2 operator+(uint2 a, uint b)
+{
+ return make_uint2(a.x + b, a.y + b);
+}
+inline __host__ __device__ uint2 operator+(uint b, uint2 a)
+{
+ return make_uint2(a.x + b, a.y + b);
+}
+inline __host__ __device__ void operator+=(uint2 &a, uint b)
+{
+ a.x += b; a.y += b;
+}
+
+
+inline __host__ __device__ float3 operator+(float3 a, float3 b)
+{
+ return make_float3(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+inline __host__ __device__ void operator+=(float3 &a, float3 b)
+{
+ a.x += b.x; a.y += b.y; a.z += b.z;
+}
+inline __host__ __device__ float3 operator+(float3 a, float b)
+{
+ return make_float3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ void operator+=(float3 &a, float b)
+{
+ a.x += b; a.y += b; a.z += b;
+}
+
+inline __host__ __device__ int3 operator+(int3 a, int3 b)
+{
+ return make_int3(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+inline __host__ __device__ void operator+=(int3 &a, int3 b)
+{
+ a.x += b.x; a.y += b.y; a.z += b.z;
+}
+inline __host__ __device__ int3 operator+(int3 a, int b)
+{
+ return make_int3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ void operator+=(int3 &a, int b)
+{
+ a.x += b; a.y += b; a.z += b;
+}
+
+inline __host__ __device__ uint3 operator+(uint3 a, uint3 b)
+{
+ return make_uint3(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+inline __host__ __device__ void operator+=(uint3 &a, uint3 b)
+{
+ a.x += b.x; a.y += b.y; a.z += b.z;
+}
+inline __host__ __device__ uint3 operator+(uint3 a, uint b)
+{
+ return make_uint3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ void operator+=(uint3 &a, uint b)
+{
+ a.x += b; a.y += b; a.z += b;
+}
+
+inline __host__ __device__ int3 operator+(int b, int3 a)
+{
+ return make_int3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ uint3 operator+(uint b, uint3 a)
+{
+ return make_uint3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ float3 operator+(float b, float3 a)
+{
+ return make_float3(a.x + b, a.y + b, a.z + b);
+}
+
+inline __host__ __device__ float4 operator+(float4 a, float4 b)
+{
+ return make_float4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
+}
+inline __host__ __device__ void operator+=(float4 &a, float4 b)
+{
+ a.x += b.x; a.y += b.y; a.z += b.z; a.w += b.w;
+}
+inline __host__ __device__ float4 operator+(float4 a, float b)
+{
+ return make_float4(a.x + b, a.y + b, a.z + b, a.w + b);
+}
+inline __host__ __device__ float4 operator+(float b, float4 a)
+{
+ return make_float4(a.x + b, a.y + b, a.z + b, a.w + b);
+}
+inline __host__ __device__ void operator+=(float4 &a, float b)
+{
+ a.x += b; a.y += b; a.z += b; a.w += b;
+}
+
+inline __host__ __device__ int4 operator+(int4 a, int4 b)
+{
+ return make_int4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
+}
+inline __host__ __device__ void operator+=(int4 &a, int4 b)
+{
+ a.x += b.x; a.y += b.y; a.z += b.z; a.w += b.w;
+}
+inline __host__ __device__ int4 operator+(int4 a, int b)
+{
+ return make_int4(a.x + b, a.y + b, a.z + b, a.w + b);
+}
+inline __host__ __device__ int4 operator+(int b, int4 a)
+{
+ return make_int4(a.x + b, a.y + b, a.z + b, a.w + b);
+}
+inline __host__ __device__ void operator+=(int4 &a, int b)
+{
+ a.x += b; a.y += b; a.z += b; a.w += b;
+}
+
+inline __host__ __device__ uint4 operator+(uint4 a, uint4 b)
+{
+ return make_uint4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
+}
+inline __host__ __device__ void operator+=(uint4 &a, uint4 b)
+{
+ a.x += b.x; a.y += b.y; a.z += b.z; a.w += b.w;
+}
+inline __host__ __device__ uint4 operator+(uint4 a, uint b)
+{
+ return make_uint4(a.x + b, a.y + b, a.z + b, a.w + b);
+}
+inline __host__ __device__ uint4 operator+(uint b, uint4 a)
+{
+ return make_uint4(a.x + b, a.y + b, a.z + b, a.w + b);
+}
+inline __host__ __device__ void operator+=(uint4 &a, uint b)
+{
+ a.x += b; a.y += b; a.z += b; a.w += b;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// subtract
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator-(float2 a, float2 b)
+{
+ return make_float2(a.x - b.x, a.y - b.y);
+}
+inline __host__ __device__ void operator-=(float2 &a, float2 b)
+{
+ a.x -= b.x; a.y -= b.y;
+}
+inline __host__ __device__ float2 operator-(float2 a, float b)
+{
+ return make_float2(a.x - b, a.y - b);
+}
+inline __host__ __device__ float2 operator-(float b, float2 a)
+{
+ return make_float2(b - a.x, b - a.y);
+}
+inline __host__ __device__ void operator-=(float2 &a, float b)
+{
+ a.x -= b; a.y -= b;
+}
+
+inline __host__ __device__ int2 operator-(int2 a, int2 b)
+{
+ return make_int2(a.x - b.x, a.y - b.y);
+}
+inline __host__ __device__ void operator-=(int2 &a, int2 b)
+{
+ a.x -= b.x; a.y -= b.y;
+}
+inline __host__ __device__ int2 operator-(int2 a, int b)
+{
+ return make_int2(a.x - b, a.y - b);
+}
+inline __host__ __device__ int2 operator-(int b, int2 a)
+{
+ return make_int2(b - a.x, b - a.y);
+}
+inline __host__ __device__ void operator-=(int2 &a, int b)
+{
+ a.x -= b; a.y -= b;
+}
+
+inline __host__ __device__ uint2 operator-(uint2 a, uint2 b)
+{
+ return make_uint2(a.x - b.x, a.y - b.y);
+}
+inline __host__ __device__ void operator-=(uint2 &a, uint2 b)
+{
+ a.x -= b.x; a.y -= b.y;
+}
+inline __host__ __device__ uint2 operator-(uint2 a, uint b)
+{
+ return make_uint2(a.x - b, a.y - b);
+}
+inline __host__ __device__ uint2 operator-(uint b, uint2 a)
+{
+ return make_uint2(b - a.x, b - a.y);
+}
+inline __host__ __device__ void operator-=(uint2 &a, uint b)
+{
+ a.x -= b; a.y -= b;
+}
+
+inline __host__ __device__ float3 operator-(float3 a, float3 b)
+{
+ return make_float3(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+inline __host__ __device__ void operator-=(float3 &a, float3 b)
+{
+ a.x -= b.x; a.y -= b.y; a.z -= b.z;
+}
+inline __host__ __device__ float3 operator-(float3 a, float b)
+{
+ return make_float3(a.x - b, a.y - b, a.z - b);
+}
+inline __host__ __device__ float3 operator-(float b, float3 a)
+{
+ return make_float3(b - a.x, b - a.y, b - a.z);
+}
+inline __host__ __device__ void operator-=(float3 &a, float b)
+{
+ a.x -= b; a.y -= b; a.z -= b;
+}
+
+inline __host__ __device__ int3 operator-(int3 a, int3 b)
+{
+ return make_int3(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+inline __host__ __device__ void operator-=(int3 &a, int3 b)
+{
+ a.x -= b.x; a.y -= b.y; a.z -= b.z;
+}
+inline __host__ __device__ int3 operator-(int3 a, int b)
+{
+ return make_int3(a.x - b, a.y - b, a.z - b);
+}
+inline __host__ __device__ int3 operator-(int b, int3 a)
+{
+ return make_int3(b - a.x, b - a.y, b - a.z);
+}
+inline __host__ __device__ void operator-=(int3 &a, int b)
+{
+ a.x -= b; a.y -= b; a.z -= b;
+}
+
+inline __host__ __device__ uint3 operator-(uint3 a, uint3 b)
+{
+ return make_uint3(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+inline __host__ __device__ void operator-=(uint3 &a, uint3 b)
+{
+ a.x -= b.x; a.y -= b.y; a.z -= b.z;
+}
+inline __host__ __device__ uint3 operator-(uint3 a, uint b)
+{
+ return make_uint3(a.x - b, a.y - b, a.z - b);
+}
+inline __host__ __device__ uint3 operator-(uint b, uint3 a)
+{
+ return make_uint3(b - a.x, b - a.y, b - a.z);
+}
+inline __host__ __device__ void operator-=(uint3 &a, uint b)
+{
+ a.x -= b; a.y -= b; a.z -= b;
+}
+
+inline __host__ __device__ float4 operator-(float4 a, float4 b)
+{
+ return make_float4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
+}
+inline __host__ __device__ void operator-=(float4 &a, float4 b)
+{
+ a.x -= b.x; a.y -= b.y; a.z -= b.z; a.w -= b.w;
+}
+inline __host__ __device__ float4 operator-(float4 a, float b)
+{
+ return make_float4(a.x - b, a.y - b, a.z - b, a.w - b);
+}
+inline __host__ __device__ void operator-=(float4 &a, float b)
+{
+ a.x -= b; a.y -= b; a.z -= b; a.w -= b;
+}
+
+inline __host__ __device__ int4 operator-(int4 a, int4 b)
+{
+ return make_int4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
+}
+inline __host__ __device__ void operator-=(int4 &a, int4 b)
+{
+ a.x -= b.x; a.y -= b.y; a.z -= b.z; a.w -= b.w;
+}
+inline __host__ __device__ int4 operator-(int4 a, int b)
+{
+ return make_int4(a.x - b, a.y - b, a.z - b, a.w - b);
+}
+inline __host__ __device__ int4 operator-(int b, int4 a)
+{
+ return make_int4(b - a.x, b - a.y, b - a.z, b - a.w);
+}
+inline __host__ __device__ void operator-=(int4 &a, int b)
+{
+ a.x -= b; a.y -= b; a.z -= b; a.w -= b;
+}
+
+inline __host__ __device__ uint4 operator-(uint4 a, uint4 b)
+{
+ return make_uint4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
+}
+inline __host__ __device__ void operator-=(uint4 &a, uint4 b)
+{
+ a.x -= b.x; a.y -= b.y; a.z -= b.z; a.w -= b.w;
+}
+inline __host__ __device__ uint4 operator-(uint4 a, uint b)
+{
+ return make_uint4(a.x - b, a.y - b, a.z - b, a.w - b);
+}
+inline __host__ __device__ uint4 operator-(uint b, uint4 a)
+{
+ return make_uint4(b - a.x, b - a.y, b - a.z, b - a.w);
+}
+inline __host__ __device__ void operator-=(uint4 &a, uint b)
+{
+ a.x -= b; a.y -= b; a.z -= b; a.w -= b;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// multiply
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator*(float2 a, float2 b)
+{
+ return make_float2(a.x * b.x, a.y * b.y);
+}
+inline __host__ __device__ void operator*=(float2 &a, float2 b)
+{
+ a.x *= b.x; a.y *= b.y;
+}
+inline __host__ __device__ float2 operator*(float2 a, float b)
+{
+ return make_float2(a.x * b, a.y * b);
+}
+inline __host__ __device__ float2 operator*(float b, float2 a)
+{
+ return make_float2(b * a.x, b * a.y);
+}
+inline __host__ __device__ void operator*=(float2 &a, float b)
+{
+ a.x *= b; a.y *= b;
+}
+
+inline __host__ __device__ int2 operator*(int2 a, int2 b)
+{
+ return make_int2(a.x * b.x, a.y * b.y);
+}
+inline __host__ __device__ void operator*=(int2 &a, int2 b)
+{
+ a.x *= b.x; a.y *= b.y;
+}
+inline __host__ __device__ int2 operator*(int2 a, int b)
+{
+ return make_int2(a.x * b, a.y * b);
+}
+inline __host__ __device__ int2 operator*(int b, int2 a)
+{
+ return make_int2(b * a.x, b * a.y);
+}
+inline __host__ __device__ void operator*=(int2 &a, int b)
+{
+ a.x *= b; a.y *= b;
+}
+
+inline __host__ __device__ uint2 operator*(uint2 a, uint2 b)
+{
+ return make_uint2(a.x * b.x, a.y * b.y);
+}
+inline __host__ __device__ void operator*=(uint2 &a, uint2 b)
+{
+ a.x *= b.x; a.y *= b.y;
+}
+inline __host__ __device__ uint2 operator*(uint2 a, uint b)
+{
+ return make_uint2(a.x * b, a.y * b);
+}
+inline __host__ __device__ uint2 operator*(uint b, uint2 a)
+{
+ return make_uint2(b * a.x, b * a.y);
+}
+inline __host__ __device__ void operator*=(uint2 &a, uint b)
+{
+ a.x *= b; a.y *= b;
+}
+
+inline __host__ __device__ float3 operator*(float3 a, float3 b)
+{
+ return make_float3(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+inline __host__ __device__ void operator*=(float3 &a, float3 b)
+{
+ a.x *= b.x; a.y *= b.y; a.z *= b.z;
+}
+inline __host__ __device__ float3 operator*(float3 a, float b)
+{
+ return make_float3(a.x * b, a.y * b, a.z * b);
+}
+inline __host__ __device__ float3 operator*(float b, float3 a)
+{
+ return make_float3(b * a.x, b * a.y, b * a.z);
+}
+inline __host__ __device__ void operator*=(float3 &a, float b)
+{
+ a.x *= b; a.y *= b; a.z *= b;
+}
+
+inline __host__ __device__ int3 operator*(int3 a, int3 b)
+{
+ return make_int3(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+inline __host__ __device__ void operator*=(int3 &a, int3 b)
+{
+ a.x *= b.x; a.y *= b.y; a.z *= b.z;
+}
+inline __host__ __device__ int3 operator*(int3 a, int b)
+{
+ return make_int3(a.x * b, a.y * b, a.z * b);
+}
+inline __host__ __device__ int3 operator*(int b, int3 a)
+{
+ return make_int3(b * a.x, b * a.y, b * a.z);
+}
+inline __host__ __device__ void operator*=(int3 &a, int b)
+{
+ a.x *= b; a.y *= b; a.z *= b;
+}
+
+inline __host__ __device__ uint3 operator*(uint3 a, uint3 b)
+{
+ return make_uint3(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+inline __host__ __device__ void operator*=(uint3 &a, uint3 b)
+{
+ a.x *= b.x; a.y *= b.y; a.z *= b.z;
+}
+inline __host__ __device__ uint3 operator*(uint3 a, uint b)
+{
+ return make_uint3(a.x * b, a.y * b, a.z * b);
+}
+inline __host__ __device__ uint3 operator*(uint b, uint3 a)
+{
+ return make_uint3(b * a.x, b * a.y, b * a.z);
+}
+inline __host__ __device__ void operator*=(uint3 &a, uint b)
+{
+ a.x *= b; a.y *= b; a.z *= b;
+}
+
+inline __host__ __device__ float4 operator*(float4 a, float4 b)
+{
+ return make_float4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w);
+}
+inline __host__ __device__ void operator*=(float4 &a, float4 b)
+{
+ a.x *= b.x; a.y *= b.y; a.z *= b.z; a.w *= b.w;
+}
+inline __host__ __device__ float4 operator*(float4 a, float b)
+{
+ return make_float4(a.x * b, a.y * b, a.z * b, a.w * b);
+}
+inline __host__ __device__ float4 operator*(float b, float4 a)
+{
+ return make_float4(b * a.x, b * a.y, b * a.z, b * a.w);
+}
+inline __host__ __device__ void operator*=(float4 &a, float b)
+{
+ a.x *= b; a.y *= b; a.z *= b; a.w *= b;
+}
+
+inline __host__ __device__ int4 operator*(int4 a, int4 b)
+{
+ return make_int4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w);
+}
+inline __host__ __device__ void operator*=(int4 &a, int4 b)
+{
+ a.x *= b.x; a.y *= b.y; a.z *= b.z; a.w *= b.w;
+}
+inline __host__ __device__ int4 operator*(int4 a, int b)
+{
+ return make_int4(a.x * b, a.y * b, a.z * b, a.w * b);
+}
+inline __host__ __device__ int4 operator*(int b, int4 a)
+{
+ return make_int4(b * a.x, b * a.y, b * a.z, b * a.w);
+}
+inline __host__ __device__ void operator*=(int4 &a, int b)
+{
+ a.x *= b; a.y *= b; a.z *= b; a.w *= b;
+}
+
+inline __host__ __device__ uint4 operator*(uint4 a, uint4 b)
+{
+ return make_uint4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w);
+}
+inline __host__ __device__ void operator*=(uint4 &a, uint4 b)
+{
+ a.x *= b.x; a.y *= b.y; a.z *= b.z; a.w *= b.w;
+}
+inline __host__ __device__ uint4 operator*(uint4 a, uint b)
+{
+ return make_uint4(a.x * b, a.y * b, a.z * b, a.w * b);
+}
+inline __host__ __device__ uint4 operator*(uint b, uint4 a)
+{
+ return make_uint4(b * a.x, b * a.y, b * a.z, b * a.w);
+}
+inline __host__ __device__ void operator*=(uint4 &a, uint b)
+{
+ a.x *= b; a.y *= b; a.z *= b; a.w *= b;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// divide
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator/(float2 a, float2 b)
+{
+ return make_float2(a.x / b.x, a.y / b.y);
+}
+inline __host__ __device__ void operator/=(float2 &a, float2 b)
+{
+ a.x /= b.x; a.y /= b.y;
+}
+inline __host__ __device__ float2 operator/(float2 a, float b)
+{
+ return make_float2(a.x / b, a.y / b);
+}
+inline __host__ __device__ void operator/=(float2 &a, float b)
+{
+ a.x /= b; a.y /= b;
+}
+inline __host__ __device__ float2 operator/(float b, float2 a)
+{
+ return make_float2(b / a.x, b / a.y);
+}
+
+inline __host__ __device__ float3 operator/(float3 a, float3 b)
+{
+ return make_float3(a.x / b.x, a.y / b.y, a.z / b.z);
+}
+inline __host__ __device__ void operator/=(float3 &a, float3 b)
+{
+ a.x /= b.x; a.y /= b.y; a.z /= b.z;
+}
+inline __host__ __device__ float3 operator/(float3 a, float b)
+{
+ return make_float3(a.x / b, a.y / b, a.z / b);
+}
+inline __host__ __device__ void operator/=(float3 &a, float b)
+{
+ a.x /= b; a.y /= b; a.z /= b;
+}
+inline __host__ __device__ float3 operator/(float b, float3 a)
+{
+ return make_float3(b / a.x, b / a.y, b / a.z);
+}
+
+inline __host__ __device__ float4 operator/(float4 a, float4 b)
+{
+ return make_float4(a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w);
+}
+inline __host__ __device__ void operator/=(float4 &a, float4 b)
+{
+ a.x /= b.x; a.y /= b.y; a.z /= b.z; a.w /= b.w;
+}
+inline __host__ __device__ float4 operator/(float4 a, float b)
+{
+ return make_float4(a.x / b, a.y / b, a.z / b, a.w / b);
+}
+inline __host__ __device__ void operator/=(float4 &a, float b)
+{
+ a.x /= b; a.y /= b; a.z /= b; a.w /= b;
+}
+inline __host__ __device__ float4 operator/(float b, float4 a){
+ return make_float4(b / a.x, b / a.y, b / a.z, b / a.w);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// min
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 fminf(float2 a, float2 b)
+{
+ return make_float2(fminf(a.x,b.x), fminf(a.y,b.y));
+}
+inline __host__ __device__ float3 fminf(float3 a, float3 b)
+{
+ return make_float3(fminf(a.x,b.x), fminf(a.y,b.y), fminf(a.z,b.z));
+}
+inline __host__ __device__ float4 fminf(float4 a, float4 b)
+{
+ return make_float4(fminf(a.x,b.x), fminf(a.y,b.y), fminf(a.z,b.z), fminf(a.w,b.w));
+}
+
+inline __host__ __device__ int2 min(int2 a, int2 b)
+{
+ return make_int2(min(a.x,b.x), min(a.y,b.y));
+}
+inline __host__ __device__ int3 min(int3 a, int3 b)
+{
+ return make_int3(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z));
+}
+inline __host__ __device__ int4 min(int4 a, int4 b)
+{
+ return make_int4(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z), min(a.w,b.w));
+}
+
+inline __host__ __device__ uint2 min(uint2 a, uint2 b)
+{
+ return make_uint2(min(a.x,b.x), min(a.y,b.y));
+}
+inline __host__ __device__ uint3 min(uint3 a, uint3 b)
+{
+ return make_uint3(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z));
+}
+inline __host__ __device__ uint4 min(uint4 a, uint4 b)
+{
+ return make_uint4(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z), min(a.w,b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// max
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 fmaxf(float2 a, float2 b)
+{
+ return make_float2(fmaxf(a.x,b.x), fmaxf(a.y,b.y));
+}
+inline __host__ __device__ float3 fmaxf(float3 a, float3 b)
+{
+ return make_float3(fmaxf(a.x,b.x), fmaxf(a.y,b.y), fmaxf(a.z,b.z));
+}
+inline __host__ __device__ float4 fmaxf(float4 a, float4 b)
+{
+ return make_float4(fmaxf(a.x,b.x), fmaxf(a.y,b.y), fmaxf(a.z,b.z), fmaxf(a.w,b.w));
+}
+
+inline __host__ __device__ int2 max(int2 a, int2 b)
+{
+ return make_int2(max(a.x,b.x), max(a.y,b.y));
+}
+inline __host__ __device__ int3 max(int3 a, int3 b)
+{
+ return make_int3(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z));
+}
+inline __host__ __device__ int4 max(int4 a, int4 b)
+{
+ return make_int4(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z), max(a.w,b.w));
+}
+
+inline __host__ __device__ uint2 max(uint2 a, uint2 b)
+{
+ return make_uint2(max(a.x,b.x), max(a.y,b.y));
+}
+inline __host__ __device__ uint3 max(uint3 a, uint3 b)
+{
+ return make_uint3(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z));
+}
+inline __host__ __device__ uint4 max(uint4 a, uint4 b)
+{
+ return make_uint4(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z), max(a.w,b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// lerp
+// - linear interpolation between a and b, based on value t in [0, 1] range
+////////////////////////////////////////////////////////////////////////////////
+
+inline __device__ __host__ float lerp(float a, float b, float t)
+{
+ return a + t*(b-a);
+}
+inline __device__ __host__ float2 lerp(float2 a, float2 b, float t)
+{
+ return a + t*(b-a);
+}
+inline __device__ __host__ float3 lerp(float3 a, float3 b, float t)
+{
+ return a + t*(b-a);
+}
+inline __device__ __host__ float4 lerp(float4 a, float4 b, float t)
+{
+ return a + t*(b-a);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// clamp
+// - clamp the value v to be in the range [a, b]
+////////////////////////////////////////////////////////////////////////////////
+
+inline __device__ __host__ float clamp(float f, float a, float b)
+{
+ return fmaxf(a, fminf(f, b));
+}
+inline __device__ __host__ int clamp(int f, int a, int b)
+{
+ return max(a, min(f, b));
+}
+inline __device__ __host__ uint clamp(uint f, uint a, uint b)
+{
+ return max(a, min(f, b));
+}
+
+inline __device__ __host__ float2 clamp(float2 v, float a, float b)
+{
+ return make_float2(clamp(v.x, a, b), clamp(v.y, a, b));
+}
+inline __device__ __host__ float2 clamp(float2 v, float2 a, float2 b)
+{
+ return make_float2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y));
+}
+inline __device__ __host__ float3 clamp(float3 v, float a, float b)
+{
+ return make_float3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b));
+}
+inline __device__ __host__ float3 clamp(float3 v, float3 a, float3 b)
+{
+ return make_float3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z));
+}
+inline __device__ __host__ float4 clamp(float4 v, float a, float b)
+{
+ return make_float4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b));
+}
+inline __device__ __host__ float4 clamp(float4 v, float4 a, float4 b)
+{
+ return make_float4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w));
+}
+
+inline __device__ __host__ int2 clamp(int2 v, int a, int b)
+{
+ return make_int2(clamp(v.x, a, b), clamp(v.y, a, b));
+}
+inline __device__ __host__ int2 clamp(int2 v, int2 a, int2 b)
+{
+ return make_int2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y));
+}
+inline __device__ __host__ int3 clamp(int3 v, int a, int b)
+{
+ return make_int3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b));
+}
+inline __device__ __host__ int3 clamp(int3 v, int3 a, int3 b)
+{
+ return make_int3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z));
+}
+inline __device__ __host__ int4 clamp(int4 v, int a, int b)
+{
+ return make_int4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b));
+}
+inline __device__ __host__ int4 clamp(int4 v, int4 a, int4 b)
+{
+ return make_int4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w));
+}
+
+inline __device__ __host__ uint2 clamp(uint2 v, uint a, uint b)
+{
+ return make_uint2(clamp(v.x, a, b), clamp(v.y, a, b));
+}
+inline __device__ __host__ uint2 clamp(uint2 v, uint2 a, uint2 b)
+{
+ return make_uint2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y));
+}
+inline __device__ __host__ uint3 clamp(uint3 v, uint a, uint b)
+{
+ return make_uint3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b));
+}
+inline __device__ __host__ uint3 clamp(uint3 v, uint3 a, uint3 b)
+{
+ return make_uint3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z));
+}
+inline __device__ __host__ uint4 clamp(uint4 v, uint a, uint b)
+{
+ return make_uint4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b));
+}
+inline __device__ __host__ uint4 clamp(uint4 v, uint4 a, uint4 b)
+{
+ return make_uint4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// dot product
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float dot(float2 a, float2 b)
+{
+ return a.x * b.x + a.y * b.y;
+}
+inline __host__ __device__ float dot(float3 a, float3 b)
+{
+ return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+inline __host__ __device__ float dot(float4 a, float4 b)
+{
+ return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+inline __host__ __device__ int dot(int2 a, int2 b)
+{
+ return a.x * b.x + a.y * b.y;
+}
+inline __host__ __device__ int dot(int3 a, int3 b)
+{
+ return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+inline __host__ __device__ int dot(int4 a, int4 b)
+{
+ return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+inline __host__ __device__ uint dot(uint2 a, uint2 b)
+{
+ return a.x * b.x + a.y * b.y;
+}
+inline __host__ __device__ uint dot(uint3 a, uint3 b)
+{
+ return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+inline __host__ __device__ uint dot(uint4 a, uint4 b)
+{
+ return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// length
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float length(float2 v)
+{
+ return sqrtf(dot(v, v));
+}
+inline __host__ __device__ float length(float3 v)
+{
+ return sqrtf(dot(v, v));
+}
+inline __host__ __device__ float length(float4 v)
+{
+ return sqrtf(dot(v, v));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// normalize
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 normalize(float2 v)
+{
+ float invLen = rsqrtf(dot(v, v));
+ return v * invLen;
+}
+inline __host__ __device__ float3 normalize(float3 v)
+{
+ float invLen = rsqrtf(dot(v, v));
+ return v * invLen;
+}
+inline __host__ __device__ float4 normalize(float4 v)
+{
+ float invLen = rsqrtf(dot(v, v));
+ return v * invLen;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// floor
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 floorf(float2 v)
+{
+ return make_float2(floorf(v.x), floorf(v.y));
+}
+inline __host__ __device__ float3 floorf(float3 v)
+{
+ return make_float3(floorf(v.x), floorf(v.y), floorf(v.z));
+}
+inline __host__ __device__ float4 floorf(float4 v)
+{
+ return make_float4(floorf(v.x), floorf(v.y), floorf(v.z), floorf(v.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// frac - returns the fractional portion of a scalar or each vector component
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float fracf(float v)
+{
+ return v - floorf(v);
+}
+inline __host__ __device__ float2 fracf(float2 v)
+{
+ return make_float2(fracf(v.x), fracf(v.y));
+}
+inline __host__ __device__ float3 fracf(float3 v)
+{
+ return make_float3(fracf(v.x), fracf(v.y), fracf(v.z));
+}
+inline __host__ __device__ float4 fracf(float4 v)
+{
+ return make_float4(fracf(v.x), fracf(v.y), fracf(v.z), fracf(v.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// fmod
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 fmodf(float2 a, float2 b)
+{
+ return make_float2(fmodf(a.x, b.x), fmodf(a.y, b.y));
+}
+inline __host__ __device__ float3 fmodf(float3 a, float3 b)
+{
+ return make_float3(fmodf(a.x, b.x), fmodf(a.y, b.y), fmodf(a.z, b.z));
+}
+inline __host__ __device__ float4 fmodf(float4 a, float4 b)
+{
+ return make_float4(fmodf(a.x, b.x), fmodf(a.y, b.y), fmodf(a.z, b.z), fmodf(a.w, b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// absolute value
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 fabs(float2 v)
+{
+ return make_float2(fabs(v.x), fabs(v.y));
+}
+inline __host__ __device__ float3 fabs(float3 v)
+{
+ return make_float3(fabs(v.x), fabs(v.y), fabs(v.z));
+}
+inline __host__ __device__ float4 fabs(float4 v)
+{
+ return make_float4(fabs(v.x), fabs(v.y), fabs(v.z), fabs(v.w));
+}
+
+inline __host__ __device__ int2 abs(int2 v)
+{
+ return make_int2(abs(v.x), abs(v.y));
+}
+inline __host__ __device__ int3 abs(int3 v)
+{
+ return make_int3(abs(v.x), abs(v.y), abs(v.z));
+}
+inline __host__ __device__ int4 abs(int4 v)
+{
+ return make_int4(abs(v.x), abs(v.y), abs(v.z), abs(v.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// reflect
+// - returns reflection of incident ray I around surface normal N
+// - N should be normalized, reflected vector's length is equal to length of I
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float3 reflect(float3 i, float3 n)
+{
+ return i - 2.0f * n * dot(n,i);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// cross product
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float3 cross(float3 a, float3 b)
+{
+ return make_float3(a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// smoothstep
+// - returns 0 if x < a
+// - returns 1 if x > b
+// - otherwise returns smooth interpolation between 0 and 1 based on x
+////////////////////////////////////////////////////////////////////////////////
+
+inline __device__ __host__ float smoothstep(float a, float b, float x)
+{
+ float y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+ return (y*y*(3.0f - (2.0f*y)));
+}
+inline __device__ __host__ float2 smoothstep(float2 a, float2 b, float2 x)
+{
+ float2 y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+ return (y*y*(make_float2(3.0f) - (make_float2(2.0f)*y)));
+}
+inline __device__ __host__ float3 smoothstep(float3 a, float3 b, float3 x)
+{
+ float3 y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+ return (y*y*(make_float3(3.0f) - (make_float3(2.0f)*y)));
+}
+inline __device__ __host__ float4 smoothstep(float4 a, float4 b, float4 x)
+{
+ float4 y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+ return (y*y*(make_float4(3.0f) - (make_float4(2.0f)*y)));
+}
+
+#endif
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_GPU_DEVICE_EMULATION_HPP_
+#define PCL_GPU_DEVICE_EMULATION_HPP_
+
+#include <pcl/gpu/device/warp_reduce.hpp>
+
+namespace pcl
+{
+ namespace device
+ {
+ struct Emulation
+ {
+ static __forceinline__ __device__ int ballot(int predicate, volatile int* cta_buffer)
+ {
+#if __CUDA_ARCH__ >= 200
+ (void)cta_buffer;
+ return __ballot(predicate);
+#else
+ int tid = threadIdx.x;
+ cta_buffer[tid] = predicate ? (1 << (tid & 31)) : 0;
+ return warp_reduce(cta_buffer);
+#endif
+ }
+ };
+ }
+}
+
+#endif /* PCL_GPU_DEVICE_EMULATION_HPP_ */
\ No newline at end of file
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_DEVICE_FUNCATTRIB_HPP_
+#define PCL_DEVICE_FUNCATTRIB_HPP_
+
+#include<cstdio>
+
+namespace pcl
+{
+ namespace device
+ {
+ template<class Func>
+ void printFuncAttrib(Func& func)
+ {
+
+ cudaFuncAttributes attrs;
+ cudaFuncGetAttributes(&attrs, func);
+
+ printf("=== Function stats ===\n");
+ printf("Name: \n");
+ printf("sharedSizeBytes = %d\n", (int)attrs.sharedSizeBytes);
+ printf("constSizeBytes = %d\n", (int)attrs.constSizeBytes);
+ printf("localSizeBytes = %d\n", (int)attrs.localSizeBytes);
+ printf("maxThreadsPerBlock = %d\n", attrs.maxThreadsPerBlock);
+ printf("numRegs = %d\n", attrs.numRegs);
+ printf("ptxVersion = %d\n", attrs.ptxVersion);
+ printf("binaryVersion = %d\n", attrs.binaryVersion);
+ printf("\n");
+ fflush(stdout);
+ }
+ }
+
+}
+
+#endif /* PCL_DEVICE_FUNCATTRIB_HPP_ */
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+
+#ifndef PCL_DEVICE_FUNCTIONAL_HPP_
+#define PCL_DEVICE_FUNCTIONAL_HPP_
+
+#include <thrust/functional.h>
+
+namespace pcl
+{
+ namespace device
+ {
+ // Function Objects
+
+ using thrust::unary_function;
+ using thrust::binary_function;
+
+ // Arithmetic Operations
+
+ using thrust::plus;
+ using thrust::minus;
+ using thrust::multiplies;
+ using thrust::divides;
+ using thrust::modulus;
+ using thrust::negate;
+
+ // Comparison Operations
+
+ using thrust::equal_to;
+ using thrust::not_equal_to;
+ using thrust::greater;
+ using thrust::less;
+ using thrust::greater_equal;
+ using thrust::less_equal;
+
+ // Logical Operations
+
+ using thrust::logical_and;
+ using thrust::logical_or;
+ using thrust::logical_not;
+
+ // Bitwise Operations
+
+ using thrust::bit_and;
+ using thrust::bit_or;
+ using thrust::bit_xor;
+
+ template <typename T> struct bit_not : unary_function<T, T>
+ {
+ __forceinline__ __device__ T operator ()(const T& v) const {return ~v;}
+ };
+
+ // Generalized Identity Operations
+
+ using thrust::identity;
+ using thrust::project1st;
+ using thrust::project2nd;
+
+
+ // Other functors
+
+ template<typename T, typename W>
+ struct plusWeighted : public plus<T>
+ {
+ W w;
+ __device__ __host__ __forceinline__ plusWeighted(W weight) : w(weight) {}
+ __device__ __host__ __forceinline__ float operator()(const T& f1, const T& f2) const
+ {
+ return f1 + f2 * w;
+ }
+ };
+ }
+};
+
+
+#endif /* PCL_DEVICE_FUNCTIONAL_HPP_ */
\ No newline at end of file
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_DEVICE_NUMERIC_LIMITS_HPP_
+#define PCL_DEVICE_NUMERIC_LIMITS_HPP_
+
+namespace pcl
+{
+ namespace device
+ {
+ template<class T> struct numeric_limits
+ {
+ typedef T type;
+ __device__ __forceinline__ static type min() { return type(); };
+ __device__ __forceinline__ static type max() { return type(); };
+ __device__ __forceinline__ static type epsilon() { return type(); }
+ __device__ __forceinline__ static type round_error() { return type(); }
+ __device__ __forceinline__ static type denorm_min() { return type(); }
+ __device__ __forceinline__ static type infinity() { return type(); }
+ __device__ __forceinline__ static type quiet_NaN() { return type(); }
+ __device__ __forceinline__ static type signaling_NaN() { return T(); }
+ static const bool is_signed;
+ };
+
+ template<> struct numeric_limits<bool>
+ {
+ typedef bool type;
+ __device__ __forceinline__ static type min() { return false; };
+ __device__ __forceinline__ static type max() { return true; };
+ __device__ __forceinline__ static type epsilon();
+ __device__ __forceinline__ static type round_error();
+ __device__ __forceinline__ static type denorm_min();
+ __device__ __forceinline__ static type infinity();
+ __device__ __forceinline__ static type quiet_NaN();
+ __device__ __forceinline__ static type signaling_NaN();
+ static const bool is_signed = false;
+ };
+
+ template<> struct numeric_limits<char>
+ {
+ typedef char type;
+ __device__ __forceinline__ static type min() { return CHAR_MIN; };
+ __device__ __forceinline__ static type max() { return CHAR_MAX; };
+ __device__ __forceinline__ static type epsilon();
+ __device__ __forceinline__ static type round_error();
+ __device__ __forceinline__ static type denorm_min();
+ __device__ __forceinline__ static type infinity();
+ __device__ __forceinline__ static type quiet_NaN();
+ __device__ __forceinline__ static type signaling_NaN();
+ static const bool is_signed = (char)-1 == -1;
+ };
+
+ template<> struct numeric_limits<signed char>
+ {
+ typedef char type;
+ __device__ __forceinline__ static type min() { return CHAR_MIN; };
+ __device__ __forceinline__ static type max() { return CHAR_MAX; };
+ __device__ __forceinline__ static type epsilon();
+ __device__ __forceinline__ static type round_error();
+ __device__ __forceinline__ static type denorm_min();
+ __device__ __forceinline__ static type infinity();
+ __device__ __forceinline__ static type quiet_NaN();
+ __device__ __forceinline__ static type signaling_NaN();
+ static const bool is_signed = (signed char)-1 == -1;
+ };
+
+ template<> struct numeric_limits<unsigned char>
+ {
+ typedef unsigned char type;
+ __device__ __forceinline__ static type min() { return 0; };
+ __device__ __forceinline__ static type max() { return UCHAR_MAX; };
+ __device__ __forceinline__ static type epsilon();
+ __device__ __forceinline__ static type round_error();
+ __device__ __forceinline__ static type denorm_min();
+ __device__ __forceinline__ static type infinity();
+ __device__ __forceinline__ static type quiet_NaN();
+ __device__ __forceinline__ static type signaling_NaN();
+ static const bool is_signed = false;
+ };
+
+ template<> struct numeric_limits<short>
+ {
+ typedef short type;
+ __device__ __forceinline__ static type min() { return SHRT_MIN; };
+ __device__ __forceinline__ static type max() { return SHRT_MAX; };
+ __device__ __forceinline__ static type epsilon();
+ __device__ __forceinline__ static type round_error();
+ __device__ __forceinline__ static type denorm_min();
+ __device__ __forceinline__ static type infinity();
+ __device__ __forceinline__ static type quiet_NaN();
+ __device__ __forceinline__ static type signaling_NaN();
+ static const bool is_signed = true;
+ };
+
+ template<> struct numeric_limits<unsigned short>
+ {
+ typedef unsigned short type;
+ __device__ __forceinline__ static type min() { return 0; };
+ __device__ __forceinline__ static type max() { return USHRT_MAX; };
+ __device__ __forceinline__ static type epsilon();
+ __device__ __forceinline__ static type round_error();
+ __device__ __forceinline__ static type denorm_min();
+ __device__ __forceinline__ static type infinity();
+ __device__ __forceinline__ static type quiet_NaN();
+ __device__ __forceinline__ static type signaling_NaN();
+ static const bool is_signed = false;
+ };
+
+ template<> struct numeric_limits<int>
+ {
+ typedef int type;
+ __device__ __forceinline__ static type min() { return INT_MIN; };
+ __device__ __forceinline__ static type max() { return INT_MAX; };
+ __device__ __forceinline__ static type epsilon();
+ __device__ __forceinline__ static type round_error();
+ __device__ __forceinline__ static type denorm_min();
+ __device__ __forceinline__ static type infinity();
+ __device__ __forceinline__ static type quiet_NaN();
+ __device__ __forceinline__ static type signaling_NaN();
+ static const bool is_signed = true;
+ };
+
+
+ template<> struct numeric_limits<unsigned int>
+ {
+ typedef unsigned int type;
+ __device__ __forceinline__ static type min() { return 0; };
+ __device__ __forceinline__ static type max() { return UINT_MAX; };
+ __device__ __forceinline__ static type epsilon();
+ __device__ __forceinline__ static type round_error();
+ __device__ __forceinline__ static type denorm_min();
+ __device__ __forceinline__ static type infinity();
+ __device__ __forceinline__ static type quiet_NaN();
+ __device__ __forceinline__ static type signaling_NaN();
+ static const bool is_signed = false;
+ };
+
+ template<> struct numeric_limits<long>
+ {
+ typedef long type;
+ __device__ __forceinline__ static type min() { return LONG_MIN; };
+ __device__ __forceinline__ static type max() { return LONG_MAX; };
+ __device__ __forceinline__ static type epsilon();
+ __device__ __forceinline__ static type round_error();
+ __device__ __forceinline__ static type denorm_min();
+ __device__ __forceinline__ static type infinity();
+ __device__ __forceinline__ static type quiet_NaN();
+ __device__ __forceinline__ static type signaling_NaN();
+ static const bool is_signed = true;
+ };
+
+ template<> struct numeric_limits<unsigned long>
+ {
+ typedef unsigned long type;
+ __device__ __forceinline__ static type min() { return 0; };
+ __device__ __forceinline__ static type max() { return ULONG_MAX; };
+ __device__ __forceinline__ static type epsilon();
+ __device__ __forceinline__ static type round_error();
+ __device__ __forceinline__ static type denorm_min();
+ __device__ __forceinline__ static type infinity();
+ __device__ __forceinline__ static type quiet_NaN();
+ __device__ __forceinline__ static type signaling_NaN();
+ static const bool is_signed = false;
+ };
+
+ template<> struct numeric_limits<float>
+ {
+ typedef float type;
+ __device__ __forceinline__ static type min() { return 1.175494351e-38f/*FLT_MIN*/; };
+ __device__ __forceinline__ static type max() { return 3.402823466e+38f/*FLT_MAX*/; };
+ __device__ __forceinline__ static type epsilon() { return 1.192092896e-07f/*FLT_EPSILON*/; };
+ __device__ __forceinline__ static type round_error();
+ __device__ __forceinline__ static type denorm_min();
+ __device__ __forceinline__ static type infinity() { return __int_as_float(0x7f800000); /*CUDART_INF_F*/ };
+ __device__ __forceinline__ static type quiet_NaN() { return __int_as_float(0x7fffffff); /*CUDART_NAN_F*/ };
+ __device__ __forceinline__ static type signaling_NaN();
+ static const bool is_signed = true;
+ };
+
+ template<> struct numeric_limits<double>
+ {
+ typedef double type;
+ __device__ __forceinline__ static type min() { return 2.2250738585072014e-308/*DBL_MIN*/; };
+ __device__ __forceinline__ static type max() { return 1.7976931348623158e+308/*DBL_MAX*/; };
+ __device__ __forceinline__ static type epsilon() { return 2.2204460492503131e-016 /*DBL_EPSILON*/; };
+ __device__ __forceinline__ static type round_error();
+ __device__ __forceinline__ static type denorm_min();
+ __device__ __forceinline__ static type infinity();
+ __device__ __forceinline__ static type quiet_NaN();
+ __device__ __forceinline__ static type signaling_NaN();
+ static const bool is_signed = true;
+ };
+ }
+}
+#endif /* PCL_DEVICE_NUMERIC_LIMITS_HPP_ */
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_GPU_DEVUCE_REDUCE_HPP_
+#define PCL_GPU_DEVUCE_REDUCE_HPP_
+
+namespace pcl
+{
+ namespace device
+ {
+ template <unsigned int CTA_SIZE, typename T, typename BinaryFunction>
+ __device__ __forceinline__ void reduce_block(volatile T* data, BinaryFunction op, unsigned int tid = threadIdx.x)
+ {
+ T val = data[tid];
+
+ //if (CTA_SIZE >= 1024) { if (tid < 512) { data[tid] = val = op(val, data[tdi + 512]); } __syncthreads(); }
+ if (CTA_SIZE >= 512) { if (tid < 256) { data[tid] = val = op(val, data[tid + 256]); } __syncthreads(); }
+ if (CTA_SIZE >= 256) { if (tid < 128) { data[tid] = val = op(val, data[tid + 128]); } __syncthreads(); }
+ if (CTA_SIZE >= 128) { if (tid < 64) { data[tid] = val = op(val, data[tid + 64]); } __syncthreads(); }
+
+ if (tid < 32)
+ {
+ if (CTA_SIZE >= 64) data[tid] = val = op(val, data[tid + 32]);
+ if (CTA_SIZE >= 32) data[tid] = val = op(val, data[tid + 16]);
+ if (CTA_SIZE >= 16) data[tid] = val = op(val, data[tid + 8]);
+ if (CTA_SIZE >= 8) data[tid] = val = op(val, data[tid + 4]);
+ if (CTA_SIZE >= 4) data[tid] = val = op(val, data[tid + 2]);
+ if (CTA_SIZE >= 2) data[tid] = val = op(val, data[tid + 1]);
+ }
+ };
+ }
+}
+
+
+#endif
\ No newline at end of file
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_GPU_DEVICE_STATIC_CHECK_HPP_
+#define PCL_GPU_DEVICE_STATIC_CHECK_HPP_
+
+#if defined(__CUDACC__)
+ #define __PCL_GPU_HOST_DEVICE__ __host__ __device__ __forceinline__
+#else
+ #define __PCL_GPU_HOST_DEVICE__
+#endif
+
+namespace pcl
+{
+ namespace device
+ {
+ template<bool expr> struct Static {};
+
+ template<> struct Static<true>
+ {
+ __PCL_GPU_HOST_DEVICE__ static void check() {};
+ };
+ }
+
+ namespace gpu
+ {
+ using pcl::device::Static;
+ }
+}
+
+#undef __PCL_GPU_HOST_DEVICE__
+
+#endif /* PCL_GPU_DEVICE_STATIC_CHECK_HPP_ */
\ No newline at end of file
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_GPU_UTILS_DEVICE_VECTOR_MATH_HPP_
+#define PCL_GPU_UTILS_DEVICE_VECTOR_MATH_HPP_
+
+namespace pcl
+{
+ namespace device
+ {
+ ////////////////////////////////
+ // one element vectors
+
+
+ ////////////////////////////////
+ // two element vectors
+
+
+ ////////////////////////////////
+ // three element vectors
+
+#define PCL_GPU_IMPLEMENT_COMPOUND_VEC3_OP(type, scalar, op) \
+ __device__ __host__ __forceinline__ type & operator op (type & v1, const type & v2) { v1.x op v2.x; v1.y op v2.y; v1.z op v2.z; return v1; } \
+ __device__ __host__ __forceinline__ type & operator op (type & v, scalar val) { v.x op val; v.y op val; v.z op val; return v; }
+
+ PCL_GPU_IMPLEMENT_COMPOUND_VEC3_OP(float3, float, -=)
+ PCL_GPU_IMPLEMENT_COMPOUND_VEC3_OP(float3, float, +=)
+ PCL_GPU_IMPLEMENT_COMPOUND_VEC3_OP(float3, float, *=)
+
+ PCL_GPU_IMPLEMENT_COMPOUND_VEC3_OP(short3, short, -=)
+
+ PCL_GPU_IMPLEMENT_COMPOUND_VEC3_OP(int3, int, +=)
+
+#undef PCL_GPU_IMPLEMENT_COMPOUND_VEC3_OP
+
+ __device__ __host__ __forceinline__ float dot(const float3& v1, const float3& v2)
+ {
+ return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
+ }
+
+ __device__ __host__ __forceinline__ float3 cross(const float3& v1, const float3& v2)
+ {
+ return make_float3(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x);
+ }
+
+ ////////////////////////////////
+ // four element vectors
+
+ __device__ __host__ __forceinline__ float dot(const float4& v1, const float4& v2)
+ {
+ return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z + v1.w * v2.w;
+ }
+
+ ////////////////////////////////
+ // alltype binary operarators
+
+#define PCL_GPU_IMPLEMENT_VEC_BINOP(type, scalar, op, cop) \
+ __device__ __host__ __forceinline__ type operator op (const type & v1, const type & v2) { type r = v1; r cop v2; return r; } \
+ __device__ __host__ __forceinline__ type operator op (const type & v1, scalar c) { type r = v1; r cop c; return r; }
+
+ PCL_GPU_IMPLEMENT_VEC_BINOP(float3, float, -, -=)
+ PCL_GPU_IMPLEMENT_VEC_BINOP(float3, float, +, +=)
+ PCL_GPU_IMPLEMENT_VEC_BINOP(float3, float, *, *=)
+
+ PCL_GPU_IMPLEMENT_VEC_BINOP(short3, short, -, -=)
+
+ PCL_GPU_IMPLEMENT_VEC_BINOP(int3, int, +, +=)
+
+#undef PCL_GPU_IMPLEMENT_VEC_BINOP
+
+
+ ////////////////////////////////
+ // tempalted operations vectors
+
+ template<typename T> __device__ __host__ __forceinline__ float norm(const T& val)
+ {
+ return sqrtf(dot(val, val));
+ }
+
+ template<typename T> __host__ __device__ __forceinline__ float inverse_norm(const T& v)
+ {
+ return rsqrtf(dot(v, v));
+ }
+
+ template<typename T> __host__ __device__ __forceinline__ T normalized(const T& v)
+ {
+ return v * inverse_norm(v);
+ }
+
+ template<typename T> __host__ __device__ __forceinline__ T normalized_safe(const T& v)
+ {
+ return (dot(v, v) > 0) ? (v * rsqrtf(dot(v, v))) : v;
+ }
+ }
+}
+
+#endif /* PCL_GPU_UTILS_DEVICE_VECTOR_MATH_HPP_ */
+
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2011, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of Willow Garage, Inc. nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+*/
+
+#ifndef PCL_DEVICE_UTILS_WARP_HPP_
+#define PCL_DEVICE_UTILS_WARP_HPP_
+
+namespace pcl
+{
+ namespace device
+ {
+ struct Warp
+ {
+ enum
+ {
+ LOG_WARP_SIZE = 5,
+ WARP_SIZE = 1 << LOG_WARP_SIZE,
+ STRIDE = WARP_SIZE
+ };
+
+ /** \brief Returns the warp lane ID of the calling thread. */
+ static __device__ __forceinline__ unsigned int laneId()
+ {
+ unsigned int ret;
+ asm("mov.u32 %0, %laneid;" : "=r"(ret) );
+ return ret;
+ }
+
+ static __device__ __forceinline__ int laneMaskLe()
+ {
+#if (__CUDA_ARCH__ >= 200)
+ unsigned int ret;
+ asm("mov.u32 %0, %lanemask_le;" : "=r"(ret) );
+ return ret;
+#else
+ return 0xFFFFFFFF >> (31 - laneId());
+#endif
+ }
+
+ static __device__ __forceinline__ int laneMaskLt()
+ {
+#if (__CUDA_ARCH__ >= 200)
+ unsigned int ret;
+ asm("mov.u32 %0, %lanemask_lt;" : "=r"(ret) );
+ return ret;
+#else
+ return 0xFFFFFFFF >> (32 - laneId());
+#endif
+ }
+ static __device__ __forceinline__ unsigned int id()
+ {
+ int tid = threadIdx.z * blockDim.x * blockDim.y + threadIdx.y * blockDim.x + threadIdx.x;
+ return tid >> LOG_WARP_SIZE;
+ }
+
+ static __device__ __forceinline__ int binaryInclScan(int ballot_mask)
+ {
+ return __popc(Warp::laneMaskLe() & ballot_mask);
+ }
+
+ static __device__ __forceinline__ int binaryExclScan(int ballot_mask)
+ {
+ return __popc(Warp::laneMaskLt() & ballot_mask);
+ }
+
+ template<typename It, typename T>
+ static __device__ __forceinline__ void fill(It beg, It end, const T& value)
+ {
+ for(It t = beg + laneId(); t < end; t += STRIDE)
+ *t = value;
+ }
+
+ template<typename InIt, typename OutIt>
+ static __device__ __forceinline__ OutIt copy(InIt beg, InIt end, OutIt out)
+ {
+ unsigned int lane = laneId();
+ InIt t = beg + lane;
+ OutIt o = out + lane;
+
+ for(; t < end; t += STRIDE, o += STRIDE)
+ *o = *t;
+ return o;
+ }
+
+ template<typename InIt, typename OutIt, class UnOp>
+ static __device__ __forceinline__ OutIt transform(InIt beg, InIt end, OutIt out, UnOp op)
+ {
+ unsigned int lane = laneId();
+ InIt t = beg + lane;
+ OutIt o = out + lane;
+
+ for(InIt t = beg + laneId(); t < end; t += STRIDE, o += STRIDE)
+ *o = op(*t);
+ return o;
+ }
+
+ template<typename InIt1, typename InIt2, typename OutIt, class BinOp>
+ static __device__ __forceinline__ OutIt transform(InIt1 beg1, InIt1 end1, InIt2 beg2, OutIt out, BinOp op)
+ {
+ unsigned int lane = laneId();
+ InIt1 t1 = beg1 + lane;
+ InIt2 t2 = beg2 + lane;
+ OutIt o = out + lane;
+
+ for(; t1 < end1; t1 += STRIDE, t2 += STRIDE, o += STRIDE)
+ *o = op(*t1, *t2);
+ return o;
+ }
+
+ template<typename OutIt, typename T>
+ static __device__ __forceinline__ void yota(OutIt beg, OutIt end, T value)
+ {
+ unsigned int lane = laneId();
+ value += lane;
+
+ for(OutIt t = beg + lane; t < end; t += STRIDE, value += STRIDE)
+ *t = value;
+ }
+
+ template<typename T, class BinOp>
+ static __device__ __forceinline__ void reduce(volatile T* buffer, BinOp op)
+ {
+ unsigned int lane = laneId();
+ T val = buffer[lane];
+
+ if (lane < 16)
+ {
+ buffer[lane] = val = op(val, buffer[lane + 16]);
+ buffer[lane] = val = op(val, buffer[lane + 8]);
+ buffer[lane] = val = op(val, buffer[lane + 4]);
+ buffer[lane] = val = op(val, buffer[lane + 2]);
+ buffer[lane] = val = op(val, buffer[lane + 1]);
+ }
+ }
+
+ template<typename T, class BinOp>
+ static __device__ __forceinline__ T reduce(volatile T* buffer, T init, BinOp op)
+ {
+ unsigned int lane = laneId();
+ T val = buffer[lane] = init;
+
+ if (lane < 16)
+ {
+ buffer[lane] = val = op(val, buffer[lane + 16]);
+ buffer[lane] = val = op(val, buffer[lane + 8]);
+ buffer[lane] = val = op(val, buffer[lane + 4]);
+ buffer[lane] = val = op(val, buffer[lane + 2]);
+ buffer[lane] = val = op(val, buffer[lane + 1]);
+ }
+ return buffer[0];
+ }
+ };
+ }
+}
+
+#endif /* PCL_DEVICE_UTILS_WARP_HPP_ */
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#ifndef __PCL_GPU_UTILS_REPACKS_HPP__
+#define __PCL_GPU_UTILS_REPACKS_HPP__
+
+#include <pcl/pcl_macros.h>
+#include <pcl/point_types.h>
+
+#include <pcl/gpu/containers/device_array.h>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ ///////////////////////////////////////
+ /// This is an experimental code ///
+ ///////////////////////////////////////
+
+ const int NoCP = 0xFFFFFFFF;
+
+ /** \brief Returns field copy operation code. */
+ inline int cp(int from, int to)
+ {
+ return ((to & 0xF) << 4) + (from & 0xF);
+ }
+
+ /* Combines several field copy operations to one int (called rule) */
+ inline int rule(int cp1, int cp2 = NoCP, int cp3 = NoCP, int cp4 = NoCP)
+ {
+ return (cp1 & 0xFF) + ((cp2 & 0xFF) << 8) + ((cp3 & 0xFF) << 16) + ((cp4 & 0xFF) << 24);
+ }
+
+ /* Combines performs all field copy operations in given rule array (can be 0, 1, or 16 copies) */
+ void copyFieldsImpl(int in_size, int out_size, int rules[4], int size, const void* input, void* output);
+
+ template<typename PointIn, typename PointOut>
+ void copyFieldsEx(const DeviceArray<PointIn>& src, DeviceArray<PointOut>& dst, int rule1, int rule2 = NoCP, int rule3 = NoCP, int rule4 = NoCP)
+ {
+ int rules[4] = { rule1, rule2, rule3, rule4 };
+ dst.create(src.size());
+ copyFieldsImpl(sizeof(PointIn)/sizeof(int), sizeof(PointOut)/sizeof(int), rules, (int)src.size(), src.ptr(), dst.ptr());
+ }
+
+ void copyFields(const DeviceArray<PointXYZ>& src, DeviceArray<PointNormal>& dst)
+ {
+ //PointXYZ.x (0) -> PointNormal.x (0)
+ //PointXYZ.y (1) -> PointNormal.y (1)
+ //PointXYZ.z (2) -> PointNormal.z (2)
+ copyFieldsEx(src, dst, rule(cp(0, 0), cp(1, 1), cp(2, 2)));
+ };
+
+ void copyFields(const DeviceArray<Normal>& src, DeviceArray<PointNormal>& dst)
+ {
+ //PointXYZ.normal_x (0) -> PointNormal.normal_x (4)
+ //PointXYZ.normal_y (1) -> PointNormal.normal_y (5)
+ //PointXYZ.normal_z (2) -> PointNormal.normal_z (6)
+ //PointXYZ.curvature (4) -> PointNormal.curvature (8)
+ copyFieldsEx(src, dst, rule(cp(0, 4), cp(1, 5), cp(2, 6), cp(4,8)));
+ };
+
+ void copyFields(const DeviceArray<PointXYZRGBL>& src, DeviceArray<PointXYZ>& dst)
+ {
+ //PointXYZRGBL.x (0) -> PointXYZ.x (0)
+ //PointXYZRGBL.y (1) -> PointXYZ.y (1)
+ //PointXYZRGBL.z (2) -> PointXYZ.z (2)
+ copyFieldsEx(src, dst, rule(cp(0, 0), cp(1, 1), cp(2, 2)));
+ };
+
+ void copyFields(const DeviceArray<PointXYZRGB>& src, DeviceArray<PointXYZ>& dst)
+ {
+ //PointXYZRGB.x (0) -> PointXYZ.x (0)
+ //PointXYZRGB.y (1) -> PointXYZ.y (1)
+ //PointXYZRGB.z (2) -> PointXYZ.z (2)
+ copyFieldsEx(src, dst, rule(cp(0, 0), cp(1, 1), cp(2, 2)));
+ };
+
+ void copyFields(const DeviceArray<PointXYZRGBA>& src, DeviceArray<PointXYZ>& dst)
+ {
+ //PointXYZRGBA.x (0) -> PointXYZ.x (0)
+ //PointXYZRGBA.y (1) -> PointXYZ.y (1)
+ //PointXYZRGBA.z (2) -> PointXYZ.z (2)
+ copyFieldsEx(src, dst, rule(cp(0, 0), cp(1, 1), cp(2, 2)));
+ };
+
+ void copyFieldsZ(const DeviceArray<PointXYZ>& src, DeviceArray<float>& dst)
+ {
+ //PointXYZRGBL.z (2) -> float (1)
+ copyFieldsEx(src, dst, rule(cp(2, 0)));
+ };
+
+ void copyFieldsZ(const DeviceArray<PointXYZRGB>& src, DeviceArray<float>& dst)
+ {
+ //PointXYZRGBL.z (2) -> float (1)
+ copyFieldsEx(src, dst, rule(cp(2, 0)));
+ };
+ }
+}
+
+#endif /* __PCL_GPU_UTILS_REPACKS_HPP__ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#ifndef __PCL_CUDA_SAFE_CALL_HPP__
+#define __PCL_CUDA_SAFE_CALL_HPP__
+
+#include "cuda_runtime_api.h"
+#include <pcl/gpu/containers/initialization.h>
+
+#if defined(__GNUC__)
+ #define cudaSafeCall(expr) pcl::gpu::___cudaSafeCall(expr, __FILE__, __LINE__, __func__)
+#else /* defined(__CUDACC__) || defined(__MSVC__) */
+ #define cudaSafeCall(expr) pcl::gpu::___cudaSafeCall(expr, __FILE__, __LINE__)
+#endif
+
+namespace pcl
+{
+ namespace gpu
+ {
+ static inline void ___cudaSafeCall(cudaError_t err, const char *file, const int line, const char *func = "")
+ {
+ if (cudaSuccess != err)
+ error(cudaGetErrorString(err), file, line, func);
+ }
+
+ static inline int divUp(int total, int grain) { return (total + grain - 1) / grain; }
+ }
+
+ namespace device
+ {
+ using pcl::gpu::divUp;
+ }
+}
+
+
+#endif /* __PCL_CUDA_SAFE_CALL_HPP__ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#ifndef PCL_GPU_UTILS_TEXTURE_BINDER_HPP_
+#define PCL_GPU_UTILS_TEXTURE_BINDER_HPP_
+
+#include <pcl/gpu/utils/safe_call.hpp>
+#include <pcl/gpu/containers/device_array.h>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ class TextureBinder
+ {
+ public:
+ template<class T, enum cudaTextureReadMode readMode>
+ TextureBinder(const DeviceArray2D<T>& arr, const struct texture<T, 2, readMode>& tex) : texref(&tex)
+ {
+ cudaChannelFormatDesc desc = cudaCreateChannelDesc<T>();
+ cudaSafeCall( cudaBindTexture2D(0, tex, arr.ptr(), desc, arr.cols(), arr.rows(), arr.step()) );
+ }
+
+ template<class T, enum cudaTextureReadMode readMode>
+ TextureBinder(const DeviceArray<T>& arr, const struct texture<T, 1, readMode> &tex) : texref(&tex)
+ {
+ cudaChannelFormatDesc desc = cudaCreateChannelDesc<T>();
+ cudaSafeCall( cudaBindTexture(0, tex, arr.ptr(), desc, arr.sizeBytes()) );
+ }
+
+ template<class T, enum cudaTextureReadMode readMode>
+ TextureBinder(const PtrStepSz<T>& arr, const struct texture<T, 2, readMode>& tex) : texref(&tex)
+ {
+ cudaChannelFormatDesc desc = cudaCreateChannelDesc<T>();
+ cudaSafeCall( cudaBindTexture2D(0, tex, arr.data, desc, arr.cols, arr.rows, arr.step) );
+ }
+
+ template<class T, enum cudaTextureReadMode readMode>
+ TextureBinder(const PtrSz<T>& arr, const struct texture<T, 1, readMode> &tex) : texref(&tex)
+ {
+ cudaChannelFormatDesc desc = cudaCreateChannelDesc<T>();
+ cudaSafeCall( cudaBindTexture(0, tex, arr.data, desc, arr.size * arr.elemSize()) );
+ }
+
+ ~TextureBinder()
+ {
+ cudaSafeCall( cudaUnbindTexture(texref) );
+ }
+ private:
+ const struct textureReference *texref;
+ };
+ }
+
+ namespace device
+ {
+ using pcl::gpu::TextureBinder;
+ }
+}
+
+#endif /* PCL_GPU_UTILS_TEXTURE_BINDER_HPP_*/
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#ifndef _PCL_CUDA_TIMERS_HPP_
+#define _PCL_CUDA_TIMERS_HPP_
+
+#include <cuda_runtime_api.h>
+#include <cstdio>
+
+namespace pcl
+{
+ namespace gpu
+ {
+ struct Timer
+ {
+ cudaEvent_t start_, stop_;
+ Timer(bool runTimer = false)
+ {
+ cudaEventCreate(&start_);
+ cudaEventCreate(&stop_);
+ if (runTimer)
+ start();
+ }
+ ~Timer()
+ {
+ cudaEventDestroy(start_);
+ cudaEventDestroy(stop_);
+ }
+
+ void start() { cudaEventRecord(start_, 0); }
+ Timer& stop() { cudaEventRecord(stop_, 0); cudaEventSynchronize(stop_); return *this; }
+
+ float time()
+ {
+ float elapsed_time;
+ cudaEventElapsedTime(&elapsed_time, start_, stop_);
+ return elapsed_time;
+ }
+ };
+
+ struct ScopeTimer
+ {
+ const char* name;
+ cudaEvent_t start, stop;
+ ScopeTimer(const char* name_) : name(name_)
+ {
+ cudaEventCreate(&start);
+ cudaEventCreate(&stop);
+ cudaEventRecord(start);
+ }
+ ~ScopeTimer()
+ {
+ float elapsed_time;
+ cudaEventRecord(stop);
+ cudaEventSynchronize(stop);
+ cudaEventElapsedTime(&elapsed_time, start, stop);
+ printf("Time(%s) = %fms\n", name, elapsed_time);
+ cudaEventDestroy(start);
+ cudaEventDestroy(stop);
+ }
+ };
+ }
+}
+
+#endif /* _PCL_CUDA_TIMERS_HPP_ */
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#ifndef PCL_GPU_UTILS_INTERNAL_HPP_
+#define PCL_GPU_UTILS_INTERNAL_HPP_
+
+namespace pcl
+{
+ namespace device
+ {
+ void copyFields(int in_size, int out_size, int info[4], int size, const void* input, void* output);
+ }
+}
+
+#endif
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
+ */
+
+#include <assert.h>
+
+#include <pcl/gpu/utils/repacks.hpp>
+#include "internal.hpp"
+
--- /dev/null
+#include "internal.hpp"
+#include <algorithm>
+#include "pcl/gpu/utils/safe_call.hpp"
+#include "pcl/pcl_exports.h"
+
+namespace pcl
+{
+ namespace device
+ {
+ struct Info
+ {
+ enum { SIZE = 4 };
+ int data[SIZE];
+ };
+
+ template<int n>
+ struct Point
+ {
+ int data[n];
+ };
+
+ template<int in, int out, typename Info>
+ __global__ void deviceCopyFields4B(const Info info, const int size, const void* input, void* output)
+ {
+ int idx = blockIdx.x * blockDim.x + threadIdx.x;
+
+ if (idx < size)
+ {
+ Point<in> point_in = reinterpret_cast<const Point<in>* >( input)[idx];
+ Point<out> point_out = reinterpret_cast< Point<out>* >(output)[idx];
+
+ for(int i = 0; i < Info::SIZE; ++i)
+ {
+ int byte;
+ int code = info.data[i];
+
+ byte = ((code >> 0) & 0xFF);
+
+ if (byte == 0xFF)
+ break;
+ else
+ point_out.data[byte >> 4] = point_in.data[byte & 0xF];
+
+ byte = ((code >> 8) & 0xFF);
+
+ if (byte == 0xFF)
+ break;
+ else
+ point_out.data[byte >> 4] = point_in.data[byte & 0xF];
+
+ byte = ((code >> 16) & 0xFF);
+
+ if (byte == 0xFF)
+ break;
+ else
+ point_out.data[byte >> 4] = point_in.data[byte & 0xF];
+
+ byte = ((code >> 24) & 0xFF);
+
+ if (byte == 0xFF)
+ break;
+ else
+ point_out.data[byte >> 4] = point_in.data[byte & 0xF];
+ }
+
+ reinterpret_cast< Point<out>* >(output)[idx] = point_out;
+ }
+ };
+
+ template<int in_size, int out_size>
+ void cf(int info[4], int size, const void* input, void* output)
+ {
+ Info i;
+ std::copy(info, info + 4, i.data);
+
+ dim3 block(256);
+ dim3 grid(divUp(size, block.x));
+
+ deviceCopyFields4B<in_size, out_size><<<grid, block>>>(i, size, input, output);
+ cudaSafeCall ( cudaGetLastError () );
+ cudaSafeCall (cudaDeviceSynchronize ());
+ }
+
+ typedef void (*copy_fields_t)(int info[4], int size, const void* input, void* output);
+ }
+}
+
+namespace pcl
+{
+ namespace gpu
+ {
+ using namespace pcl::device;
+
+ PCL_EXPORTS void copyFieldsImpl(int in_size, int out_size, int rules[4], int size, const void* input, void* output)
+ {
+ pcl::device::copy_fields_t funcs[16][16] =
+ {
+ { /**/ cf<1,1>, cf<1, 2>, cf<1, 3>, cf<1, 4>, /**/ cf<1, 5>, cf<1, 6>, cf<1, 7>, cf<1, 8>, /**/ cf<1, 9>, cf<1,10>, cf<1, 11>, cf<1, 12>, /**/ cf<1, 13>, cf<1, 14>, cf<1, 15>, cf<1,16> },
+ { /**/ cf<2,1>, cf<2, 2>, cf<2, 3>, cf<2, 4>, /**/ cf<2, 5>, cf<2, 6>, cf<2, 7>, cf<2, 8>, /**/ cf<2, 9>, cf<1,10>, cf<2, 11>, cf<2, 12>, /**/ cf<2, 13>, cf<2, 14>, cf<2, 15>, cf<2,16> },
+ { /**/ cf<3,1>, cf<3, 2>, cf<3, 3>, cf<3, 4>, /**/ cf<3, 5>, cf<3, 6>, cf<3, 7>, cf<3, 8>, /**/ cf<3, 9>, cf<1,10>, cf<3, 11>, cf<3, 12>, /**/ cf<3, 13>, cf<3, 14>, cf<3, 15>, cf<3,16> },
+ { /**/ cf<4,1>, cf<4, 2>, cf<4, 3>, cf<4, 4>, /**/ cf<4, 5>, cf<4, 6>, cf<4, 7>, cf<4, 8>, /**/ cf<4, 9>, cf<1,10>, cf<4, 11>, cf<4, 12>, /**/ cf<4, 13>, cf<4, 14>, cf<4, 15>, cf<4,16> },
+ { /**/ cf<5,1>, cf<5, 2>, cf<5, 3>, cf<5, 4>, /**/ cf<5, 5>, cf<5, 6>, cf<5, 7>, cf<5, 8>, /**/ cf<5, 9>, cf<1,10>, cf<5, 11>, cf<5, 12>, /**/ cf<5, 13>, cf<5, 14>, cf<5, 15>, cf<5,16> },
+ { /**/ cf<6,1>, cf<6, 2>, cf<6, 3>, cf<6, 4>, /**/ cf<6, 5>, cf<6, 6>, cf<6, 7>, cf<6, 8>, /**/ cf<6, 9>, cf<1,10>, cf<6, 11>, cf<6, 12>, /**/ cf<6, 13>, cf<6, 14>, cf<6, 15>, cf<6,16> },
+ { /**/ cf<7,1>, cf<7, 2>, cf<7, 3>, cf<7, 4>, /**/ cf<7, 5>, cf<7, 6>, cf<7, 7>, cf<7, 8>, /**/ cf<7, 9>, cf<1,10>, cf<7, 11>, cf<7, 12>, /**/ cf<7, 13>, cf<7, 14>, cf<7, 15>, cf<7,16> },
+ { /**/ cf<8,1>, cf<8, 2>, cf<8, 3>, cf<8, 4>, /**/ cf<8, 5>, cf<8, 6>, cf<8, 7>, cf<8, 8>, /**/ cf<8, 9>, cf<1,10>, cf<8, 11>, cf<8, 12>, /**/ cf<8, 13>, cf<8, 14>, cf<8, 15>, cf<8,16> },
+ { /**/ cf<9,1>, cf<9, 2>, cf<9, 3>, cf<9, 4>, /**/ cf<9, 5>, cf<9, 6>, cf<9, 7>, cf<9, 8>, /**/ cf<9, 9>, cf<1,10>, cf<9, 11>, cf<9, 12>, /**/ cf<9, 13>, cf<9, 14>, cf<9, 15>, cf<9,16> },
+ { /**/ cf<10,1>, cf<10,2>, cf<10,3>, cf<10,4>, /**/ cf<10,5>, cf<10,6>, cf<10,7>, cf<10,8>, /**/ cf<10,9>, cf<1,10>, cf<10,11>, cf<10,12>, /**/ cf<10,13>, cf<10,14>, cf<10,15>, cf<10,16> },
+ { /**/ cf<11,1>, cf<11,2>, cf<11,3>, cf<11,4>, /**/ cf<11,5>, cf<11,6>, cf<11,7>, cf<11,8>, /**/ cf<11,9>, cf<1,10>, cf<11,11>, cf<11,12>, /**/ cf<11,13>, cf<11,14>, cf<11,15>, cf<11,16> },
+ { /**/ cf<12,1>, cf<12,2>, cf<12,3>, cf<12,4>, /**/ cf<12,5>, cf<12,6>, cf<12,7>, cf<12,8>, /**/ cf<12,9>, cf<1,10>, cf<12,11>, cf<12,12>, /**/ cf<12,13>, cf<12,14>, cf<12,15>, cf<12,16> },
+ { /**/ cf<13,1>, cf<13,2>, cf<13,3>, cf<13,4>, /**/ cf<13,5>, cf<13,6>, cf<13,7>, cf<13,8>, /**/ cf<13,9>, cf<1,10>, cf<13,11>, cf<13,12>, /**/ cf<13,13>, cf<13,14>, cf<13,15>, cf<13,16> },
+ { /**/ cf<14,1>, cf<14,2>, cf<14,3>, cf<14,4>, /**/ cf<14,5>, cf<14,6>, cf<14,7>, cf<14,8>, /**/ cf<14,9>, cf<1,10>, cf<14,11>, cf<14,12>, /**/ cf<14,13>, cf<14,14>, cf<14,15>, cf<14,16> },
+ { /**/ cf<15,1>, cf<15,2>, cf<15,3>, cf<15,4>, /**/ cf<15,5>, cf<15,6>, cf<15,7>, cf<15,8>, /**/ cf<15,9>, cf<1,10>, cf<15,11>, cf<15,12>, /**/ cf<15,13>, cf<15,14>, cf<15,15>, cf<15,16> },
+ { /**/ cf<16,1>, cf<16,2>, cf<16,3>, cf<16,4>, /**/ cf<16,5>, cf<16,6>, cf<16,7>, cf<16,8>, /**/ cf<16,9>, cf<1,10>, cf<16,11>, cf<16,12>, /**/ cf<16,13>, cf<16,14>, cf<16,15>, cf<16,16> }
+ };
+
+ funcs[in_size-1][out_size-1](rules, size, input, output);
+ }
+ }
+}
+
set(build TRUE)
PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
if(WIN32)
- PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS openni openni2 pcap png vtk)
+ PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS openni openni2 ensenso davidSDK dssdk rssdk pcap png vtk)
else(WIN32)
- PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS openni openni2 pcap png vtk libusb-1.0)
+ PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS openni openni2 ensenso davidSDK dssdk pcap png vtk libusb-1.0)
endif(WIN32)
PCL_ADD_DOC("${SUBSYS_NAME}")
if(build)
- set(IMAGE_INCLUDES
- include/pcl/io/image_metadata_wrapper.h
- include/pcl/io/image.h
- include/pcl/io/image_rgb24.h
- include/pcl/io/image_yuv422.h
- include/pcl/io/image_ir.h
- include/pcl/io/image_depth.h
- )
-
- set(IMAGE_SOURCES
- src/image_rgb24.cpp
- src/image_yuv422.cpp
- src/image_ir.cpp
- src/image_depth.cpp
- )
-
- ## OpenNI 2.x
- OPTION(BUILD_OPENNI2 "Build the OpenNI 2 Grabber." OFF)
- MARK_AS_ADVANCED(BUILD_OPENNI2)
- if(NOT BUILD_OPENNI2)
- # Set OPENNI2_FOUND to false locally to avoid building anything OpenNI 2 related
- # Remember that other modules (libraries) need to check explicitly for BUILD_OPENNI2
- set(OPENNI2_FOUND FALSE)
- endif()
- if(OPENNI2_FOUND)
+ if(WITH_OPENNI2)
+ set(IMAGE_INCLUDES
+ include/pcl/io/image_metadata_wrapper.h
+ include/pcl/io/image.h
+ include/pcl/io/image_rgb24.h
+ include/pcl/io/image_yuv422.h
+ include/pcl/io/image_ir.h
+ include/pcl/io/image_depth.h
+ )
+
+ set(IMAGE_SOURCES
+ src/image_rgb24.cpp
+ src/image_yuv422.cpp
+ src/image_ir.cpp
+ src/image_depth.cpp
+ )
+
set(OPENNI2_GRABBER_INCLUDES
include/pcl/io/openni2_grabber.h
)
if(MSVC)
file(COPY ${OPENNI2_REDIST_DIR} DESTINATION ${CMAKE_BINARY_DIR}/bin PATTERN *.*)
endif(MSVC)
- endif(OPENNI2_FOUND)
-
- ## OpenNI 1.x
- OPTION(BUILD_OPENNI "Build the OpenNI Grabber." ON)
- MARK_AS_ADVANCED(BUILD_OPENNI)
- if(NOT BUILD_OPENNI)
- # Set OPENNI_FOUND to false locally to avoid building anything OpenNI related
- # Remember that other modules (libraries) need to check explicitly for BUILD_OPENNI
- set(OPENNI_FOUND FALSE)
endif()
- if(OPENNI_FOUND)
+
+ if(WITH_OPENNI)
set(OPENNI_GRABBER_INCLUDES
include/pcl/io/openni_grabber.h
include/pcl/io/oni_grabber.h
src/oni_grabber.cpp
${IMAGE_SOURCES}
)
- endif(OPENNI_FOUND)
+ endif()
source_group("Image Headers" FILES ${IMAGE_INCLUDES})
source_group("Image Sources" FILES ${IMAGE_SOURCES})
- if(FZAPI_FOUND)
+ if(WITH_FZAPI)
set(FZAPI_GRABBER_INCLUDES
include/pcl/io/fotonic_grabber.h
)
set(FZAPI_GRABBER_SOURCES
src/fotonic_grabber.cpp
)
- endif(FZAPI_FOUND)
+ endif()
- if(PXCAPI_FOUND)
- set(PXC_GRABBER_INCLUDES
- include/pcl/io/pxc_grabber.h
- )
- set(PXC_GRABBER_SOURCES
- src/pxc_grabber.cpp
- )
- endif(PXCAPI_FOUND)
+ if(WITH_ENSENSO)
+ set(ENSENSO_GRABBER_INCLUDES
+ include/pcl/io/ensenso_grabber.h
+ )
+ set(ENSENSO_GRABBER_SOURCES
+ src/ensenso_grabber.cpp
+ )
+ endif()
+
+ if(WITH_DAVIDSDK AND VTK_FOUND) # VTK is mandatory for OBJ/PLY/STL input/output
+ set(DAVIDSDK_GRABBER_INCLUDES
+ include/pcl/io/davidsdk_grabber.h
+ )
+ set(DAVIDSDK_GRABBER_SOURCES
+ src/davidsdk_grabber.cpp
+ )
+ endif()
+
+ if(WITH_DSSDK)
+ set(DSSDK_GRABBER_INCLUDES
+ include/pcl/io/depth_sense_grabber.h
+ )
+ set(DSSDK_GRABBER_SOURCES
+ src/depth_sense_grabber.cpp
+ src/depth_sense/depth_sense_grabber_impl.cpp
+ src/depth_sense/depth_sense_device_manager.cpp
+ )
+ endif()
+
+ if(WITH_RSSDK)
+ set(RSSDK_GRABBER_INCLUDES
+ include/pcl/io/real_sense_grabber.h
+ )
+ set(RSSDK_GRABBER_SOURCES
+ src/real_sense_grabber.cpp
+ src/real_sense/real_sense_device_manager.cpp
+ )
+ endif()
if(LIBUSB_1_FOUND)
set(DINAST_GRABBER_INCLUDES
src/ifs_io.cpp
src/image_grabber.cpp
src/hdl_grabber.cpp
+ src/vlp_grabber.cpp
src/robot_eye_grabber.cpp
src/file_io.cpp
+ src/auto_io.cpp
src/io_exception.cpp
${VTK_IO_SOURCE}
${OPENNI_GRABBER_SOURCES}
${IMAGE_SOURCES}
${DINAST_GRABBER_SOURCES}
${FZAPI_GRABBER_SOURCES}
- ${PXC_GRABBER_SOURCES}
+ ${ENSENSO_GRABBER_SOURCES}
+ ${DAVIDSDK_GRABBER_SOURCES}
+ ${DSSDK_GRABBER_SOURCES}
+ ${RSSDK_GRABBER_SOURCES}
)
if(PNG_FOUND)
list(APPEND srcs
"include/pcl/${SUBSYS_NAME}/eigen.h"
"include/pcl/${SUBSYS_NAME}/debayer.h"
"include/pcl/${SUBSYS_NAME}/file_io.h"
+ "include/pcl/${SUBSYS_NAME}/auto_io.h"
"include/pcl/${SUBSYS_NAME}/lzf.h"
"include/pcl/${SUBSYS_NAME}/lzf_image_io.h"
"include/pcl/${SUBSYS_NAME}/io.h"
"include/pcl/${SUBSYS_NAME}/ifs_io.h"
"include/pcl/${SUBSYS_NAME}/image_grabber.h"
"include/pcl/${SUBSYS_NAME}/hdl_grabber.h"
+ "include/pcl/${SUBSYS_NAME}/vlp_grabber.h"
"include/pcl/${SUBSYS_NAME}/robot_eye_grabber.h"
"include/pcl/${SUBSYS_NAME}/point_cloud_image_extractors.h"
"include/pcl/${SUBSYS_NAME}/io_exception.h"
${IMAGE_INCLUDES}
${DINAST_GRABBER_INCLUDES}
${FZAPI_GRABBER_INCLUDES}
- ${PXC_GRABBER_INCLUDES}
+ ${ENSENSO_GRABBER_INCLUDES}
+ ${DAVIDSDK_GRABBER_INCLUDES}
+ ${DSSDK_GRABBER_INCLUDES}
+ ${RSSDK_GRABBER_INCLUDES}
+ "include/pcl/${SUBSYS_NAME}/pxc_grabber.h" # contains only depreciation note
)
set(compression_incs
include/pcl/compression/organized_pointcloud_conversion.h
include/pcl/compression/libpng_wrapper.h
)
- if(OPENNI_FOUND OR OPENNI2_FOUND)
+ if(WITH_OPENNI OR WITH_OPENNI2)
list(APPEND compression_incs
include/pcl/compression/organized_pointcloud_compression.h
)
- endif(OPENNI_FOUND OR OPENNI2_FOUND)
+ endif()
endif(PNG_FOUND)
set(impl_incs
"include/pcl/${SUBSYS_NAME}/impl/pcd_io.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/auto_io.hpp"
"include/pcl/${SUBSYS_NAME}/impl/lzf_image_io.hpp"
"include/pcl/${SUBSYS_NAME}/impl/synchronized_queue.hpp"
"include/pcl/${SUBSYS_NAME}/impl/point_cloud_image_extractors.hpp"
include/pcl/compression/impl/octree_pointcloud_compression.hpp
${VTK_IO_INCLUDES_IMPL}
)
- if(PNG_FOUND AND (OPENNI_FOUND OR OPENNI2_FOUND) )
+ if(PNG_FOUND AND (WITH_OPENNI OR WITH_OPENNI2))
list(APPEND impl_incs
include/pcl/compression/impl/organized_pointcloud_compression.hpp
)
- endif(PNG_FOUND AND (OPENNI_FOUND OR OPENNI2_FOUND) )
+ endif()
set(LIB_NAME "pcl_${SUBSYS_NAME}")
link_directories(${VTK_LINK_DIRECTORIES})
target_link_libraries("${LIB_NAME}" pcl_common pcl_io_ply ${VTK_LIBRARIES} )
if(PNG_FOUND)
- target_link_libraries("${LIB_NAME}" "${PNG_LIBRARY}")
+ target_link_libraries("${LIB_NAME}" ${PNG_LIBRARIES})
endif(PNG_FOUND)
if(LIBUSB_1_FOUND)
target_link_libraries("${LIB_NAME}" ${LIBUSB_1_LIBRARIES})
endif(LIBUSB_1_FOUND)
- if(OPENNI2_FOUND)
+ if(WITH_OPENNI2)
target_link_libraries(${LIB_NAME} ${OPENNI2_LIBRARIES})
- endif(OPENNI2_FOUND)
+ endif()
- if(OPENNI_FOUND)
+ if(WITH_OPENNI)
target_link_libraries("${LIB_NAME}" ${OPENNI_LIBRARIES})
- endif(OPENNI_FOUND)
+ endif()
- if(FZAPI_FOUND)
+ if(WITH_FZAPI)
target_link_libraries("${LIB_NAME}" ${FZAPI_LIBS})
- if(WIN32)
- target_link_libraries("${LIB_NAME}" Version.lib)
- endif(WIN32)
- endif(FZAPI_FOUND)
+ if(WIN32)
+ target_link_libraries("${LIB_NAME}" Version.lib)
+ endif(WIN32)
+ endif()
+
+ if(WITH_ENSENSO)
+ target_link_libraries(${LIB_NAME} ${ENSENSO_LIBRARIES})
+ endif()
- if(PXCAPI_FOUND)
- link_directories(${PXCAPI_LIB_DIRS})
- target_link_libraries("${LIB_NAME}" ${PXCAPI_LIBS})
- endif(PXCAPI_FOUND)
+ if(WITH_DAVIDSDK)
+ target_link_libraries(${LIB_NAME} ${DAVIDSDK_LIBRARIES})
+ endif()
+
+ if(WITH_DSSDK)
+ target_link_libraries(${LIB_NAME} ${DSSDK_LIBRARIES})
+ endif()
+
+ if(WITH_RSSDK)
+ target_link_libraries(${LIB_NAME} ${RSSDK_LIBRARIES})
+ endif()
if (PCAP_FOUND)
target_link_libraries("${LIB_NAME}" ${PCAP_LIBRARIES})
set(EXT_DEPS eigen3)
- if(OPENNI_FOUND)
- list(APPEND EXT_DEPS openni-dev)
- endif(OPENNI_FOUND)
- if(OPENNI2_FOUND)
- list(APPEND EXT_DEPS openni2-dev)
- endif(OPENNI2_FOUND)
+ if(WITH_OPENNI)
+ list(APPEND EXT_DEPS libopenni)
+ endif()
+ if(WITH_OPENNI2)
+ list(APPEND EXT_DEPS libopenni2)
+ endif()
+ if(WITH_ENSENSO)
+ list(APPEND EXT_DEPS ensenso)
+ endif()
PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}"
"${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "")
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_IO_AUTO_IO_H_
+#define PCL_IO_AUTO_IO_H_
+
+#include <pcl/pcl_macros.h>
+#include <pcl/common/io.h>
+#include <pcl/io/boost.h>
+#include <cmath>
+#include <sstream>
+#include <pcl/PolygonMesh.h>
+#include <pcl/TextureMesh.h>
+
+namespace pcl
+{
+
+ namespace io
+ {
+ /** \brief Load a file into a PointCloud2 according to extension.
+ * \param[in] file_name the name of the file to load
+ * \param[out] blob the resultant pcl::PointCloud2 blob
+ * \ingroup io
+ */
+ PCL_EXPORTS int
+ load (const std::string& file_name, pcl::PCLPointCloud2& blob);
+
+ /** \brief Load a file into a template PointCloud type according to extension.
+ * \param[in] file_name the name of the file to load
+ * \param[out] cloud the resultant templated point cloud
+ * \ingroup io
+ */
+ template<typename PointT> int
+ load (const std::string& file_name, pcl::PointCloud<PointT>& cloud);
+
+ /** \brief Load a file into a PolygonMesh according to extension.
+ * \param[in] file_name the name of the file to load
+ * \param[out] mesh the resultant pcl::PolygonMesh
+ * \ingroup io
+ */
+ PCL_EXPORTS int
+ load (const std::string& file_name, pcl::PolygonMesh& mesh);
+
+ /** \brief Load a file into a TextureMesh according to extension.
+ * \param[in] file_name the name of the file to load
+ * \param[out] mesh the resultant pcl::TextureMesh
+ * \ingroup io
+ */
+ PCL_EXPORTS int
+ load (const std::string& file_name, pcl::TextureMesh& mesh);
+
+ /** \brief Save point cloud data to a binary file when available else to ASCII.
+ * \param[in] file_name the output file name
+ * \param[in] blob the point cloud data message
+ * \param[in] precision float precision when saving to ASCII files
+ * \ingroup io
+ */
+ PCL_EXPORTS int
+ save (const std::string& file_name, const pcl::PCLPointCloud2& blob, unsigned precision = 5);
+
+ /** \brief Save point cloud to a binary file when available else to ASCII.
+ * \param[in] file_name the output file name
+ * \param[in] cloud the point cloud
+ * \param[in] precision float precision when saving to ASCII files
+ * \ingroup io
+ */
+ template<typename PointT> int
+ save (const std::string& file_name, const pcl::PointCloud<PointT>& cloud, unsigned precision = 5);
+
+ /** \brief Saves a TextureMesh to a binary file when available else to ASCII.
+ * \param[in] file_name the name of the file to write to disk
+ * \param[in] tex_mesh the texture mesh to save
+ * \param[in] precision float precision when saving to ASCII files
+ * \ingroup io
+ */
+ PCL_EXPORTS int
+ save (const std::string &file_name, const pcl::TextureMesh &tex_mesh, unsigned precision = 5);
+
+ /** \brief Saves a PolygonMesh to a binary file when available else to ASCII.
+ * \param[in] file_name the name of the file to write to disk
+ * \param[in] mesh the polygonal mesh to save
+ * \param[in] precision float precision when saving to ASCII files
+ * \ingroup io
+ */
+ PCL_EXPORTS int
+ save (const std::string &file_name, const pcl::PolygonMesh &mesh, unsigned precision = 5);
+ }
+}
+
+#include <pcl/io/impl/auto_io.hpp>
+
+#endif //#ifndef PCL_IO_AUTO_IO_H_
#ifndef __CUDACC__
//https://bugreports.qt-project.org/browse/QTBUG-22829
#ifndef Q_MOC_RUN
+#include <boost/version.hpp>
#include <boost/numeric/conversion/cast.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/condition.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/algorithm/string.hpp>
+#ifndef Q_MOC_RUN
#include <boost/date_time/posix_time/posix_time.hpp>
+#endif
+#if BOOST_VERSION >= 104700
#include <boost/chrono.hpp>
+#endif
#include <boost/tokenizer.hpp>
#include <boost/foreach.hpp>
#include <boost/shared_array.hpp>
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_IO_BUFFERS_H
+#define PCL_IO_BUFFERS_H
+
+#include <vector>
+#include <limits>
+#include <cassert>
+
+#include <boost/cstdint.hpp>
+#include <boost/thread/mutex.hpp>
+
+namespace pcl
+{
+
+ namespace io
+ {
+
+ /** An abstract base class for fixed-size data buffers.
+ *
+ * A new chunk of data can be inserted using the push() method; the data
+ * elements stored in the buffer can be accessed using operator[]().
+ *
+ * Concrete implementations of this interface (such as AverageBuffer or
+ * MedianBuffer) may perform arbitrary data processing under the hood and
+ * provide access to certain quantities computed based on the input data
+ * rather than the data themselves.
+ *
+ * \author Sergey Alexandrov
+ * \ingroup io */
+ template <typename T>
+ class Buffer
+ {
+
+ public:
+
+ typedef T value_type;
+
+ virtual
+ ~Buffer ();
+
+ /** Access an element at a given index. */
+ virtual T
+ operator[] (size_t idx) const = 0;
+
+ /** Insert a new chunk of data into the buffer.
+ *
+ * Note that the \a data parameter is not `const`-qualified. This is
+ * done to allow deriving classes to implement no-copy data insertion,
+ * where the data is "stolen" from the input argument. */
+ virtual void
+ push (std::vector<T>& data) = 0;
+
+ /** Get the size of the buffer. */
+ inline size_t
+ size () const
+ {
+ return (size_);
+ }
+
+ protected:
+
+ Buffer (size_t size);
+
+ const size_t size_;
+
+ };
+
+ /** A simple buffer that only stores data.
+ *
+ * The buffer is thread-safe. */
+ template <typename T>
+ class SingleBuffer : public Buffer<T>
+ {
+
+ public:
+
+ /** Construct a buffer of given size. */
+ SingleBuffer (size_t size);
+
+ virtual
+ ~SingleBuffer ();
+
+ virtual T
+ operator[] (size_t idx) const;
+
+ virtual void
+ push (std::vector<T>& data);
+
+ private:
+
+ std::vector<T> data_;
+ mutable boost::mutex data_mutex_;
+
+ using Buffer<T>::size_;
+
+ };
+
+ /** A buffer that computes running window median of the data inserted.
+ *
+ * The buffer and window sizes are specified at construction time. The
+ * buffer size defines the number of elements in each data chunk that is
+ * inserted in the buffer. The window size is the number of last data
+ * chunks that are considered for median computation. The median is
+ * computed separately for 1st, 2nd, etc. element in data chunks.
+ *
+ * The data can contain invalid elements. For integral types zeros are
+ * assumed to be invalid elements, whereas for floating-point types it is
+ * quiet NaN. Invalid elements are ignored when computing median.
+ *
+ * The buffer is thread-safe. */
+ template <typename T>
+ class MedianBuffer : public Buffer<T>
+ {
+
+ public:
+
+ /** Construct a buffer of given size with given running window size.
+ *
+ * \param[in] size buffer size
+ * \param[in] window_size running window size over which the median
+ * value should be computed (0..255) */
+ MedianBuffer (size_t size, unsigned char window_size);
+
+ virtual
+ ~MedianBuffer ();
+
+ /** Access an element at a given index.
+ *
+ * This operation is constant time. */
+ virtual T
+ operator[] (size_t idx) const;
+
+ /** Insert a new chunk of data into the buffer.
+ *
+ * This operation is linear in buffer size and window size.
+ *
+ * \param[in] data input data chunk, the memory will be "stolen" */
+ virtual void
+ push (std::vector<T>& data);
+
+ private:
+
+ /** Compare two data elements.
+ *
+ * Invalid value is assumed to be larger than everything else. If both values
+ * are invalid, they are assumed to be equal.
+ *
+ * \return -1 if \c a < \c b, 0 if \c a == \c b, 1 if \c a > \c b */
+ static int compare (T a, T b);
+
+ const unsigned char window_size_;
+ const unsigned char midpoint_;
+
+ /// Data pushed into the buffer (last window_size_ chunks), logically
+ /// organized as a circular buffer
+ std::vector<std::vector<T> > data_;
+
+ /// Index of the last pushed data chunk in the data_ circular buffer
+ unsigned char data_current_idx_;
+
+ /// Indices that the argsort function would produce for data_ (with
+ /// dimensions swapped)
+ std::vector<std::vector<unsigned char> > data_argsort_indices_;
+
+ /// Number of invalid values in the buffer
+ std::vector<unsigned char> data_invalid_count_;
+
+ mutable boost::mutex data_mutex_;
+
+ using Buffer<T>::size_;
+
+ };
+
+ /** A buffer that computes running window average of the data inserted.
+ *
+ * The buffer and window sizes are specified at construction time. The
+ * buffer size defines the number of elements in each data chunk that is
+ * inserted in the buffer. The window size is the number of last data
+ * chunks that are considered for average computation. The average is
+ * computed separately for 1st, 2nd, etc. element in data chunks.
+ *
+ * The data can contain invalid elements. For integral types zeros are
+ * assumed to be invalid elements, whereas for floating-point types it is
+ * quiet NaN. Invalid elements are ignored when computing average.
+ *
+ * The buffer is thread-safe. */
+ template <typename T>
+ class AverageBuffer : public Buffer<T>
+ {
+
+ public:
+
+ /** Construct a buffer of given size with given running window size.
+ *
+ * \param[in] size buffer size
+ * \param[in] window_size running window size over which the median
+ * value should be computed (0..255) */
+ AverageBuffer (size_t size, unsigned char window_size);
+
+ virtual
+ ~AverageBuffer ();
+
+ /** Access an element at a given index.
+ *
+ * This operation is constant time. */
+ virtual T
+ operator[] (size_t idx) const;
+
+ /** Insert a new chunk of data into the buffer.
+ *
+ * This operation is linear in buffer size.
+ *
+ * \param[in] data input data chunk, the memory will be "stolen" */
+ virtual void
+ push (std::vector<T>& data);
+
+ private:
+
+ const unsigned char window_size_;
+
+ /// Data pushed into the buffer (last window_size_ chunks), logically
+ /// organized as a circular buffer
+ std::vector<std::vector<T> > data_;
+
+ /// Index of the last pushed data chunk in the data_ circular buffer
+ unsigned char data_current_idx_;
+
+ /// Current sum of the buffer
+ std::vector<T> data_sum_;
+
+ /// Number of invalid values in the buffer
+ std::vector<unsigned char> data_invalid_count_;
+
+ mutable boost::mutex data_mutex_;
+
+ using Buffer<T>::size_;
+
+ };
+
+ }
+
+}
+
+#include <pcl/io/impl/buffers.hpp>
+
+#endif /* PCL_IO_BUFFERS_H */
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Victor Lamoine (victor.lamoine@gmail.com)
+ */
+
+#include <pcl/pcl_config.h>
+
+#ifndef __PCL_IO_DAVIDSDK_GRABBER__
+#define __PCL_IO_DAVIDSDK_GRABBER__
+
+#include <pcl/common/time.h>
+#include <pcl/common/io.h>
+#include <boost/thread.hpp>
+#include <pcl/PolygonMesh.h>
+#include <pcl/io/grabber.h>
+
+#include <david.h>
+
+namespace pcl
+{
+ struct PointXYZ;
+ template <typename T> class PointCloud;
+
+ /** @brief Grabber for davidSDK structured light compliant devices.\n
+ * The [davidSDK SDK](http://www.david-3d.com/en/products/david-sdk) allows to use a structured light scanner to
+ * fetch clouds/meshes.\n
+ * The purpose of this grabber is NOT to provide all davidSDK functionalities but rather provide a PCL-unified interface to the sensor for
+ * basic operations.\n
+ * Please consult the [David-3d wiki](http://wiki.david-3d.com/david-wiki) for more information.
+ * @author Victor Lamoine (victor.lamoine@gmail.com)\n
+ * @ingroup io
+ */
+ class PCL_EXPORTS DavidSDKGrabber : public Grabber
+ {
+ public:
+ /** @cond */
+ typedef boost::shared_ptr<DavidSDKGrabber> Ptr;
+ typedef boost::shared_ptr<const DavidSDKGrabber> ConstPtr;
+
+ // Define callback signature typedefs
+ typedef void
+ (sig_cb_davidsdk_point_cloud) (const pcl::PointCloud<pcl::PointXYZ>::Ptr &);
+
+ typedef void
+ (sig_cb_davidsdk_mesh) (const pcl::PolygonMesh::Ptr &);
+
+ typedef void
+ (sig_cb_davidsdk_image) (const boost::shared_ptr<pcl::PCLImage> &);
+
+ typedef void
+ (sig_cb_davidsdk_point_cloud_image) (const pcl::PointCloud<pcl::PointXYZ>::Ptr &,
+ const boost::shared_ptr<pcl::PCLImage> &);
+
+ typedef void
+ (sig_cb_davidsdk_mesh_image) (const pcl::PolygonMesh::Ptr &,
+ const boost::shared_ptr<pcl::PCLImage> &);
+ /** @endcond */
+
+ /** @brief Constructor */
+ DavidSDKGrabber ();
+
+ /** @brief Destructor inherited from the Grabber interface. It never throws. */
+ virtual
+ ~DavidSDKGrabber () throw ();
+
+ /** @brief [Connect](http://docs.david-3d.com/sdk/en/classdavid_1_1_client_json_rpc.html#a4b948e57a2e5e7f9cdcf1171c500aa24) client
+ * @param[in] address
+ * @param[in] port
+ * @return Server info*/
+ david::ServerInfo
+ connect (const std::string & address = "127.0.0.1",
+ uint16_t port = david::DAVID_SDK_DefaultPort);
+
+ /** @brief [Disconnect](http://docs.david-3d.com/sdk/en/classdavid_1_1_client_json_rpc.html#a2770728a6de2c708df767bedf8be0814) client
+ * @param[in] stop_server */
+ void
+ disconnect (const bool stop_server);
+
+ /** @brief Start the point cloud and or image acquisition */
+ void
+ start ();
+
+ /** @brief Stop the data acquisition */
+ void
+ stop ();
+
+ /** @brief Check if the data acquisition is still running
+ * @return True if running, false otherwise */
+ bool
+ isRunning () const;
+
+ /** @brief Check if the client is connected
+ * @return True if connected, false otherwise */
+ bool
+ isConnected () const;
+
+ /** @brief Get class name
+ * @returns A string containing the class name */
+ std::string
+ getName () const;
+
+ /** @brief Get @ref local_path_ path directory
+ * @returns the path */
+ std::string
+ getLocalPath ();
+
+ /** @brief Get @ref remote_path_ path directory
+ * @returns the path */
+ std::string
+ getRemotePath ();
+
+ /** @brief Set @ref file_format_ to "obj" */
+ void
+ setFileFormatToOBJ ();
+
+ /** @brief Set @ref file_format_ to "ply"
+ * @warning This format is NOT available with trial versions of the davidSDK server! */
+ void
+ setFileFormatToPLY ();
+
+ /** @brief Set @ref file_format_ to "stl" */
+ void
+ setFileFormatToSTL ();
+
+ /** @brief Get @ref file_format_
+ * @returns the file format */
+ std::string
+ getFileFormat ();
+
+ /** @brief Set @ref local_path_ path directory for scanning files
+ * @param[in] path The directory path
+ *
+ * If the path is empty, using default value ("C:/temp") */
+ void
+ setLocalPath (std::string path);
+
+ /** @brief Set @ref remote_path_ path directory for scanning files
+ * @param[in] path The directory path
+ *
+ * If the string is empty, @ref remote_path_ = @ref local_path_ */
+ void
+ setRemotePath (std::string path);
+
+ /** @brief Set @ref local_path_ and @ref remote_path_ directory paths
+ * @param[in] local_path
+ * @param[in] remote_path
+ *
+ * If the path is empty, using default value ("C:/temp") */
+ void
+ setLocalAndRemotePaths (std::string local_path,
+ std::string remote_path);
+
+ /** @brief Calibrate the scanner
+ * @param[in] grid_size Size of the calibration grid in millimeters
+ * @return True if successful, false otherwise
+ *
+ * More information [here](http://wiki.david-3d.com/david3_user_manual/structured_light).\n
+ * Also see [ImportCalibration](http://docs.david-3d.com/sdk/en/classdavid_1_1_i_structured_light_scanner.html#a68e888636883d90aac7891d2ef9e6b27).\n
+ * and [ExportCalibration](http://docs.david-3d.com/sdk/en/classdavid_1_1_i_structured_light_scanner.html#a66817b07227f9a8852663d9141ae48db).
+ *
+ * @warning You MUST perform calibration each time you modify the camera/projector focus or move the camera relatively to the projector.\n
+ */
+ bool
+ calibrate (double grid_size);
+
+ /** @brief Capture a single point cloud and store it
+ * @param[out] cloud The cloud to be filled
+ * @return True if successful, false otherwise
+ * @warning Calls [DeleteAllMeshes()](http://docs.david-3d.com/sdk/en/classdavid_1_1_i_shape_fusion.html#aed22e458b51f1361803360c02c2d1403) */
+ bool
+ grabSingleCloud (pcl::PointCloud<pcl::PointXYZ> &cloud);
+
+ /** @brief Capture a single mesh and store it
+ * @param[out] mesh The mesh to be filled
+ * @return True if successful, false otherwise
+ * @warning Calls [DeleteAllMeshes()](http://docs.david-3d.com/sdk/en/classdavid_1_1_i_shape_fusion.html#aed22e458b51f1361803360c02c2d1403) */
+ bool
+ grabSingleMesh (pcl::PolygonMesh &mesh);
+
+ /** @brief Obtain the number of frames per second (FPS) */
+ float
+ getFramesPerSecond () const;
+
+ /** @brief davidSDK client */
+ david::Client david_;
+
+ protected:
+ /** @brief Grabber thread */
+ boost::thread grabber_thread_;
+
+ /** @brief Boost point cloud signal */
+ boost::signals2::signal<sig_cb_davidsdk_point_cloud>* point_cloud_signal_;
+
+ /** @brief Boost mesh signal */
+ boost::signals2::signal<sig_cb_davidsdk_mesh>* mesh_signal_;
+
+ /** @brief Boost image signal */
+ boost::signals2::signal<sig_cb_davidsdk_image>* image_signal_;
+
+ /** @brief Boost image + point cloud signal */
+ boost::signals2::signal<sig_cb_davidsdk_point_cloud_image>* point_cloud_image_signal_;
+
+ /** @brief Boost mesh + image signal */
+ boost::signals2::signal<sig_cb_davidsdk_mesh_image>* mesh_image_signal_;
+
+ /** @brief Whether the client is connected */
+ bool client_connected_;
+
+ /** @brief Whether an davidSDK device is running or not */
+ bool running_;
+
+ /** @brief Local path of directory where the scanning file will be located.
+ * @note Default value is @c C:/temp */
+ std::string local_path_;
+
+ /** @brief Remote path of directory where the scanning file will be located.
+ * @note If this is empty, the @ref local_path_ will be used instead
+ * Default value is @c C:/temp */
+ std::string remote_path_;
+
+ /** @brief Export file extension, available formats are STL, OBJ, PLY */
+ std::string file_format_;
+
+ /** @brief processGrabbing capture/processing frequency */
+ pcl::EventFrequency frequency_;
+
+ /** @brief Mutual exclusion for FPS computation */
+ mutable boost::mutex fps_mutex_;
+
+ /** @brief Continuously asks for images and or point clouds/meshes data from the device and publishes them if available. */
+ void
+ processGrabbing ();
+ };
+} // namespace pcl
+
+#endif // __PCL_IO_DAVIDSDK_GRABBER__
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_IO_DEPTH_SENSE_DEVICE_MANAGER_H
+#define PCL_IO_DEPTH_SENSE_DEVICE_MANAGER_H
+
+#include <boost/utility.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread.hpp>
+
+#include <pcl/pcl_exports.h>
+
+#include <DepthSense.hxx>
+
+namespace pcl
+{
+
+ namespace io
+ {
+
+ namespace depth_sense
+ {
+
+ struct DepthSenseGrabberImpl;
+
+ /** A helper class for enumerating and managing access to DepthSense
+ * devices. */
+ class PCL_EXPORTS DepthSenseDeviceManager : boost::noncopyable
+ {
+
+ public:
+
+ typedef boost::shared_ptr<DepthSenseDeviceManager> Ptr;
+
+ static Ptr&
+ getInstance ()
+ {
+ static Ptr instance;
+ if (!instance)
+ {
+ boost::mutex::scoped_lock lock (mutex_);
+ if (!instance)
+ instance.reset (new DepthSenseDeviceManager);
+ }
+ return (instance);
+ }
+
+ /** Get the number of connected DepthSense devices. */
+ inline size_t
+ getNumDevices ()
+ {
+ return (context_.getDevices ().size ());
+ }
+
+ /** Capture first available device and associate it with a given
+ * grabber instance. */
+ std::string
+ captureDevice (DepthSenseGrabberImpl* grabber);
+
+ /** Capture the device with given index and associate it with a given
+ * grabber instance. */
+ std::string
+ captureDevice (DepthSenseGrabberImpl* grabber, size_t index);
+
+ /** Capture the device with given serial number and associate it with
+ * a given grabber instance. */
+ std::string
+ captureDevice (DepthSenseGrabberImpl* grabber, const std::string& sn);
+
+ /** Release DepthSense device with given serial number. */
+ void
+ releaseDevice (const std::string& sn);
+
+ /** Reconfigure DepthSense device with given serial number. */
+ void
+ reconfigureDevice (const std::string& sn);
+
+ /** Start data capturing for a given device. */
+ void
+ startDevice (const std::string& sn);
+
+ /** Stop data capturing for a given device. */
+ void
+ stopDevice (const std::string& sn);
+
+ ~DepthSenseDeviceManager ();
+
+ private:
+
+ DepthSenseDeviceManager ();
+
+ std::string
+ captureDevice (DepthSenseGrabberImpl* grabber, DepthSense::Device device);
+
+ inline bool
+ isCaptured (const std::string& sn) const
+ {
+ return (captured_devices_.count (sn) != 0);
+ }
+
+ DepthSense::Context context_;
+
+ static boost::mutex mutex_;
+
+ /// Thread where the grabbing takes place.
+ boost::thread depth_sense_thread_;
+
+ struct CapturedDevice
+ {
+ DepthSenseGrabberImpl* grabber;
+ DepthSense::DepthNode depth_node;
+ DepthSense::ColorNode color_node;
+ };
+
+ std::map<std::string, CapturedDevice> captured_devices_;
+
+ };
+
+ } // namespace depth_sense
+
+ } // namespace io
+
+} // namespace pcl
+
+#endif /* PCL_IO_DEPTH_SENSE_DEVICE_MANAGER_H */
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_IO_DEPTH_SENSE_GRABBER_IMPL_H
+#define PCL_IO_DEPTH_SENSE_GRABBER_IMPL_H
+
+#include <boost/thread/mutex.hpp>
+
+#include <DepthSense.hxx>
+
+#include <pcl/common/time.h>
+#include <pcl/io/buffers.h>
+#include <pcl/io/depth_sense_grabber.h>
+
+namespace pcl
+{
+
+ namespace io
+ {
+
+ namespace depth_sense
+ {
+
+ struct DepthSenseGrabberImpl
+ {
+
+ /// Parent grabber
+ DepthSenseGrabber* p_;
+
+ /// Serial number of the device captured by this grabber
+ std::string device_id_;
+
+ bool is_running_;
+
+ int confidence_threshold_;
+ DepthSenseGrabber::TemporalFilteringType temporal_filtering_type_;
+
+ boost::shared_ptr<DepthSense::ProjectionHelper> projection_;
+
+ typedef DepthSenseGrabber::sig_cb_depth_sense_point_cloud sig_cb_depth_sense_point_cloud;
+ typedef DepthSenseGrabber::sig_cb_depth_sense_point_cloud_rgba sig_cb_depth_sense_point_cloud_rgba;
+
+ /// Signal to indicate whether new XYZ cloud is available
+ boost::signals2::signal<sig_cb_depth_sense_point_cloud>* point_cloud_signal_;
+ /// Signal to indicate whether new XYZRGBA cloud is available
+ boost::signals2::signal<sig_cb_depth_sense_point_cloud_rgba>* point_cloud_rgba_signal_;
+
+ /// Indicates whether there are subscribers for PointXYZ signal. This is
+ /// computed and stored on start()
+ bool need_xyz_;
+
+ /// Indicates whether there are subscribers for PointXYZRGBA signal. This
+ /// is computed and stored on start()
+ bool need_xyzrgba_;
+
+ EventFrequency frequency_;
+ mutable boost::mutex fps_mutex_;
+
+ /// Temporary buffer to store color data
+ std::vector<uint8_t> color_data_;
+
+ boost::shared_ptr<pcl::io::Buffer<float> > depth_buffer_;
+
+ static const int FRAMERATE = 30;
+ static const int WIDTH = 320;
+ static const int HEIGHT = 240;
+ static const int SIZE = WIDTH * HEIGHT;
+ static const int COLOR_WIDTH = 640;
+ static const int COLOR_HEIGHT = 480;
+ static const int COLOR_SIZE = COLOR_WIDTH * COLOR_HEIGHT;
+
+ DepthSenseGrabberImpl (DepthSenseGrabber* parent, const std::string& device_id);
+
+ ~DepthSenseGrabberImpl () throw ();
+
+ void
+ start ();
+
+ void
+ stop ();
+
+ float
+ getFramesPerSecond () const;
+
+ void
+ setConfidenceThreshold (int threshold);
+
+ void
+ enableTemporalFiltering (DepthSenseGrabber::TemporalFilteringType type, size_t window_size);
+
+ void
+ setCameraParameters (const DepthSense::StereoCameraParameters& parameters);
+
+ void
+ configureDepthNode (DepthSense::DepthNode node) const;
+
+ void
+ configureColorNode (DepthSense::ColorNode node) const;
+
+ /** A callback for processing depth data.
+ *
+ * It is supposed to be called from the DepthSense::Context thread that
+ * is managed by DepthSenseDeviceManager. */
+ void
+ onDepthDataReceived (DepthSense::DepthNode node, DepthSense::DepthNode::NewSampleReceivedData data);
+
+ /** A callback for processing color data.
+ *
+ * It is supposed to be called from the DepthSense::Context thread that
+ * is managed by DepthSenseDeviceManager. */
+ void
+ onColorDataReceived (DepthSense::ColorNode node, DepthSense::ColorNode::NewSampleReceivedData data);
+
+ template <typename Point> void
+ computeXYZ (PointCloud<Point>& cloud);
+
+ };
+
+ }
+
+ }
+
+}
+
+#endif /* PCL_IO_DEPTH_SENSE_GRABBER_IMPL_H */
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_IO_DEPTH_SENSE_GRABBER_H
+#define PCL_IO_DEPTH_SENSE_GRABBER_H
+
+#include <pcl/io/grabber.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+namespace pcl
+{
+
+ // Forward declaration of a class that contains actual grabber implementation
+ namespace io { namespace depth_sense { struct DepthSenseGrabberImpl; } }
+
+ /** Grabber for DepthSense devices (e.g. Creative Senz3D, SoftKinetic DS325).
+ *
+ * Requires [SoftKinetic DepthSense SDK](http://www.softkinetic.com/Support/Download).
+ *
+ * \author Sergey Alexandrov
+ * \ingroup io */
+ class PCL_EXPORTS DepthSenseGrabber : public Grabber
+ {
+
+ public:
+
+ typedef
+ void (sig_cb_depth_sense_point_cloud)
+ (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&);
+
+ typedef
+ void (sig_cb_depth_sense_point_cloud_rgba)
+ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&);
+
+ enum Mode
+ {
+ DepthSense_QVGA_30Hz = 0,
+ };
+
+ enum TemporalFilteringType
+ {
+ DepthSense_None = 0,
+ DepthSense_Median = 1,
+ DepthSense_Average = 2,
+ };
+
+ /** Create a grabber for a DepthSense device.
+ *
+ * The grabber "captures" the device, making it impossible for other
+ * grabbers to interact with it. The device is "released" when the
+ * grabber is destructed.
+ *
+ * This will throw pcl::IOException if there are no free devices that
+ * match the supplied \a device_id.
+ *
+ * \param[in] device_id device identifier, which might be a serial
+ * number, an index (with '#' prefix), or an empty string (to select the
+ * first available device)
+ */
+ DepthSenseGrabber (const std::string& device_id = "");
+
+ virtual
+ ~DepthSenseGrabber () throw ();
+
+ virtual void
+ start ();
+
+ virtual void
+ stop ();
+
+ virtual bool
+ isRunning () const;
+
+ virtual std::string
+ getName () const
+ {
+ return (std::string ("DepthSenseGrabber"));
+ }
+
+ virtual float
+ getFramesPerSecond () const;
+
+ /** Set the confidence threshold for depth data.
+ *
+ * Each pixel in a depth image output by the device has an associated
+ * confidence value. The higher this value is, the more reliable the
+ * datum is.
+ *
+ * The depth pixels (and their associated 3D points) are filtered based
+ * on the confidence value. Those that are below the threshold are
+ * discarded (i.e. their coordinates are set to NaN). */
+ void
+ setConfidenceThreshold (int threshold);
+
+ /** Enable temporal filtering of the depth data received from the device.
+ *
+ * The window size parameter is not relevant for `DepthSense_None`
+ * filtering type. */
+ void
+ enableTemporalFiltering (TemporalFilteringType type, size_t window_size = 1);
+
+ /** Disable temporal filtering. */
+ void
+ disableTemporalFiltering ();
+
+ /** Get the serial number of device captured by the grabber. */
+ std::string
+ getDeviceSerialNumber () const;
+
+ private:
+
+ pcl::io::depth_sense::DepthSenseGrabberImpl* p_;
+ friend struct pcl::io::depth_sense::DepthSenseGrabberImpl;
+
+ };
+
+}
+
+#endif /* PCL_IO_DEPTH_SENSE_GRABBER_H */
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Victor Lamoine (victor.lamoine@gmail.com)
+ */
+
+#include <pcl/pcl_config.h>
+
+#ifndef __PCL_IO_ENSENSO_GRABBER__
+#define __PCL_IO_ENSENSO_GRABBER__
+
+#include <pcl/common/time.h>
+#include <pcl/common/io.h>
+#include <pcl/io/eigen.h>
+#include <Eigen/Geometry>
+#include <Eigen/StdVector>
+#include <pcl/io/boost.h>
+#include <boost/thread.hpp>
+#include <boost/lexical_cast.hpp> // TODO: Remove when setExtrinsicCalibration is fixed
+
+#include <pcl/io/grabber.h>
+#include <pcl/common/synchronizer.h>
+
+#include <nxLib.h> // Ensenso SDK
+
+namespace pcl
+{
+ struct PointXYZ;
+ template <typename T> class PointCloud;
+
+ /** @brief Grabber for IDS-Imaging Ensenso's devices.\n
+ * The [Ensenso SDK](http://www.ensenso.de/manual/) allow to use multiple Ensenso devices to produce a single cloud.\n
+ * This feature is not implemented here, it is up to the user to configure multiple Ensenso cameras.\n
+ * @author Victor Lamoine (victor.lamoine@gmail.com)\n
+ * @ingroup io
+ */
+ class PCL_EXPORTS EnsensoGrabber : public Grabber
+ {
+ typedef std::pair<pcl::PCLImage, pcl::PCLImage> PairOfImages;
+
+ public:
+ /** @cond */
+ typedef boost::shared_ptr<EnsensoGrabber> Ptr;
+ typedef boost::shared_ptr<const EnsensoGrabber> ConstPtr;
+
+ // Define callback signature typedefs
+ typedef void
+ (sig_cb_ensenso_point_cloud) (const pcl::PointCloud<pcl::PointXYZ>::Ptr &);
+
+ typedef void
+ (sig_cb_ensenso_images) (const boost::shared_ptr<PairOfImages> &);
+
+ typedef void
+ (sig_cb_ensenso_point_cloud_images) (const pcl::PointCloud<pcl::PointXYZ>::Ptr &,
+ const boost::shared_ptr<PairOfImages> &);
+ /** @endcond */
+
+ /** @brief Constructor */
+ EnsensoGrabber ();
+
+ /** @brief Destructor inherited from the Grabber interface. It never throws. */
+ virtual
+ ~EnsensoGrabber () throw ();
+
+ /** @brief Searches for available devices
+ * @returns The number of Ensenso devices connected */
+ int
+ enumDevices () const;
+
+ /** @brief Opens an Ensenso device
+ * @param[in] device The device ID to open
+ * @return True if successful, false otherwise */
+ bool
+ openDevice (const int device = 0);
+
+ /** @brief Closes the Ensenso device
+ * @return True if successful, false otherwise */
+ bool
+ closeDevice ();
+
+ /** @brief Start the point cloud and or image acquisition
+ * @note Opens device "0" if no device is open */
+ void
+ start ();
+
+ /** @brief Stop the data acquisition */
+ void
+ stop ();
+
+ /** @brief Check if the data acquisition is still running
+ * @return True if running, false otherwise */
+ bool
+ isRunning () const;
+
+ /** @brief Check if a TCP port is opened
+ * @return True if open, false otherwise */
+ bool
+ isTcpPortOpen () const;
+
+ /** @brief Get class name
+ * @returns A string containing the class name */
+ std::string
+ getName () const;
+
+ /** @brief Configure Ensenso capture settings
+ * @param[in] auto_exposure If set to yes, the exposure parameter will be ignored
+ * @param[in] auto_gain If set to yes, the gain parameter will be ignored
+ * @param[in] bining Pixel bining: 1, 2 or 4
+ * @param[in] exposure In milliseconds, from 0.01 to 20 ms
+ * @param[in] front_light Infrared front light (useful for calibration)
+ * @param[in] gain Float between 1 and 4
+ * @param[in] gain_boost
+ * @param[in] hardware_gamma
+ * @param[in] hdr High Dynamic Range (check compatibility with other options in Ensenso manual)
+ * @param[in] pixel_clock In MegaHertz, from 5 to 85
+ * @param[in] projector Use the central infrared projector or not
+ * @param[in] target_brightness Between 40 and 210
+ * @param[in] trigger_mode
+ * @param[in] use_disparity_map_area_of_interest
+ * @return True if successful, false otherwise
+ * @note See [Capture tree item](http://www.ensenso.de/manual/index.html?capture.htm) for more
+ * details about the parameters. */
+ bool
+ configureCapture (const bool auto_exposure = true,
+ const bool auto_gain = true,
+ const int bining = 1,
+ const float exposure = 0.32,
+ const bool front_light = false,
+ const int gain = 1,
+ const bool gain_boost = false,
+ const bool hardware_gamma = false,
+ const bool hdr = false,
+ const int pixel_clock = 10,
+ const bool projector = true,
+ const int target_brightness = 80,
+ const std::string trigger_mode = "Software",
+ const bool use_disparity_map_area_of_interest = false) const;
+
+ /** @brief Capture a single point cloud and store it
+ * @param[out] cloud The cloud to be filled
+ * @return True if successful, false otherwise
+ * @warning A device must be opened and not running */
+ bool
+ grabSingleCloud (pcl::PointCloud<pcl::PointXYZ> &cloud);
+
+ /** @brief Set up the Ensenso sensor and API to do 3D extrinsic calibration using the Ensenso 2D patterns
+ * @param[in] grid_spacing
+ * @return True if successful, false otherwise
+ *
+ * Configures the capture parameters to default values (eg: @c projector = @c false and @c front_light = @c true)
+ * Discards all previous patterns, configures @c grid_spacing
+ * @warning A device must be opened and must not be running.
+ * @note See the [Ensenso manual](http://www.ensenso.de/manual/index.html?calibratehandeyeparameters.htm) for more
+ * information about the extrinsic calibration process.
+ * @note [GridSize](http://www.ensenso.de/manual/index.html?gridsize.htm) item is protected in the NxTree, you can't modify it.
+ */
+ bool
+ initExtrinsicCalibration (const int grid_spacing) const;
+
+ /** @brief Clear calibration patterns buffer */
+ bool
+ clearCalibrationPatternBuffer () const;
+
+ /** @brief Captures a calibration pattern
+ * @return the number of calibration patterns stored in the nxTree, -1 on error
+ * @warning A device must be opened and must not be running.
+ * @note You should use @ref initExtrinsicCalibration before */
+ int
+ captureCalibrationPattern () const;
+
+ /** @brief Estimate the calibration pattern pose
+ * @param[out] pattern_pose the calibration pattern pose
+ * @return true if successful, false otherwise
+ * @warning A device must be opened and must not be running.
+ * @note At least one calibration pattern must have been captured before, use @ref captureCalibrationPattern before */
+ bool
+ estimateCalibrationPatternPose (Eigen::Affine3d &pattern_pose) const;
+
+ /** @brief Computes the calibration matrix using the collected patterns and the robot poses vector
+ * @param[in] robot_poses A list of robot poses, 1 for each pattern acquired (in the same order)
+ * @param[out] json The extrinsic calibration data in JSON format
+ * @param[in] setup Moving or Fixed, please refer to the Ensenso documentation
+ * @param[in] target Please refer to the Ensenso documentation
+ * @param[in] guess_tf Guess transformation for the calibration matrix (translation in meters)
+ * @param[in] pretty_format JSON formatting style
+ * @return True if successful, false otherwise
+ * @warning This can take up to 120 seconds
+ * @note Check the result with @ref getResultAsJson.
+ * If you want to permanently store the result, use @ref storeEEPROMExtrinsicCalibration. */
+ bool
+ computeCalibrationMatrix (const std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d> > &robot_poses,
+ std::string &json,
+ const std::string setup = "Moving", // Default values: Moving or Fixed
+ const std::string target = "Hand", // Default values: Hand or Workspace
+ const Eigen::Affine3d &guess_tf = Eigen::Affine3d::Identity (),
+ const bool pretty_format = true) const;
+
+ /** @brief Copy the link defined in the Link node of the nxTree to the EEPROM
+ * @return True if successful, false otherwise
+ * Refer to @ref setExtrinsicCalibration for more information about how the EEPROM works.\n
+ * After calling @ref computeCalibrationMatrix, this enables to permanently store the matrix.
+ * @note The target must be specified (@ref computeCalibrationMatrix specifies the target) */
+ bool
+ storeEEPROMExtrinsicCalibration () const;
+
+ /** @brief Clear the extrinsic calibration stored in the EEPROM by writing an identity matrix
+ * @return True if successful, false otherwise */
+ bool
+ clearEEPROMExtrinsicCalibration ();
+
+ /** @brief Update Link node in NxLib tree
+ * @param[in] target "Hand" or "Workspace" for example
+ * @param[in] euler_angle
+ * @param[in] rotation_axis
+ * @param[in] translation Translation in meters
+ * @return True if successful, false otherwise
+ * @warning Translation are in meters, rotation angles in radians! (stored in mm/radians in Ensenso tree)
+ * @note If a calibration has been stored in the EEPROM, it is copied in the Link node at nxLib tree start.
+ * This method overwrites the Link node but does not write to the EEPROM.
+ *
+ * More information on the parameters can be found in [Link node](http://www.ensenso.de/manual/index.html?cameralink.htm)
+ * section of the Ensenso manual.
+ *
+ * The point cloud you get from the Ensenso is already transformed using this calibration matrix.
+ * Make sure it is the identity transformation if you want the original point cloud! (use @ref clearEEPROMExtrinsicCalibration)
+ * Use @ref storeEEPROMExtrinsicCalibration to permanently store this transformation */
+ bool
+ setExtrinsicCalibration (const double euler_angle,
+ Eigen::Vector3d &rotation_axis,
+ const Eigen::Vector3d &translation,
+ const std::string target = "Hand");
+
+ /** @brief Update Link node in NxLib tree with an identity matrix
+ * @param[in] target "Hand" or "Workspace" for example
+ * @return True if successful, false otherwise */
+ bool
+ setExtrinsicCalibration (const std::string target = "Hand");
+
+ /** @brief Update Link node in NxLib tree
+ * @param[in] transformation Transformation matrix
+ * @param[in] target "Hand" or "Workspace" for example
+ * @return True if successful, false otherwise
+ * @warning Translation are in meters, rotation angles in radians! (stored in mm/radians in Ensenso tree)
+ * @note If a calibration has been stored in the EEPROM, it is copied in the Link node at nxLib tree start.
+ * This method overwrites the Link node but does not write to the EEPROM.
+ *
+ * More information on the parameters can be found in [Link node](http://www.ensenso.de/manual/index.html?cameralink.htm)
+ * section of the Ensenso manual.
+ *
+ * The point cloud you get from the Ensenso is already transformed using this calibration matrix.
+ * Make sure it is the identity transformation if you want the original point cloud! (use @ref clearEEPROMExtrinsicCalibration)
+ * Use @ref storeEEPROMExtrinsicCalibration to permanently store this transformation */
+ bool
+ setExtrinsicCalibration (const Eigen::Affine3d &transformation,
+ const std::string target = "Hand");
+
+ /** @brief Obtain the number of frames per second (FPS) */
+ float
+ getFramesPerSecond () const;
+
+ /** @brief Open TCP port to enable access via the [nxTreeEdit](http://www.ensenso.de/manual/software_components.htm) program.
+ * @param[in] port The port number
+ * @return True if successful, false otherwise */
+ bool
+ openTcpPort (const int port = 24000);
+
+ /** @brief Close TCP port program
+ * @return True if successful, false otherwise
+ * @warning If you do not close the TCP port the program might exit with the port still open, if it is the case
+ * use @code ps -ef @endcode and @code kill PID @endcode to kill the application and effectively close the port. */
+ bool
+ closeTcpPort (void);
+
+ /** @brief Returns the full NxLib tree as a JSON string
+ * @param[in] pretty_format JSON formatting style
+ * @return A string containing the NxLib tree in JSON format */
+ std::string
+ getTreeAsJson (const bool pretty_format = true) const;
+
+ /** @brief Returns the Result node (of the last command) as a JSON string
+ * @param[in] pretty_format JSON formatting style
+ * @return A string containing the Result node in JSON format
+ */
+ std::string
+ getResultAsJson (const bool pretty_format = true) const;
+
+ /** @brief Get the Euler angles corresponding to a JSON string (an angle axis transformation)
+ * @param[in] json A string containing the angle axis transformation in JSON format
+ * @param[out] x The X translation
+ * @param[out] y The Y translation
+ * @param[out] z The Z translation
+ * @param[out] w The yaW angle
+ * @param[out] p The Pitch angle
+ * @param[out] r The Roll angle
+ * @return True if successful, false otherwise
+ * @warning The units are meters and radians!
+ * @note See: [transformation page](http://www.ensenso.de/manual/transformation.htm) in the EnsensoSDK documentation
+ */
+ bool
+ jsonTransformationToEulerAngles (const std::string &json,
+ double &x,
+ double &y,
+ double &z,
+ double &w,
+ double &p,
+ double &r) const;
+
+ /** @brief Get the angle axis parameters corresponding to a JSON string
+ * @param[in] json A string containing the angle axis transformation in JSON format
+ * @param[out] alpha Euler angle
+ * @param[out] axis Axis vector
+ * @param[out] translation Translation vector
+ * @return True if successful, false otherwise
+ * @warning The units are meters and radians!
+ * @note See: [transformation page](http://www.ensenso.de/manual/transformation.htm) in the EnsensoSDK documentation
+ */
+ bool
+ jsonTransformationToAngleAxis (const std::string json,
+ double &alpha,
+ Eigen::Vector3d &axis,
+ Eigen::Vector3d &translation) const;
+
+
+ /** @brief Get the JSON string corresponding to a 4x4 matrix
+ * @param[in] transformation The input transformation
+ * @param[out] matrix A matrix containing JSON transformation
+ * @return True if successful, false otherwise
+ * @warning The units are meters and radians!
+ * @note See: [ConvertTransformation page](http://www.ensenso.de/manual/index.html?cmdconverttransformation.htm) in the EnsensoSDK documentation
+ */
+ bool
+ jsonTransformationToMatrix (const std::string transformation,
+ Eigen::Affine3d &matrix) const;
+
+
+ /** @brief Get the JSON string corresponding to the Euler angles transformation
+ * @param[in] x The X translation
+ * @param[in] y The Y translation
+ * @param[in] z The Z translation
+ * @param[in] w The yaW angle
+ * @param[in] p The Pitch angle
+ * @param[in] r The Roll angle
+ * @param[out] json A string containing the Euler angles transformation in JSON format
+ * @param[in] pretty_format JSON formatting style
+ * @return True if successful, false otherwise
+ * @warning The units are meters and radians!
+ * @note See: [transformation page](http://www.ensenso.de/manual/transformation.htm) in the EnsensoSDK documentation
+ */
+ bool
+ eulerAnglesTransformationToJson (const double x,
+ const double y,
+ const double z,
+ const double w,
+ const double p,
+ const double r,
+ std::string &json,
+ const bool pretty_format = true) const;
+
+ /** @brief Get the JSON string corresponding to an angle axis transformation
+ * @param[in] x The X angle
+ * @param[in] y The Y angle
+ * @param[in] z The Z angle
+ * @param[in] rx The X component of the Euler axis
+ * @param[in] ry The Y component of the Euler axis
+ * @param[in] rz The Z component of the Euler axis
+ * @param[in] alpha The Euler rotation angle
+ * @param[out] json A string containing the angle axis transformation in JSON format
+ * @param[in] pretty_format JSON formatting style
+ * @return True if successful, false otherwise
+ * @warning The units are meters and radians! (the Euler axis doesn't need to be normalized)
+ * @note See: [transformation page](http://www.ensenso.de/manual/transformation.htm) in the EnsensoSDK documentation
+ */
+ bool
+ angleAxisTransformationToJson (const double x,
+ const double y,
+ const double z,
+ const double rx,
+ const double ry,
+ const double rz,
+ const double alpha,
+ std::string &json,
+ const bool pretty_format = true) const;
+
+ /** @brief Get the JSON string corresponding to a 4x4 matrix
+ * @param[in] matrix The input matrix
+ * @param[out] json A string containing the matrix transformation in JSON format
+ * @param[in] pretty_format JSON formatting style
+ * @return True if successful, false otherwise
+ * @warning The units are meters and radians!
+ * @note See: [ConvertTransformation page](http://www.ensenso.de/manual/index.html?cmdconverttransformation.htm)
+ * in the EnsensoSDK documentation */
+ bool
+ matrixTransformationToJson (const Eigen::Affine3d &matrix,
+ std::string &json,
+ const bool pretty_format = true) const;
+
+ /** @brief Reference to the NxLib tree root
+ * @warning You must handle NxLib exceptions manually when playing with @ref root_ !
+ * See ensensoExceptionHandling in ensenso_grabber.cpp */
+ boost::shared_ptr<const NxLibItem> root_;
+
+ /** @brief Reference to the camera tree
+ * @warning You must handle NxLib exceptions manually when playing with @ref camera_ ! */
+ NxLibItem camera_;
+
+ protected:
+ /** @brief Grabber thread */
+ boost::thread grabber_thread_;
+
+ /** @brief Boost point cloud signal */
+ boost::signals2::signal<sig_cb_ensenso_point_cloud>* point_cloud_signal_;
+
+ /** @brief Boost images signal */
+ boost::signals2::signal<sig_cb_ensenso_images>* images_signal_;
+
+ /** @brief Boost images + point cloud signal */
+ boost::signals2::signal<sig_cb_ensenso_point_cloud_images>* point_cloud_images_signal_;
+
+ /** @brief Whether an Ensenso device is opened or not */
+ bool device_open_;
+
+ /** @brief Whether an TCP port is opened or not */
+ bool tcp_open_;
+
+ /** @brief Whether an Ensenso device is running or not */
+ bool running_;
+
+ /** @brief Point cloud capture/processing frequency */
+ pcl::EventFrequency frequency_;
+
+ /** @brief Mutual exclusion for FPS computation */
+ mutable boost::mutex fps_mutex_;
+
+ /** @brief Convert an Ensenso time stamp into a PCL/ROS time stamp
+ * @param[in] ensenso_stamp
+ * @return PCL stamp
+ * The Ensenso API returns the time elapsed from January 1st, 1601 (UTC); on Linux OS the reference time is January 1st, 1970 (UTC).
+ * See [time-stamp page](http://www.ensenso.de/manual/index.html?json_types.htm) for more info about the time stamp conversion. */
+ pcl::uint64_t
+ static
+ getPCLStamp (const double ensenso_stamp);
+
+ /** @brief Get OpenCV image type corresponding to the parameters given
+ * @param channels number of channels in the image
+ * @param bpe bytes per element
+ * @param isFlt is float
+ * @return the OpenCV type as a string */
+ std::string
+ static
+ getOpenCVType (const int channels,
+ const int bpe,
+ const bool isFlt);
+
+ /** @brief Continuously asks for images and or point clouds data from the device and publishes them if available.
+ * PCL time stamps are filled for both the images and clouds grabbed (see @ref getPCLStamp)
+ * @note The cloud time stamp is the RAW image time stamp */
+ void
+ processGrabbing ();
+ };
+} // namespace pcl
+
+#endif // __PCL_IO_ENSENSO_GRABBER__
+
unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
{
Type value;
- if (st == "nan")
+ if (boost::iequals(st, "nan"))
{
value = std::numeric_limits<Type>::quiet_NaN ();
cloud.is_dense = false;
unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
{
int8_t value;
- if (st == "nan")
+ if (boost::iequals(st, "nan"))
{
value = static_cast<int8_t> (std::numeric_limits<int>::quiet_NaN ());
cloud.is_dense = false;
unsigned int point_index, unsigned int field_idx, unsigned int fields_count)
{
uint8_t value;
- if (st == "nan")
+ if (boost::iequals(st, "nan"))
{
value = static_cast<uint8_t> (std::numeric_limits<int>::quiet_NaN ());
cloud.is_dense = false;
fields_count * sizeof (uint8_t)], reinterpret_cast<char*> (&value), sizeof (uint8_t));
}
- namespace io
- {
- /** \brief Load a file into a PointCloud2 according to extension.
- * \param[in] file_name the name of the file to load
- * \param[out] blob the resultant pcl::PointCloud2 blob
- * \ingroup io
- */
- PCL_EXPORTS int
- load (const std::string& file_name, pcl::PCLPointCloud2& blob);
-
- /** \brief Load a file into a template PointCloud type according to extension.
- * \param[in] file_name the name of the file to load
- * \param[out] cloud the resultant templated point cloud
- * \ingroup io
- */
- template<typename PointT> int
- load (const std::string& file_name, pcl::PointCloud<PointT>& cloud);
-
- /** \brief Load a file into a PolygonMesh according to extension.
- * \param[in] file_name the name of the file to load
- * \param[out] mesh the resultant pcl::PolygonMesh
- * \ingroup io
- */
- PCL_EXPORTS int
- load (const std::string& file_name, pcl::PolygonMesh& mesh);
-
- /** \brief Load a file into a TextureMesh according to extension.
- * \param[in] file_name the name of the file to load
- * \param[out] mesh the resultant pcl::TextureMesh
- * \ingroup io
- */
- PCL_EXPORTS int
- load (const std::string& file_name, pcl::TextureMesh& mesh);
-
- /** \brief Save point cloud data to a binary file when available else to ASCII.
- * \param[in] file_name the output file name
- * \param[in] blob the point cloud data message
- * \param[in] precision float precision when saving to ASCII files
- * \ingroup io
- */
- PCL_EXPORTS int
- save (const std::string& file_name, const pcl::PCLPointCloud2& blob, unsigned precision = 5);
-
- /** \brief Save point cloud to a binary file when available else to ASCII.
- * \param[in] file_name the output file name
- * \param[in] cloud the point cloud
- * \param[in] precision float precision when saving to ASCII files
- * \ingroup io
- */
- template<typename PointT> int
- save (const std::string& file_name, const pcl::PointCloud<PointT>& cloud, unsigned precision = 5);
-
- /** \brief Saves a TextureMesh to a binary file when available else to ASCII.
- * \param[in] file_name the name of the file to write to disk
- * \param[in] tex_mesh the texture mesh to save
- * \param[in] precision float precision when saving to ASCII files
- * \ingroup io
- */
- PCL_EXPORTS int
- save (const std::string &file_name, const pcl::TextureMesh &tex_mesh, unsigned precision = 5);
-
- /** \brief Saves a PolygonMesh to a binary file when available else to ASCII.
- * \param[in] file_name the name of the file to write to disk
- * \param[in] mesh the polygonal mesh to save
- * \param[in] precision float precision when saving to ASCII files
- * \ingroup io
- */
- PCL_EXPORTS int
- save (const std::string &file_name, const pcl::PolygonMesh &mesh, unsigned precision = 5);
- }
}
#endif //#ifndef PCL_IO_FILE_IO_H_
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
- * Copyright (c) 2012 The MITRE Corporation
+ * Copyright (c) 2012-, Open Perception, Inc.
+ * Copyright (c) 2012,2015 The MITRE Corporation
*
* All rights reserved.
*
/** \brief Signal used for a single sector
* Represents 1 corrected packet from the HDL Velodyne
*/
- typedef void (sig_cb_velodyne_hdl_scan_point_cloud_xyz) (
- const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&,
- float, float);
+ typedef void
+ (sig_cb_velodyne_hdl_scan_point_cloud_xyz) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&,
+ float,
+ float);
/** \brief Signal used for a single sector
* Represents 1 corrected packet from the HDL Velodyne. Each laser has a different RGB
*/
- typedef void (sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb) (
- const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&,
- float, float);
+ typedef void
+ (sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&,
+ float,
+ float);
/** \brief Signal used for a single sector
* Represents 1 corrected packet from the HDL Velodyne with the returned intensity.
*/
- typedef void (sig_cb_velodyne_hdl_scan_point_cloud_xyzi) (
- const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&,
- float startAngle, float);
+ typedef void
+ (sig_cb_velodyne_hdl_scan_point_cloud_xyzi) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&,
+ float startAngle,
+ float);
/** \brief Signal used for a 360 degree sweep
* Represents multiple corrected packets from the HDL Velodyne
* This signal is sent when the Velodyne passes angle "0"
*/
- typedef void (sig_cb_velodyne_hdl_sweep_point_cloud_xyz) (
-
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&);
+ typedef void
+
(sig_cb_velodyne_hdl_sweep_point_cloud_xyz) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&);
/** \brief Signal used for a 360 degree sweep
* Represents multiple corrected packets from the HDL Velodyne with the returned intensity
* This signal is sent when the Velodyne passes angle "0"
*/
- typedef void (sig_cb_velodyne_hdl_sweep_point_cloud_xyzi) (
-
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&);
+ typedef void
+
(sig_cb_velodyne_hdl_sweep_point_cloud_xyzi) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&);
/** \brief Signal used for a 360 degree sweep
* Represents multiple corrected packets from the HDL Velodyne
* This signal is sent when the Velodyne passes angle "0". Each laser has a different RGB
*/
- typedef void (sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb) (
-
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&);
+ typedef void
+
(sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&);
/** \brief Constructor taking an optional path to an HDL corrections file. The Grabber will listen on the default IP/port for data packets [192.168.3.255/2368]
* \param[in] correctionsFile Path to a file which contains the correction parameters for the HDL. This parameter is mandatory for the HDL-64, optional for the HDL-32
* \param[in] pcapFile Path to a file which contains previously captured data packets. This parameter is optional
*/
HDLGrabber (const std::string& correctionsFile = "",
- const std::string& pcapFile = "");
+ const std::string& pcapFile = "");
/** \brief Constructor taking a pecified IP/port and an optional path to an HDL corrections file.
* \param[in] ipAddress IP Address that should be used to listen for HDL packets
* \param[in] correctionsFile Path to a file which contains the correction parameters for the HDL. This field is mandatory for the HDL-64, optional for the HDL-32
*/
HDLGrabber (const boost::asio::ip::address& ipAddress,
- const unsigned short port, const std::string& correctionsFile = "");
+ const unsigned short port,
+ const std::string& correctionsFile = "");
/** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
- virtual ~HDLGrabber () throw ();
+ virtual
+ ~HDLGrabber () throw ();
/** \brief Starts processing the Velodyne packets, either from the network or PCAP file. */
- virtual void start ();
+ virtual void
+ start ();
/** \brief Stops processing the Velodyne packets, either from the network or PCAP file */
- virtual void stop ();
+ virtual void
+ stop ();
/** \brief Obtains the name of this I/O Grabber
* \return The name of the grabber
*/
- virtual std::string getName () const;
+ virtual std::string
+ getName () const;
/** \brief Check if the grabber is still running.
* \return TRUE if the grabber is running, FALSE otherwise
*/
- virtual bool isRunning () const;
+ virtual bool
+ isRunning () const;
/** \brief Returns the number of frames per second.
*/
- virtual float getFramesPerSecond () const;
+ virtual float
+ getFramesPerSecond () const;
/** \brief Allows one to filter packets based on the SOURCE IP address and PORT
* This can be used, for instance, if multiple HDL LIDARs are on the same network
*/
- void filterPackets (const boost::asio::ip::address& ipAddress,
- const unsigned short port = 443);
+ void
+ filterPackets (const boost::asio::ip::address& ipAddress,
+ const unsigned short port = 443);
/** \brief Allows one to customize the colors used for each of the lasers.
*/
- void setLaserColorRGB (const pcl::RGB& color, unsigned int laserNumber);
+ void
+ setLaserColorRGB (const pcl::RGB& color,
+ unsigned int laserNumber);
/** \brief Any returns from the HDL with a distance less than this are discarded.
* This value is in meters
* Default: 0.0
*/
- void setMinimumDistanceThreshold(float & minThreshold);
+ void
+ setMinimumDistanceThreshold (float & minThreshold);
/** \brief Any returns from the HDL with a distance greater than this are discarded.
* This value is in meters
* Default: 10000.0
*/
- void setMaximumDistanceThreshold(float & maxThreshold);
+ void
+ setMaximumDistanceThreshold (float & maxThreshold);
/** \brief Returns the current minimum distance threshold, in meters
*/
- float getMinimumDistanceThreshold();
+ float
+ getMinimumDistanceThreshold ();
/** \brief Returns the current maximum distance threshold, in meters
*/
- float getMaximumDistanceThreshold();
+ float
+ getMaximumDistanceThreshold ();
protected:
static const int HDL_DATA_PORT = 2368;
static const int HDL_LASER_PER_FIRING = 32;
static const int HDL_MAX_NUM_LASERS = 64;
static const int HDL_FIRING_PER_PKT = 12;
- static const boost::asio::ip::address HDL_DEFAULT_NETWORK_ADDRESS;
enum HDLBlock
{
{
HDLFiringData firingData[HDL_FIRING_PER_PKT];
unsigned int gpsTimestamp;
- unsigned char blank1;
- unsigned char blank2;
+ unsigned char mode;
+ unsigned char sensorType;
};
struct HDLLaserCorrection
double cosVertOffsetCorrection;
};
+ HDLLaserCorrection laser_corrections_[HDL_MAX_NUM_LASERS];
+ unsigned int last_azimuth_;
+ boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > current_scan_xyz_, current_sweep_xyz_;
+ boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > current_scan_xyzi_, current_sweep_xyzi_;
+ boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGBA> > current_scan_xyzrgb_, current_sweep_xyzrgb_;
+ boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyz>* sweep_xyz_signal_;
+ boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb>* sweep_xyzrgb_signal_;
+ boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzi>* sweep_xyzi_signal_;
+ boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyz>* scan_xyz_signal_;
+ boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb>* scan_xyzrgb_signal_;
+ boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzi>* scan_xyzi_signal_;
+
+ void
+ fireCurrentSweep ();
+
+ void
+ fireCurrentScan (const unsigned short startAngle,
+ const unsigned short endAngle);
+ void
+ computeXYZI (pcl::PointXYZI& pointXYZI,
+ int azimuth,
+ HDLLaserReturn laserReturn,
+ HDLLaserCorrection correction);
+
+
private:
static double *cos_lookup_table_;
static double *sin_lookup_table_;
boost::asio::ip::udp::endpoint udp_listener_endpoint_;
boost::asio::ip::address source_address_filter_;
unsigned short source_port_filter_;
- boost::asio::io_service hdl_read_socket_service_;
+ boost::asio::io_service hdl_read_socket_service_;
boost::asio::ip::udp::socket *hdl_read_socket_;
std::string pcap_file_name_;
boost::thread *queue_consumer_thread_;
boost::thread *hdl_read_packet_thread_;
- HDLLaserCorrection laser_corrections_[HDL_MAX_NUM_LASERS];
bool terminate_read_packet_thread_;
- boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > current_scan_xyz_,
- current_sweep_xyz_;
- boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > current_scan_xyzi_,
- current_sweep_xyzi_;
- boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGBA> > current_scan_xyzrgb_,
- current_sweep_xyzrgb_;
- unsigned int last_azimuth_;
- boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyz>* sweep_xyz_signal_;
- boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb>* sweep_xyzrgb_signal_;
- boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzi>* sweep_xyzi_signal_;
- boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyz>* scan_xyz_signal_;
- boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb>* scan_xyzrgb_signal_;
- boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzi>* scan_xyzi_signal_;
pcl::RGB laser_rgb_mapping_[HDL_MAX_NUM_LASERS];
float min_distance_threshold_;
float max_distance_threshold_;
- void processVelodynePackets ();
- void enqueueHDLPacket (const unsigned char *data,
- std::size_t bytesReceived);
- void initialize (const std::string& correctionsFile);
- void loadCorrectionsFile (const std::string& correctionsFile);
- void loadHDL32Corrections ();
- void readPacketsFromSocket ();
+ virtual void
+ toPointClouds (HDLDataPacket *dataPacket);
+
+ virtual boost::asio::ip::address
+ getDefaultNetworkAddress ();
+
+ void
+ initialize (const std::string& correctionsFile = "");
+
+ void
+ processVelodynePackets ();
+
+ void
+ enqueueHDLPacket (const unsigned char *data,
+ std::size_t bytesReceived);
+
+ void
+ loadCorrectionsFile (const std::string& correctionsFile);
+
+ void
+ loadHDL32Corrections ();
+
+ void
+ readPacketsFromSocket ();
+
#ifdef HAVE_PCAP
- void readPacketsFromPcap();
+ void
+ readPacketsFromPcap();
+
#endif //#ifdef HAVE_PCAP
- void toPointClouds (HDLDataPacket *dataPacket);
- void fireCurrentSweep ();
- void fireCurrentScan (const unsigned short startAngle,
- const unsigned short endAngle);
- void computeXYZI (pcl::PointXYZI& pointXYZI, int azimuth,
- HDLLaserReturn laserReturn, HDLLaserCorrection correction);
- bool isAddressUnspecified (const boost::asio::ip::address& ip_address);
+
+ bool
+ isAddressUnspecified (const boost::asio::ip::address& ip_address);
+
};
}
* @param[in] input_height height of input image
* @param[in] output_width width of desired output image
* @param[in] output_height height of desired output image
- * @return wheter the resizing is supported or not.
+ * @return whether the resizing is supported or not.
*/
virtual bool
isResizingSupported (unsigned input_width, unsigned input_height,
typedef boost::chrono::high_resolution_clock::time_point Timestamp;
/** \brief Constructor
- * \param[in] depth_meta_data the actual data from the OpenNI library
+ * \param[in] depth_metadata the actual data from the OpenNI library
* \param[in] baseline the baseline of the "stereo" camera, i.e. the distance between the projector and the IR camera for
* Primesense like cameras. e.g. 7.5cm for PSDK5 and PSDK6 reference design.
* \param[in] focal_length focal length of the "stereo" frame.
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_IO_AUTO_IO_IMPL_H_
+#define PCL_IO_AUTO_IO_IMPL_H_
+
+// #include <pcl/io/file_io.h>
+// #include <pcl/io/boost.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/ifs_io.h>
+// #include <pcl/io/vtk_io.h>
+
+namespace pcl
+{
+ namespace io
+ {
+ template<typename PointT> int
+ load (const std::string& file_name, pcl::PointCloud<PointT>& cloud)
+ {
+ boost::filesystem::path p (file_name.c_str ());
+ std::string extension = p.extension ().string ();
+ int result = -1;
+ if (extension == ".pcd")
+ result = pcl::io::loadPCDFile (file_name, cloud);
+ else if (extension == ".ply")
+ result = pcl::io::loadPLYFile (file_name, cloud);
+ else if (extension == ".ifs")
+ result = pcl::io::loadIFSFile (file_name, cloud);
+ else
+ {
+ PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s", extension.c_str ());
+ result = -1;
+ }
+ return (result);
+ }
+
+ template<typename PointT> int
+ save (const std::string& file_name, const pcl::PointCloud<PointT>& cloud)
+ {
+ boost::filesystem::path p (file_name.c_str ());
+ std::string extension = p.extension ().string ();
+ int result = -1;
+ if (extension == ".pcd")
+ result = pcl::io::savePCDFile (file_name, cloud, true);
+ else if (extension == ".ply")
+ result = pcl::io::savePLYFile (file_name, cloud, true);
+ else if (extension == ".ifs")
+ result = pcl::io::saveIFSFile (file_name, cloud);
+ else
+ {
+ PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s", extension.c_str ());
+ result = -1;
+ }
+ return (result);
+ }
+ }
+}
+
+#endif //PCL_IO_AUTO_IO_IMPL_H_
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_IO_IMPL_BUFFERS_HPP
+#define PCL_IO_IMPL_BUFFERS_HPP
+
+#include <iostream>
+#include <cstring>
+
+#include <pcl/pcl_macros.h>
+
+template <typename T>
+struct buffer_traits
+{
+ static T invalid () { return 0; }
+ static bool is_invalid (T value) { return value == invalid (); };
+};
+
+template <>
+struct buffer_traits <float>
+{
+ static float invalid () { return std::numeric_limits<float>::quiet_NaN (); };
+ static bool is_invalid (float value) { return pcl_isnan (value); };
+};
+
+template <>
+struct buffer_traits <double>
+{
+ static double invalid () { return std::numeric_limits<double>::quiet_NaN (); };
+ static bool is_invalid (double value) { return pcl_isnan (value); };
+};
+
+template <typename T>
+pcl::io::Buffer<T>::Buffer (size_t size)
+: size_ (size)
+{
+}
+
+template <typename T>
+pcl::io::Buffer<T>::~Buffer ()
+{
+}
+
+template <typename T>
+pcl::io::SingleBuffer<T>::SingleBuffer (size_t size)
+: Buffer<T> (size)
+, data_ (size, buffer_traits<T>::invalid ())
+{
+}
+
+template <typename T>
+pcl::io::SingleBuffer<T>::~SingleBuffer ()
+{
+}
+
+template <typename T> T
+pcl::io::SingleBuffer<T>::operator[] (size_t idx) const
+{
+ assert (idx < size_);
+ return (data_[idx]);
+}
+
+template <typename T> void
+pcl::io::SingleBuffer<T>::push (std::vector<T>& data)
+{
+ assert (data.size () == size_);
+ boost::mutex::scoped_lock lock (data_mutex_);
+ data_.swap (data);
+ data.clear ();
+}
+
+template <typename T>
+pcl::io::MedianBuffer<T>::MedianBuffer (size_t size,
+ unsigned char window_size)
+: Buffer<T> (size)
+, window_size_ (window_size)
+, midpoint_ (window_size_ / 2)
+, data_current_idx_ (window_size_ - 1)
+{
+ assert (size_ > 0);
+ assert (window_size_ > 0);
+
+ data_.resize (window_size_);
+ for (size_t i = 0; i < window_size_; ++i)
+ data_[i].resize (size_, buffer_traits<T>::invalid ());
+
+ data_argsort_indices_.resize (size_);
+ for (size_t i = 0; i < size_; ++i)
+ {
+ data_argsort_indices_[i].resize (window_size_);
+ for (size_t j = 0; j < window_size_; ++j)
+ data_argsort_indices_[i][j] = j;
+ }
+
+ data_invalid_count_.resize (size_, window_size_);
+}
+
+template <typename T>
+pcl::io::MedianBuffer<T>::~MedianBuffer ()
+{
+}
+
+template <typename T> T
+pcl::io::MedianBuffer<T>::operator[] (size_t idx) const
+{
+ assert (idx < size_);
+ int midpoint = (window_size_ - data_invalid_count_[idx]) / 2;
+ return (data_[data_argsort_indices_[idx][midpoint]][idx]);
+}
+
+template <typename T> void
+pcl::io::MedianBuffer<T>::push (std::vector<T>& data)
+{
+ assert (data.size () == size_);
+ boost::mutex::scoped_lock lock (data_mutex_);
+
+ if (++data_current_idx_ >= window_size_)
+ data_current_idx_ = 0;
+
+ // New data will replace the column with index data_current_idx_. Before
+ // overwriting it, we go through all the new-old value pairs and update
+ // data_argsort_indices_ to maintain sorted order.
+ for (size_t i = 0; i < size_; ++i)
+ {
+ const T& new_value = data[i];
+ const T& old_value = data_[data_current_idx_][i];
+ bool new_is_invalid = buffer_traits<T>::is_invalid (new_value);
+ bool old_is_invalid = buffer_traits<T>::is_invalid (old_value);
+ if (compare (new_value, old_value) == 0)
+ continue;
+ std::vector<unsigned char>& argsort_indices = data_argsort_indices_[i];
+ // Rewrite the argsort indices before or after the position where we insert
+ // depending on the relation between the old and new values
+ if (compare (new_value, old_value) == 1)
+ {
+ for (int j = 0; j < window_size_; ++j)
+ if (argsort_indices[j] == data_current_idx_)
+ {
+ int k = j + 1;
+ while (k < window_size_ && compare (new_value, data_[argsort_indices[k]][i]) == 1)
+ {
+ std::swap (argsort_indices[k - 1], argsort_indices[k]);
+ ++k;
+ }
+ break;
+ }
+ }
+ else
+ {
+ for (int j = window_size_ - 1; j >= 0; --j)
+ if (argsort_indices[j] == data_current_idx_)
+ {
+ int k = j - 1;
+ while (k >= 0 && compare (new_value, data_[argsort_indices[k]][i]) == -1)
+ {
+ std::swap (argsort_indices[k], argsort_indices[k + 1]);
+ --k;
+ }
+ break;
+ }
+ }
+
+ if (new_is_invalid && !old_is_invalid)
+ ++data_invalid_count_[i];
+ else if (!new_is_invalid && old_is_invalid)
+ --data_invalid_count_[i];
+ }
+
+ // Finally overwrite the data
+ data_[data_current_idx_].swap (data);
+ data.clear ();
+}
+
+template <typename T> int
+pcl::io::MedianBuffer<T>::compare (T a, T b)
+{
+ bool a_is_invalid = buffer_traits<T>::is_invalid (a);
+ bool b_is_invalid = buffer_traits<T>::is_invalid (b);
+ if (a_is_invalid && b_is_invalid)
+ return 0;
+ if (a_is_invalid)
+ return 1;
+ if (b_is_invalid)
+ return -1;
+ if (a == b)
+ return 0;
+ return a > b ? 1 : -1;
+}
+
+template <typename T>
+pcl::io::AverageBuffer<T>::AverageBuffer (size_t size,
+ unsigned char window_size)
+: Buffer<T> (size)
+, window_size_ (window_size)
+, data_current_idx_ (window_size_ - 1)
+{
+ assert (size_ > 0);
+ assert (window_size_ > 0);
+
+ data_.resize (window_size_);
+ for (size_t i = 0; i < window_size_; ++i)
+ data_[i].resize (size_, buffer_traits<T>::invalid ());
+
+ data_sum_.resize (size_, 0);
+ data_invalid_count_.resize (size_, window_size_);
+}
+
+template <typename T>
+pcl::io::AverageBuffer<T>::~AverageBuffer ()
+{
+}
+
+template <typename T> T
+pcl::io::AverageBuffer<T>::operator[] (size_t idx) const
+{
+ assert (idx < size_);
+ if (data_invalid_count_[idx] == window_size_)
+ return (buffer_traits<T>::invalid ());
+ else
+ return (data_sum_[idx] / static_cast<T> (window_size_ - data_invalid_count_[idx]));
+}
+
+template <typename T> void
+pcl::io::AverageBuffer<T>::push (std::vector<T>& data)
+{
+ assert (data.size () == size_);
+ boost::mutex::scoped_lock lock (data_mutex_);
+
+ if (++data_current_idx_ >= window_size_)
+ data_current_idx_ = 0;
+
+ // New data will replace the column with index data_current_idx_. Before
+ // overwriting it, we go through the old values and subtract them from the
+ // data_sum_
+ for (size_t i = 0; i < size_; ++i)
+ {
+ const float& new_value = data[i];
+ const float& old_value = data_[data_current_idx_][i];
+ bool new_is_invalid = buffer_traits<T>::is_invalid (new_value);
+ bool old_is_invalid = buffer_traits<T>::is_invalid (old_value);
+
+ if (!old_is_invalid)
+ data_sum_[i] -= old_value;
+ if (!new_is_invalid)
+ data_sum_[i] += new_value;
+
+ if (new_is_invalid && !old_is_invalid)
+ ++data_invalid_count_[i];
+ else if (!new_is_invalid && old_is_invalid)
+ --data_invalid_count_[i];
+ }
+
+ // Finally overwrite the data
+ data_[data_current_idx_].swap (data);
+ data.clear ();
+}
+
+#endif /* PCL_IO_IMPL_BUFFERS_HPP */
+
cloud.height = getHeight ();
cloud.is_dense = true;
cloud.resize (getWidth () * getHeight ());
- register int depth_idx = 0, point_idx = 0;
+ int depth_idx = 0, point_idx = 0;
double constant_x = 1.0 / parameters_.focal_length_x,
constant_y = 1.0 / parameters_.focal_length_y;
for (uint32_t v = 0; v < cloud.height; ++v)
{
- for (register uint32_t u = 0; u < cloud.width; ++u, ++point_idx, depth_idx += 2)
+ for (uint32_t u = 0; u < cloud.width; ++u, ++point_idx, depth_idx += 2)
{
PointT &pt = cloud.points[point_idx];
unsigned short val;
cloud.height = getHeight ();
cloud.resize (getWidth () * getHeight ());
- register int rgb_idx = 0;
+ int rgb_idx = 0;
unsigned char *color_r = reinterpret_cast<unsigned char*> (&uncompressed_data[0]);
unsigned char *color_g = reinterpret_cast<unsigned char*> (&uncompressed_data[getWidth () * getHeight ()]);
unsigned char *color_b = reinterpret_cast<unsigned char*> (&uncompressed_data[2 * getWidth () * getHeight ()]);
unsigned char *color_y = reinterpret_cast<unsigned char*> (&uncompressed_data[wh2]);
unsigned char *color_v = reinterpret_cast<unsigned char*> (&uncompressed_data[wh2 + getWidth () * getHeight ()]);
- register int y_idx = 0;
+ int y_idx = 0;
for (int i = 0; i < wh2; ++i, y_idx += 2)
{
int v = color_v[i] - 128;
cloud.width = getWidth ();
cloud.height = getHeight ();
cloud.resize (getWidth () * getHeight ());
- register int rgb_idx = 0;
+ int rgb_idx = 0;
for (size_t i = 0; i < cloud.size (); ++i, rgb_idx += 3)
{
PointT &pt = cloud.points[i];
#include <cstdlib>
#include <pcl/common/io.h>
+#include <pcl/common/colors.h>
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
return (true);
}
-// Lookup table is copied from (excluding the first entry):
-// https://github.com/fiji/fiji/blob/master/luts/glasbey.lut
-const uint8_t GLASBEY_LUT[] =
-{
- 0, 0, 255,
- 255, 0, 0,
- 0, 255, 0,
- 0, 0, 51,
- 255, 0, 182,
- 0, 83, 0,
- 255, 211, 0,
- 0, 159, 255,
- 154, 77, 66,
- 0, 255, 190,
- 120, 63, 193,
- 31, 150, 152,
- 255, 172, 253,
- 177, 204, 113,
- 241, 8, 92,
- 254, 143, 66,
- 221, 0, 255,
- 32, 26, 1,
- 114, 0, 85,
- 118, 108, 149,
- 2, 173, 36,
- 200, 255, 0,
- 136, 108, 0,
- 255, 183, 159,
- 133, 133, 103,
- 161, 3, 0,
- 20, 249, 255,
- 0, 71, 158,
- 220, 94, 147,
- 147, 212, 255,
- 0, 76, 255,
- 0, 66, 80,
- 57, 167, 106,
- 238, 112, 254,
- 0, 0, 100,
- 171, 245, 204,
- 161, 146, 255,
- 164, 255, 115,
- 255, 206, 113,
- 71, 0, 21,
- 212, 173, 197,
- 251, 118, 111,
- 171, 188, 0,
- 117, 0, 215,
- 166, 0, 154,
- 0, 115, 254,
- 165, 93, 174,
- 98, 132, 2,
- 0, 121, 168,
- 0, 255, 131,
- 86, 53, 0,
- 159, 0, 63,
- 66, 45, 66,
- 255, 242, 187,
- 0, 93, 67,
- 252, 255, 124,
- 159, 191, 186,
- 167, 84, 19,
- 74, 39, 108,
- 0, 16, 166,
- 145, 78, 109,
- 207, 149, 0,
- 195, 187, 255,
- 253, 68, 64,
- 66, 78, 32,
- 106, 1, 0,
- 181, 131, 84,
- 132, 233, 147,
- 96, 217, 0,
- 255, 111, 211,
- 102, 75, 63,
- 254, 100, 0,
- 228, 3, 127,
- 17, 199, 174,
- 210, 129, 139,
- 91, 118, 124,
- 32, 59, 106,
- 180, 84, 255,
- 226, 8, 210,
- 0, 1, 20,
- 93, 132, 68,
- 166, 250, 255,
- 97, 123, 201,
- 98, 0, 122,
- 126, 190, 58,
- 0, 60, 183,
- 255, 253, 0,
- 7, 197, 226,
- 180, 167, 57,
- 148, 186, 138,
- 204, 187, 160,
- 55, 0, 49,
- 0, 40, 1,
- 150, 122, 129,
- 39, 136, 38,
- 206, 130, 180,
- 150, 164, 196,
- 180, 32, 128,
- 110, 86, 180,
- 147, 0, 185,
- 199, 48, 61,
- 115, 102, 255,
- 15, 187, 253,
- 172, 164, 100,
- 182, 117, 250,
- 216, 220, 254,
- 87, 141, 113,
- 216, 85, 34,
- 0, 196, 103,
- 243, 165, 105,
- 216, 255, 182,
- 1, 24, 219,
- 52, 66, 54,
- 255, 154, 0,
- 87, 95, 1,
- 198, 241, 79,
- 255, 95, 133,
- 123, 172, 240,
- 120, 100, 49,
- 162, 133, 204,
- 105, 255, 220,
- 198, 82, 100,
- 121, 26, 64,
- 0, 238, 70,
- 231, 207, 69,
- 217, 128, 233,
- 255, 211, 209,
- 209, 255, 141,
- 36, 0, 3,
- 87, 163, 193,
- 211, 231, 201,
- 203, 111, 79 ,
- 62, 24, 0,
- 0, 117, 223,
- 112, 176, 88 ,
- 209, 24, 0,
- 0, 30, 107,
- 105, 200, 197,
- 255, 203, 255,
- 233, 194, 137,
- 191, 129, 46,
- 69, 42, 145,
- 171, 76, 194,
- 14, 117, 61,
- 0, 30, 25,
- 118, 73, 127,
- 255, 169, 200,
- 94, 55, 217,
- 238, 230, 138,
- 159, 54, 33,
- 80, 0, 148,
- 189, 144, 128,
- 0, 109, 126,
- 88, 223, 96,
- 71, 80, 103,
- 1, 93, 159,
- 99, 48, 60,
- 2, 206, 148,
- 139, 83, 37,
- 171, 0, 255,
- 141, 42, 135,
- 85, 83, 148,
- 150, 255, 0,
- 0, 152, 123,
- 255, 138, 203,
- 222, 69, 200,
- 107, 109, 230,
- 30, 0, 68,
- 173, 76, 138,
- 255, 134, 161,
- 0, 35, 60,
- 138, 205, 0,
- 111, 202, 157,
- 225, 75, 253,
- 255, 176, 77,
- 229, 232, 57,
- 114, 16, 255,
- 111, 82, 101,
- 134, 137, 48,
- 99, 38, 80,
- 105, 38, 32,
- 200, 110, 0,
- 209, 164, 255,
- 198, 210, 86,
- 79, 103, 77,
- 174, 165, 166,
- 170, 45, 101,
- 199, 81, 175,
- 255, 89, 172,
- 146, 102, 78,
- 102, 134, 184,
- 111, 152, 255,
- 92, 255, 159,
- 172, 137, 178,
- 210, 34, 98,
- 199, 207, 147,
- 255, 185, 30,
- 250, 148, 141,
- 49, 34, 78,
- 254, 81, 97,
- 254, 141, 100,
- 68, 54, 23,
- 201, 162, 84,
- 199, 232, 240,
- 68, 152, 0,
- 147, 172, 58,
- 22, 75, 28,
- 8, 84, 121,
- 116, 45, 0,
- 104, 60, 255,
- 64, 41, 38,
- 164, 113, 215,
- 207, 0, 155,
- 118, 1, 35,
- 83, 0, 88,
- 0, 82, 232,
- 43, 92, 87,
- 160, 217, 146,
- 176, 26, 229,
- 29, 3, 36,
- 122, 58, 159,
- 214, 209, 207,
- 160, 100, 105,
- 106, 157, 160,
- 153, 219, 113,
- 192, 56, 207,
- 125, 255, 89,
- 149, 0, 34,
- 213, 162, 223,
- 22, 131, 204,
- 166, 249, 69,
- 109, 105, 97,
- 86, 188, 78,
- 255, 109, 81,
- 255, 3, 248,
- 255, 0, 73,
- 202, 0, 35,
- 67, 109, 18,
- 234, 170, 173,
- 191, 165, 0,
- 38, 44, 51,
- 85, 185, 2,
- 121, 182, 158,
- 254, 236, 212,
- 139, 165, 89,
- 141, 254, 193,
- 0, 60, 43,
- 63, 17, 40,
- 255, 221, 246,
- 17, 26, 146,
- 154, 66, 84,
- 149, 157, 238,
- 126, 130, 72,
- 58, 6, 101,
- 189, 117, 101,
-};
-
-const size_t GLASBEY_LUT_SIZE = sizeof (GLASBEY_LUT) / sizeof (GLASBEY_LUT[0]);
-
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::io::PointCloudImageExtractorFromLabelField<PointT>::extractImpl (const PointCloud& cloud, pcl::PCLImage& img) const
// First pass: find unique labels
for (size_t i = 0; i < cloud.points.size (); ++i)
{
+ // If we need to paint NaN points with black do not waste colors on them
+ if (paint_nans_with_black_ && !pcl::isFinite (cloud.points[i]))
+ continue;
uint32_t val;
pcl::getFieldValue<PointT, uint32_t> (cloud.points[i], offset, val);
labels.insert (val);
}
// Assign Glasbey colors in ascending order of labels
+ // Note: the color LUT has a finite size (256 colors), therefore when
+ // there are more labels the colors will repeat
size_t color = 0;
for (std::set<uint32_t>::iterator iter = labels.begin (); iter != labels.end (); ++iter)
{
- colormap[*iter] = color % GLASBEY_LUT_SIZE;
+ colormap[*iter] = color % GlasbeyLUT::size ();
++color;
}
{
uint32_t val;
pcl::getFieldValue<PointT, uint32_t> (cloud.points[i], offset, val);
- memcpy (&img.data[i * 3], &GLASBEY_LUT[colormap[val] * 3], 3);
+ memcpy (&img.data[i * 3], GlasbeyLUT::data () + colormap[val] * 3, 3);
}
break;
* @param[in] input_height height of input image
* @param[in] output_width width of desired output image
* @param[in] output_height height of desired output image
- * @return wheter the resizing is supported or not.
+ * @return whether the resizing is supported or not.
*/
virtual bool isResizingSupported (unsigned input_width, unsigned input_height,
unsigned output_width, unsigned output_height) const = 0;
private:
virtual void
- publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const = 0;
+ publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation, const std::string& file_name) const = 0;
// to separate and hide the implementation from interface: PIMPL
struct PCDGrabberImpl;
PCDGrabber (const std::vector<std::string>& pcd_files, float frames_per_second = 0, bool repeat = false);
/** \brief Virtual destructor. */
- virtual ~PCDGrabber () throw () {}
+ virtual ~PCDGrabber () throw ()
+ {
+ stop ();
+ }
// Inherited from FileGrabber
const boost::shared_ptr< const pcl::PointCloud<PointT> >
protected:
virtual void
- publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const;
+ publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation, const std::string& file_name) const;
boost::signals2::signal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>* signal_;
+ boost::signals2::signal<void (const std::string&)>* file_name_signal_;
#ifdef HAVE_OPENNI
boost::signals2::signal<void (const boost::shared_ptr<openni_wrapper::DepthImage>&)>* depth_image_signal_;
: PCDGrabberBase (pcd_path, frames_per_second, repeat)
{
signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
+ file_name_signal_ = createSignal<void (const std::string&)>();
#ifdef HAVE_OPENNI
depth_image_signal_ = createSignal <void (const boost::shared_ptr<openni_wrapper::DepthImage>&)> ();
image_signal_ = createSignal <void (const boost::shared_ptr<openni_wrapper::Image>&)> ();
: PCDGrabberBase (pcd_files, frames_per_second, repeat), signal_ ()
{
signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
+ file_name_signal_ = createSignal<void (const std::string&)>();
#ifdef HAVE_OPENNI
depth_image_signal_ = createSignal <void (const boost::shared_ptr<openni_wrapper::DepthImage>&)> ();
image_signal_ = createSignal <void (const boost::shared_ptr<openni_wrapper::Image>&)> ();
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> void
- PCDGrabber<PointT>::publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const
+ PCDGrabber<PointT>::publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation, const std::string& file_name) const
{
typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ());
pcl::fromPCLPointCloud2 (blob, *cloud);
cloud->sensor_orientation_ = orientation;
signal_->operator () (cloud);
+ if (file_name_signal_->num_slots() > 0)
+ file_name_signal_->operator()(file_name);
#ifdef HAVE_OPENNI
// If dataset is not organized, return
# error
#endif
-#define PLY_TYPE_TRAITS(TYPE, NAME, OLD_NAME) \
+#define PLY_TYPE_TRAITS(TYPE, PARSE_TYPE, NAME, OLD_NAME) \
template <> \
struct type_traits<TYPE> \
{ \
typedef TYPE type; \
- static const char* name() { return NAME; } \
- static const char* old_name() { return OLD_NAME; } \
- }
+ typedef PARSE_TYPE parse_type; \
+ static const char* name () { return NAME; } \
+ static const char* old_name () { return OLD_NAME; } \
+ };
+
+ PLY_TYPE_TRAITS(int8, int16, "int8", "char");
+ PLY_TYPE_TRAITS(int16, int16, "int16", "short");
+ PLY_TYPE_TRAITS(int32, int32, "int32", "int");
+ PLY_TYPE_TRAITS(uint8, uint16, "uint8", "uchar");
+ PLY_TYPE_TRAITS(uint16, uint16, "uint16", "ushort");
+ PLY_TYPE_TRAITS(uint32, uint32, "uint32", "uint");
+ PLY_TYPE_TRAITS(float32, float32, "float32", "float");
+ PLY_TYPE_TRAITS(float64, float64, "float64", "double");
- PLY_TYPE_TRAITS(int8, "int8", "char");
- PLY_TYPE_TRAITS(int16, "int16", "short");
- PLY_TYPE_TRAITS(int32, "int32", "int");
- PLY_TYPE_TRAITS(uint8, "uint8", "uchar");
- PLY_TYPE_TRAITS(uint16, "uint16", "ushort");
- PLY_TYPE_TRAITS(uint32, "uint32", "uint");
- PLY_TYPE_TRAITS(float32, "float32", "float");
- PLY_TYPE_TRAITS(float64, "float64", "double");
#undef PLY_TYPE_TRAITS
typedef ScalarType scalar_type;
if (format == ascii_format)
{
- scalar_type value = std::numeric_limits<scalar_type>::quiet_NaN ();
+ std::string value_s;
+ scalar_type value;
char space = ' ';
- istream >> value;
+ istream >> value_s;
+ try
+ {
+ value = static_cast<scalar_type> (boost::lexical_cast<typename pcl::io::ply::type_traits<scalar_type>::parse_type> (value_s));
+ }
+ catch (boost::bad_lexical_cast &)
+ {
+ value = std::numeric_limits<scalar_type>::quiet_NaN ();
+ }
+
if (!istream.eof ())
istream >> space >> std::ws;
if (!istream || !isspace (space))
}
for (std::size_t index = 0; index < size; ++index)
{
- scalar_type value = std::numeric_limits<scalar_type>::quiet_NaN ();
+ std::string value_s;
+ scalar_type value;
char space = ' ';
- istream >> value;
+ istream >> value_s;
+ try
+ {
+ value = static_cast<scalar_type> (boost::lexical_cast<typename pcl::io::ply::type_traits<scalar_type>::parse_type> (value_s));
+ }
+ catch (boost::bad_lexical_cast &)
+ {
+ value = std::numeric_limits<scalar_type>::quiet_NaN ();
+ }
+
if (!istream.eof ())
{
istream >> space >> std::ws;
virtual bool
extractImpl (const PointCloud& cloud, pcl::PCLImage& img) const;
+ // Members derived from the base class
+ using PointCloudImageExtractor<PointT>::paint_nans_with_black_;
+
private:
ColorMode color_mode_;
-/*
- * Software License Agreement (BSD License)
- *
- * Point Cloud Library (PCL) - www.pointclouds.org
- * Copyright (c) 2012-, Open Perception, Inc.
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the copyright holder(s) nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- */
-
-#include <pcl/pcl_config.h>
-#define HAVE_PXCAPI
-#ifdef HAVE_PXCAPI
-
-#ifndef __PCL_IO_PXC_GRABBER__
-#define __PCL_IO_PXC_GRABBER__
-
-#include <pcl/io/eigen.h>
-#include <pcl/io/boost.h>
-#include <pcl/io/grabber.h>
-#include <pcl/common/synchronizer.h>
-
-#include <boost/thread.hpp>
-
-#include "pxcsmartptr.h"
-#include "pxcsession.h"
-#include "util_capture.h"
-#include "util_pipeline.h"
-//#include "util_pipeline_raw.h"
-#include "util_render.h"
-#include <util_capture.h>
-#include <pxcprojection.h>
-#include <pxcmetadata.h>
-
-#include <string>
-#include <deque>
-
-namespace pcl
-{
- struct PointXYZ;
- struct PointXYZRGB;
- struct PointXYZRGBA;
- struct PointXYZI;
- template <typename T> class PointCloud;
-
-
- /** \brief Grabber for PXC devices
- * \author Stefan Holzer <holzers@in.tum.de>
- * \ingroup io
- */
- class PCL_EXPORTS PXCGrabber : public Grabber
- {
- public:
-
- /** \brief Supported modes for grabbing from a PXC device. */
- typedef enum
- {
- PXC_Default_Mode = 0,
- } Mode;
-
- //define callback signature typedefs
- typedef void (sig_cb_pxc_point_cloud) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&);
- typedef void (sig_cb_pxc_point_cloud_rgb) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGB> >&);
- typedef void (sig_cb_pxc_point_cloud_rgba) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&);
- typedef void (sig_cb_pxc_point_cloud_i) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&);
-
- public:
- /** \brief Constructor */
- PXCGrabber ();
-
- /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
- virtual ~PXCGrabber () throw ();
-
- /** \brief Start the data acquisition. */
- virtual void
- start ();
-
- /** \brief Stop the data acquisition. */
- virtual void
- stop ();
-
- /** \brief Check if the data acquisition is still running. */
- virtual bool
- isRunning () const;
-
- /** \brief Returns the name of the grabber. */
- virtual std::string
- getName () const;
-
- /** \brief Obtain the number of frames per second (FPS). */
- virtual float
- getFramesPerSecond () const;
-
- protected:
-
- /** \brief Initializes the PXC grabber and the grabbing pipeline. */
- bool
- init ();
-
- /** \brief Closes the grabbing pipeline. */
- void
- close ();
-
- /** \brief Continously asks for data from the device and publishes it if available. */
- void
- processGrabbing ();
-
- // signals to indicate whether new clouds are available
- boost::signals2::signal<sig_cb_pxc_point_cloud>* point_cloud_signal_;
- //boost::signals2::signal<sig_cb_fotonic_point_cloud_i>* point_cloud_i_signal_;
- boost::signals2::signal<sig_cb_pxc_point_cloud_rgb>* point_cloud_rgb_signal_;
- boost::signals2::signal<sig_cb_pxc_point_cloud_rgba>* point_cloud_rgba_signal_;
-
- protected:
- // utiliy object for accessing PXC camera
- UtilPipeline pp_;
- // indicates whether grabbing is running
- bool running_;
-
- // FPS computation
- mutable float fps_;
- mutable boost::mutex fps_mutex_;
-
- // thread where the grabbing takes place
- boost::thread grabber_thread_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- };
-
-} // namespace pcl
-#endif // __PCL_IO_PXC_GRABBER__
-#endif // HAVE_PXCAPI
+#error "PXCGrabber was deprecated and removed, please use DepthSenseGrabber instead"
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2015-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_IO_REAL_SENSE_DEVICE_MANAGER_H
+#define PCL_IO_REAL_SENSE_DEVICE_MANAGER_H
+
+#include <boost/thread.hpp>
+#include <boost/utility.hpp>
+#include <boost/weak_ptr.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/thread/mutex.hpp>
+
+#include <pcl/pcl_exports.h>
+
+#include <pxcsession.h>
+#include <pxccapture.h>
+#include <pxccapturemanager.h>
+
+namespace pcl
+{
+
+ class RealSenseGrabber;
+
+ namespace io
+ {
+
+ namespace real_sense
+ {
+
+ class RealSenseDevice;
+
+ class PCL_EXPORTS RealSenseDeviceManager : boost::noncopyable
+ {
+
+ public:
+
+ typedef boost::shared_ptr<RealSenseDeviceManager> Ptr;
+
+ static Ptr&
+ getInstance ()
+ {
+ static Ptr instance;
+ if (!instance)
+ {
+ boost::mutex::scoped_lock lock (mutex_);
+ if (!instance)
+ instance.reset (new RealSenseDeviceManager);
+ }
+ return (instance);
+ }
+
+ inline size_t
+ getNumDevices ()
+ {
+ return (device_list_.size ());
+ }
+
+ boost::shared_ptr<RealSenseDevice>
+ captureDevice ();
+
+ boost::shared_ptr<RealSenseDevice>
+ captureDevice (size_t index);
+
+ boost::shared_ptr<RealSenseDevice>
+ captureDevice (const std::string& sn);
+
+ ~RealSenseDeviceManager ();
+
+ private:
+
+ struct DeviceInfo
+ {
+ pxcUID iuid;
+ pxcI32 didx;
+ std::string serial;
+ boost::weak_ptr<RealSenseDevice> device_ptr;
+ inline bool isCaptured () { return (!device_ptr.expired ()); }
+ };
+
+ /** If the device is already captured returns a pointer. */
+ boost::shared_ptr<RealSenseDevice>
+ capture (DeviceInfo& device_info);
+
+ RealSenseDeviceManager ();
+
+ /** This function discovers devices that are capable of streaming
+ * depth data. */
+ void
+ populateDeviceList ();
+
+ boost::shared_ptr<PXCSession> session_;
+ boost::shared_ptr<PXCCaptureManager> capture_manager_;
+
+ std::vector<DeviceInfo> device_list_;
+
+ static boost::mutex mutex_;
+
+ };
+
+ class PCL_EXPORTS RealSenseDevice : boost::noncopyable
+ {
+
+ public:
+
+ typedef boost::shared_ptr<RealSenseDevice> Ptr;
+
+ inline const std::string&
+ getSerialNumber () { return (device_id_); }
+
+ inline PXCCapture::Device&
+ getPXCDevice () { return (*device_); }
+
+ /** Reset the state of given device by releasing and capturing again. */
+ static void
+ reset (RealSenseDevice::Ptr& device)
+ {
+ std::string id = device->getSerialNumber ();
+ device.reset ();
+ device = RealSenseDeviceManager::getInstance ()->captureDevice (id);
+ }
+
+ private:
+
+ friend class RealSenseDeviceManager;
+
+ std::string device_id_;
+ boost::shared_ptr<PXCCapture> capture_;
+ boost::shared_ptr<PXCCapture::Device> device_;
+
+ RealSenseDevice (const std::string& id) : device_id_ (id) { };
+
+ };
+
+ } // namespace real_sense
+
+ } // namespace io
+
+} // namespace pcl
+
+#endif /* PCL_IO_REAL_SENSE_DEVICE_MANAGER_H */
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2015-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_IO_REAL_SENSE_GRABBER_H
+#define PCL_IO_REAL_SENSE_GRABBER_H
+
+#include <boost/thread/thread.hpp>
+#include <boost/thread/mutex.hpp>
+
+#include <pcl/io/grabber.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/common/time.h>
+
+namespace pcl
+{
+
+ namespace io
+ {
+
+ template <typename T> class Buffer;
+
+ namespace real_sense
+ {
+ class RealSenseDevice;
+ }
+
+ }
+
+ class PCL_EXPORTS RealSenseGrabber : public Grabber
+ {
+
+ public:
+
+ typedef
+ void (sig_cb_real_sense_point_cloud)
+ (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&);
+
+ typedef
+ void (sig_cb_real_sense_point_cloud_rgba)
+ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&);
+
+ /** A descriptor for capturing mode.
+ *
+ * Consists of framerate and resolutions of depth and color streams.
+ * Serves two purposes: to describe the desired capturing mode when
+ * creating a grabber, and to list the available modes supported by the
+ * grabber (see getAvailableModes()). In the first case setting some
+ * fields to zero means "don't care", i.e. the grabber is allowed to
+ * decide itself which concrete values to use. */
+ struct PCL_EXPORTS Mode
+ {
+ unsigned int fps;
+ unsigned int depth_width;
+ unsigned int depth_height;
+ unsigned int color_width;
+ unsigned int color_height;
+
+ /** Set all fields to zero (i.e. "don't care"). */
+ Mode ();
+
+ /** Set desired framerate, the rest is "don't care". */
+ Mode (unsigned int fps);
+
+ /** Set desired depth resolution, the rest is "don't care". */
+ Mode (unsigned int depth_width, unsigned int depth_height);
+
+ /** Set desired framerate and depth resolution, the rest is "don't
+ * care". */
+ Mode (unsigned int fps, unsigned int depth_width, unsigned int depth_height);
+
+ /** Set desired depth and color resolution, the rest is "don't
+ * care". */
+ Mode (unsigned int depth_width, unsigned int depth_height, unsigned int color_width, unsigned int color_height);
+
+ /** Set desired framerate, depth and color resolution. */
+ Mode (unsigned int fps, unsigned int depth_width, unsigned int depth_height, unsigned int color_width, unsigned int color_height);
+ };
+
+ enum TemporalFilteringType
+ {
+ RealSense_None = 0,
+ RealSense_Median = 1,
+ RealSense_Average = 2,
+ };
+
+ /** Create a grabber for a RealSense device.
+ *
+ * The grabber "captures" the device, making it impossible for other
+ * grabbers to interact with it. The device is "released" when the
+ * grabber is destructed.
+ *
+ * This will throw pcl::io::IOException if there are no free devices
+ * that match the supplied \a device_id.
+ *
+ * \param[in] device_id device identifier, which can be a serial number,
+ * a zero-based index (with '#' prefix), or an empty string (to select
+ * the first available device)
+ * \param[in] mode desired framerate and stream resolution (see Mode).
+ * If the default is supplied, then the mode closest to VGA at 30 Hz
+ * will be chosen.
+ * \param[in] strict if set to \c true, an exception will be thrown if
+ * device does not support exactly the mode requsted. Otherwise the
+ * closest available mode is selected. */
+ RealSenseGrabber (const std::string& device_id = "", const Mode& mode = Mode (), bool strict = false);
+
+ virtual
+ ~RealSenseGrabber () throw ();
+
+ virtual void
+ start ();
+
+ virtual void
+ stop ();
+
+ virtual bool
+ isRunning () const;
+
+ virtual std::string
+ getName () const
+ {
+ return (std::string ("RealSenseGrabber"));
+ }
+
+ virtual float
+ getFramesPerSecond () const;
+
+ /** Set the confidence threshold for depth data.
+ *
+ * Valid range is [0..15]. Discarded points will have their coordinates
+ * set to NaNs). */
+ void
+ setConfidenceThreshold (unsigned int threshold);
+
+ /** Enable temporal filtering of the depth data received from the device.
+ *
+ * The window size parameter is not relevant for `RealSense_None`
+ * filtering type.
+ *
+ * \note if the grabber is running and the new parameters are different
+ * from the current parameters, grabber will be restarted. */
+ void
+ enableTemporalFiltering (TemporalFilteringType type, size_t window_size);
+
+ /** Disable temporal filtering. */
+ void
+ disableTemporalFiltering ();
+
+ /** Get the serial number of device captured by the grabber. */
+ const std::string&
+ getDeviceSerialNumber () const;
+
+ /** Get a list of capturing modes supported by the PXC device
+ * controlled by this grabber.
+ *
+ * \param[in] only_depth list depth-only modes
+ *
+ * \note: this list exclude modes where framerates of the depth and
+ * color streams do not match. */
+ std::vector<Mode>
+ getAvailableModes (bool only_depth = false) const;
+
+ /** Set desired capturing mode.
+ *
+ * \note if the grabber is running and the new mode is different the
+ * one requested previously, grabber will be restarted. */
+ void
+ setMode (const Mode& mode, bool strict = false);
+
+ /** Get currently active capturing mode.
+ *
+ * \note: capturing mode is selected when start() is called; output of
+ * this function before grabber was started is undefined. */
+ const Mode&
+ getMode () const
+ {
+ return (mode_selected_);
+ }
+
+ private:
+
+ void
+ run ();
+
+ void
+ createDepthBuffer ();
+
+ void
+ selectMode ();
+
+ /** Compute a score which indicates how different is a given mode is from
+ * the mode requested by the user.
+ *
+ * Importance of factors: fps > depth resolution > color resolution. The
+ * lower the score the better. */
+ float
+ computeModeScore (const Mode& mode);
+
+ // Signals to indicate whether new clouds are available
+ boost::signals2::signal<sig_cb_real_sense_point_cloud>* point_cloud_signal_;
+ boost::signals2::signal<sig_cb_real_sense_point_cloud_rgba>* point_cloud_rgba_signal_;
+
+ boost::shared_ptr<pcl::io::real_sense::RealSenseDevice> device_;
+
+ bool is_running_;
+ unsigned int confidence_threshold_;
+
+ TemporalFilteringType temporal_filtering_type_;
+ size_t temporal_filtering_window_size_;
+
+ /// Capture mode requested by the user at construction time
+ Mode mode_requested_;
+
+ /// Whether or not selected capture mode should strictly match what the user
+ /// has requested
+ bool strict_;
+
+ /// Capture mode selected by grabber (among the modes supported by the
+ /// device), computed and stored on start()
+ Mode mode_selected_;
+
+ /// Indicates whether there are subscribers for PointXYZ signal, computed
+ /// and stored on start()
+ bool need_xyz_;
+
+ /// Indicates whether there are subscribers for PointXYZRGBA signal,
+ /// computed and stored on start()
+ bool need_xyzrgba_;
+
+ EventFrequency frequency_;
+ mutable boost::mutex fps_mutex_;
+
+ boost::thread thread_;
+
+ /// Depth buffer to perform temporal filtering of the depth images
+ boost::shared_ptr<pcl::io::Buffer<unsigned short> > depth_buffer_;
+
+ };
+
+}
+
+bool
+operator== (const pcl::RealSenseGrabber::Mode& m1, const pcl::RealSenseGrabber::Mode& m2);
+
+#endif /* PCL_IO_REAL_SENSE_GRABBER_H */
+
bool terminate_thread_;
size_t signal_point_cloud_size_;
unsigned short data_port_;
- unsigned char receive_buffer_[500];
+ enum { MAX_LENGTH = 65535 };
+ unsigned char receive_buffer_[MAX_LENGTH];
+ unsigned int data_size_;
boost::asio::ip::address sensor_address_;
boost::asio::ip::udp::endpoint sender_endpoint_;
void socketThreadLoop ();
void asyncSocketReceive ();
void resetPointCloud ();
- void socketCallback (const boost::system::error_code& error, std::size_t numberOfBytes);
- void convertPacketData (unsigned char *dataPacket, size_t length);
- void computeXYZI (pcl::PointXYZI& pointXYZI, unsigned char* pointData);
+ void socketCallback (const boost::system::error_code& error, std::size_t number_of_bytes);
+ void convertPacketData (unsigned char *data_packet, size_t length);
+ void computeXYZI (pcl::PointXYZI& point_XYZI, unsigned char* point_data);
+ void computeTimestamp (boost::uint32_t& timestamp, unsigned char* point_data);
};
}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2015-, Open Perception, Inc.
+ * Copyright (c) 2015 The MITRE Corporation
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "pcl/pcl_config.h"
+
+#ifndef PCL_IO_VLP_GRABBER_H_
+#define PCL_IO_VLP_GRABBER_H_
+
+#include <pcl/io/hdl_grabber.h>
+#include <pcl/io/grabber.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <boost/asio.hpp>
+#include <string>
+
+namespace pcl
+{
+
+ /** \brief Grabber for the Velodyne LiDAR (VLP), based on the Velodyne High Definition Laser (HDL)
+ * \author Keven Ring <keven@mitre.org>
+ * \ingroup io
+ */
+ class PCL_EXPORTS VLPGrabber : public HDLGrabber
+ {
+ public:
+ /** \brief Constructor taking an optional path to an vlp corrections file. The Grabber will listen on the default IP/port for data packets [192.168.3.255/2368]
+ * \param[in] pcapFile Path to a file which contains previously captured data packets. This parameter is optional
+ */
+ VLPGrabber (const std::string& pcapFile = "");
+
+ /** \brief Constructor taking a specified IP/port
+ * \param[in] ipAddress IP Address that should be used to listen for VLP packets
+ * \param[in] port UDP Port that should be used to listen for VLP packets
+ */
+ VLPGrabber (const boost::asio::ip::address& ipAddress,
+ const unsigned short port);
+
+ /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
+ virtual
+ ~VLPGrabber () throw ();
+
+ /** \brief Obtains the name of this I/O Grabber
+ * \return The name of the grabber
+ */
+ virtual std::string
+ getName () const;
+
+ private:
+ virtual void
+ toPointClouds (HDLDataPacket *dataPacket);
+
+ boost::asio::ip::address
+ getDefaultNetworkAddress ();
+
+ void
+ loadVLP16Corrections ();
+
+ };
+}
+
+#endif /* PCL_IO_VLP_GRABBER_H_ */
/** \brief Save a \ref PolygonMesh object given an input file name, based on the file extension
* \param[in] file_name the name of the file to save the data to
* \param[in] mesh the object that contains the data
+ * \param[in] binary_format if true, exported file is in binary format
+ * \return True if successful, false otherwise
* \ingroup io
*/
- PCL_EXPORTS int
+ PCL_EXPORTS bool
savePolygonFile (const std::string &file_name,
- const pcl::PolygonMesh& mesh);
+ const pcl::PolygonMesh& mesh,
+ const bool binary_format = true);
/** \brief Load a VTK file into a \ref PolygonMesh object
* \param[in] file_name the name of the file that contains the data
/** \brief Save a \ref PolygonMesh object into a VTK file
* \param[in] file_name the name of the file to save the data to
* \param[in] mesh the object that contains the data
+ * \param[in] binary_format if true, exported file is in binary format
+ * \return True if successful, false otherwise
* \ingroup io
*/
- PCL_EXPORTS int
+ PCL_EXPORTS bool
savePolygonFileVTK (const std::string &file_name,
- const pcl::PolygonMesh& mesh);
+ const pcl::PolygonMesh& mesh,
+ const bool binary_format = true);
/** \brief Save a \ref PolygonMesh object into a PLY file
* \param[in] file_name the name of the file to save the data to
* \param[in] mesh the object that contains the data
+ * \param[in] binary_format if true, exported file is in binary format
+ * \return True if successful, false otherwise
* \ingroup io
*/
- PCL_EXPORTS int
+ PCL_EXPORTS bool
savePolygonFilePLY (const std::string &file_name,
- const pcl::PolygonMesh& mesh);
+ const pcl::PolygonMesh& mesh,
+ const bool binary_format = true);
/** \brief Save a \ref PolygonMesh object into an STL file
* \param[in] file_name the name of the file to save the data to
* \param[in] mesh the object that contains the data
+ * \param[in] binary_format if true, exported file is in binary format
+ * \return True if successful, false otherwise
* \ingroup io
*/
- PCL_EXPORTS int
+ PCL_EXPORTS bool
savePolygonFileSTL (const std::string &file_name,
- const pcl::PolygonMesh& mesh);
+ const pcl::PolygonMesh& mesh,
+ const bool binary_format = true);
/** \brief Write a \ref RangeImagePlanar object to a PNG file
* \param[in] file_name the name of the file to save the data to
- <a href="http://pointclouds.org/documentation/tutorials/reading_pcd.php#reading-pcd">Reading PointCloud data from PCD files</a>
- <a href="http://pointclouds.org/documentation/tutorials/writing_pcd.php#writing-pcd">Writing PointCloud data to PCD files</a>
- <a href="http://pointclouds.org/documentation/tutorials/openni_grabber.php#openni-grabber">The OpenNI Grabber Framework in PCL</a>
+ - <a href="http://pointclouds.org/documentation/tutorials/ensenso_cameras.php">Grabbing point clouds from Ensenso cameras</a>
PCL is agnostic with respect to the data sources that are used to generate 3D
point clouds. While OpenNI-compatible cameras have recently been at the
- \ref common "common"
- \ref octree "octree"
- OpenNi for kinect handling
+ - uEye and Ensenso SDK for Ensenso handling
*/
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2013-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/io/auto_io.h>
+#include <pcl/io/boost.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/ifs_io.h>
+#include <pcl/io/obj_io.h>
+#include <pcl/io/vtk_io.h>
+
+
+int
+pcl::io::load (const std::string& file_name, pcl::PCLPointCloud2& blob)
+{
+ boost::filesystem::path p (file_name.c_str ());
+ std::string extension = p.extension ().string ();
+ int result = -1;
+ if (extension == ".pcd")
+ result = pcl::io::loadPCDFile (file_name, blob);
+ else if (extension == ".ply")
+ result = pcl::io::loadPLYFile (file_name, blob);
+ else if (extension == ".ifs")
+ result = pcl::io::loadIFSFile (file_name, blob);
+ else if (extension == ".obj")
+ result = pcl::io::loadOBJFile (file_name, blob);
+ else
+ {
+ PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s", extension.c_str ());
+ result = -1;
+ }
+ return (result);
+}
+
+int
+pcl::io::load (const std::string& file_name, pcl::PolygonMesh& mesh)
+{
+ boost::filesystem::path p (file_name.c_str ());
+ std::string extension = p.extension ().string ();
+ int result = -1;
+ if (extension == ".ply")
+ result = pcl::io::loadPLYFile (file_name, mesh);
+ else if (extension == ".ifs")
+ result = pcl::io::loadIFSFile (file_name, mesh);
+ else if (extension == ".obj")
+ result = pcl::io::loadOBJFile (file_name, mesh);
+ else
+ {
+ PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s", extension.c_str ());
+ result = -1;
+ }
+ return (result);
+}
+
+int
+pcl::io::load (const std::string& file_name, pcl::TextureMesh& mesh)
+{
+ boost::filesystem::path p (file_name.c_str ());
+ std::string extension = p.extension ().string ();
+ int result = -1;
+ if (extension == ".obj")
+ result = pcl::io::loadOBJFile (file_name, mesh);
+ else
+ {
+ PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s", extension.c_str ());
+ result = -1;
+ }
+ return (result);
+}
+
+int
+pcl::io::save (const std::string& file_name, const pcl::PCLPointCloud2& blob, unsigned precision)
+{
+ boost::filesystem::path p (file_name.c_str ());
+ std::string extension = p.extension ().string ();
+ int result = -1;
+ if (extension == ".pcd")
+ {
+ Eigen::Vector4f origin = Eigen::Vector4f::Zero ();
+ Eigen::Quaternionf orientation = Eigen::Quaternionf::Identity ();
+ result = pcl::io::savePCDFile (file_name, blob, origin, orientation, true);
+ }
+ else if (extension == ".ply")
+ {
+ Eigen::Vector4f origin = Eigen::Vector4f::Zero ();
+ Eigen::Quaternionf orientation = Eigen::Quaternionf::Identity ();
+ result = pcl::io::savePLYFile (file_name, blob, origin, orientation, true);
+ }
+ else if (extension == ".ifs")
+ result = pcl::io::saveIFSFile (file_name, blob);
+ else if (extension == ".vtk")
+ result = pcl::io::saveVTKFile (file_name, blob, precision);
+ else
+ {
+ PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s", extension.c_str ());
+ result = -1;
+ }
+ return (result);
+}
+
+int
+pcl::io::save (const std::string &file_name, const pcl::TextureMesh &tex_mesh, unsigned precision)
+{
+ boost::filesystem::path p (file_name.c_str ());
+ std::string extension = p.extension ().string ();
+ int result = -1;
+ if (extension == ".obj")
+ result = pcl::io::saveOBJFile (file_name, tex_mesh, precision);
+ else
+ {
+ PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s", extension.c_str ());
+ result = -1;
+ }
+ return (result);
+}
+
+int
+pcl::io::save (const std::string &file_name, const pcl::PolygonMesh &poly_mesh, unsigned precision)
+{
+ boost::filesystem::path p (file_name.c_str ());
+ std::string extension = p.extension ().string ();
+ int result = -1;
+ if (extension == ".ply")
+ result = pcl::io::savePLYFileBinary (file_name, poly_mesh);
+ else if (extension == ".obj")
+ result = pcl::io::saveOBJFile (file_name, poly_mesh, precision);
+ else if (extension == ".vtk")
+ result = pcl::io::saveVTKFile (file_name, poly_mesh, precision);
+ else
+ {
+ PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s", extension.c_str ());
+ result = -1;
+ }
+ return (result);
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Victor Lamoine (victor.lamoine@gmail.com)
+ */
+
+#include <pcl/pcl_config.h>
+#include <pcl/io/davidsdk_grabber.h>
+#include <pcl/exceptions.h>
+#include <pcl/common/io.h>
+#include <pcl/conversions.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/vtk_lib_io.h>
+#include <pcl/console/print.h>
+#include <pcl/point_types.h>
+
+// Possible improvements:
+// TODO: Add presets for david::CodedLightPhaseShiftParams to enable easy scan quality changing
+// TODO: Add texture support (call .Scan () instead of .Scan (false) and properly convert data
+// TODO: Use mesh IDs rather than clearing all meshes every time
+// TODO: In processGrabbing, start scanning again while transferring the mesh (not possible with SDK 1.5.2 because ExportMesh() is blocking)
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::DavidSDKGrabber::DavidSDKGrabber () :
+ client_connected_ (false),
+ running_ (false),
+ local_path_ ("C:/temp"),
+ remote_path_ ("C:/temp"),
+ file_format_ ("stl")
+{
+ point_cloud_signal_ = createSignal<sig_cb_davidsdk_point_cloud> ();
+ mesh_signal_ = createSignal<sig_cb_davidsdk_mesh> ();
+ image_signal_ = createSignal<sig_cb_davidsdk_image> ();
+ point_cloud_image_signal_ = createSignal<sig_cb_davidsdk_point_cloud_image> ();
+ mesh_image_signal_ = createSignal<sig_cb_davidsdk_mesh_image> ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::DavidSDKGrabber::~DavidSDKGrabber () throw ()
+{
+ try
+ {
+ stop ();
+
+ disconnect_all_slots<sig_cb_davidsdk_point_cloud> ();
+ disconnect_all_slots<sig_cb_davidsdk_mesh> ();
+ disconnect_all_slots<sig_cb_davidsdk_image> ();
+ disconnect_all_slots<sig_cb_davidsdk_point_cloud_image> ();
+ disconnect_all_slots<sig_cb_davidsdk_mesh_image> ();
+ }
+ catch (...)
+ {
+ // destructor never throws
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+david::ServerInfo
+pcl::DavidSDKGrabber::connect (const std::string &address,
+ uint16_t port)
+{
+ david::ServerInfo server_info;
+
+ if (client_connected_)
+ return (server_info);
+
+ try
+ {
+ david_.Connect (address, port);
+ client_connected_ = true;
+ }
+ catch (david::Exception& e)
+ {
+ e.PrintError ();
+ }
+
+ return (server_info);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DavidSDKGrabber::disconnect (const bool stop_server)
+{
+ if (!client_connected_)
+ return;
+
+ try
+ {
+ david_.Disconnect (stop_server);
+ }
+ catch (david::Exception& e)
+ {
+ e.PrintError ();
+ }
+
+ client_connected_ = false;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DavidSDKGrabber::start ()
+{
+ if (isRunning ())
+ return;
+
+ frequency_.reset ();
+ running_ = true;
+ grabber_thread_ = boost::thread (&pcl::DavidSDKGrabber::processGrabbing, this);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DavidSDKGrabber::stop ()
+{
+ if (running_)
+ {
+ running_ = false; // Stop processGrabbing () callback
+
+ grabber_thread_.join (); // join () waits for the thread to finish it's last iteration
+ // See: http://www.boost.org/doc/libs/1_54_0/doc/html/thread/thread_management.html#thread.thread_management.thread.join
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::DavidSDKGrabber::isRunning () const
+{
+ return (running_);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::DavidSDKGrabber::isConnected () const
+{
+ return (client_connected_);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+std::string
+pcl::DavidSDKGrabber::getName () const
+{
+ return ("DavidSDKGrabber");
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+std::string
+pcl::DavidSDKGrabber::getLocalPath ()
+{
+ return (local_path_);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+std::string
+pcl::DavidSDKGrabber::getRemotePath ()
+{
+ return (remote_path_);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DavidSDKGrabber::setFileFormatToOBJ ()
+{
+ file_format_ = "obj";
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DavidSDKGrabber::setFileFormatToPLY ()
+{
+ file_format_ = "ply";
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DavidSDKGrabber::setFileFormatToSTL ()
+{
+ file_format_ = "stl";
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+std::string
+pcl::DavidSDKGrabber::getFileFormat ()
+{
+ return (file_format_);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DavidSDKGrabber::setLocalPath (std::string path)
+{
+ local_path_ = path;
+
+ if (path.empty ())
+ local_path_ = "C:/temp";
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DavidSDKGrabber::setRemotePath (std::string path)
+{
+ remote_path_ = path;
+
+ if (path.empty ())
+ remote_path_ = local_path_;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DavidSDKGrabber::setLocalAndRemotePaths (std::string local_path,
+ std::string remote_path)
+{
+ setLocalPath (local_path);
+ setRemotePath (remote_path);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::DavidSDKGrabber::calibrate (double grid_size)
+{
+ if (!client_connected_ || running_)
+ return (false);
+
+ try
+ {
+ david_.sls ().Calibrate (grid_size);
+ }
+ catch (david::Exception& e)
+ {
+ e.PrintError ();
+ return (false);
+ }
+ return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::DavidSDKGrabber::grabSingleCloud (pcl::PointCloud<pcl::PointXYZ> &cloud)
+{
+ if (!client_connected_ || running_)
+ return (false);
+
+ try
+ {
+ david_.sls ().Scan (false);
+ david_.fusion ().DeleteAllMeshes ();
+ david_.sls ().AddScanToShapeFusion ();
+ david_.sls ().ExportMesh (remote_path_ + "scan." + file_format_);
+
+ pcl::PolygonMesh mesh;
+ if (file_format_ == "obj")
+ {
+ if (pcl::io::loadPolygonFileOBJ (local_path_ + "scan." + file_format_, mesh) == 0)
+ return (false);
+ }
+ else if (file_format_ == "ply")
+ {
+ if (pcl::io::loadPolygonFilePLY (local_path_ + "scan." + file_format_, mesh) == 0)
+ return (false);
+ }
+ else if (file_format_ == "stl")
+ {
+ if (pcl::io::loadPolygonFileSTL (local_path_ + "scan." + file_format_, mesh) == 0)
+ return (false);
+ }
+ else
+ return (false);
+
+ pcl::fromPCLPointCloud2 (mesh.cloud, cloud);
+ }
+ catch (david::Exception& e)
+ {
+ e.PrintError ();
+ return (false);
+ }
+ return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::DavidSDKGrabber::grabSingleMesh (pcl::PolygonMesh &mesh)
+{
+ if (!client_connected_ || running_)
+ return (false);
+
+ try
+ {
+ david_.sls ().Scan (false);
+ david_.fusion ().DeleteAllMeshes ();
+ david_.sls ().AddScanToShapeFusion ();
+ david_.sls ().ExportMesh (remote_path_ + "scan." + file_format_);
+
+ if (file_format_ == "obj")
+ {
+ if (pcl::io::loadPolygonFileOBJ (local_path_ + "scan." + file_format_, mesh) == 0)
+ return (false);
+ }
+ else if (file_format_ == "ply")
+ {
+ if (pcl::io::loadPolygonFilePLY (local_path_ + "scan." + file_format_, mesh) == 0)
+ return (false);
+ }
+ else if (file_format_ == "stl")
+ {
+ if (pcl::io::loadPolygonFileSTL (local_path_ + "scan." + file_format_, mesh) == 0)
+ return (false);
+ }
+ else
+ return (false);
+
+ }
+ catch (david::Exception& e)
+ {
+ e.PrintError ();
+ return (false);
+ }
+ return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float
+pcl::DavidSDKGrabber::getFramesPerSecond () const
+{
+ boost::mutex::scoped_lock lock (fps_mutex_);
+ return (frequency_.getFrequency ());
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DavidSDKGrabber::processGrabbing ()
+{
+ bool continue_grabbing = running_;
+ while (continue_grabbing)
+ {
+ try
+ {
+ // Publish cloud / images
+ if (num_slots<sig_cb_davidsdk_point_cloud> () > 0 || num_slots<sig_cb_davidsdk_mesh> () > 0 || num_slots<sig_cb_davidsdk_image> () > 0
+ || num_slots<sig_cb_davidsdk_point_cloud_image> () > 0 || num_slots<sig_cb_davidsdk_mesh_image> () > 0)
+ {
+ pcl::PolygonMesh::Ptr mesh;
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+ boost::shared_ptr<pcl::PCLImage> image;
+
+ fps_mutex_.lock ();
+ frequency_.event ();
+ fps_mutex_.unlock ();
+
+ // We need the image
+ if (num_slots<sig_cb_davidsdk_image> () > 0 || num_slots<sig_cb_davidsdk_point_cloud_image> () > 0 || num_slots<sig_cb_davidsdk_mesh_image> () > 0)
+ {
+ image.reset (new pcl::PCLImage);
+ int width, height;
+ david_.sls ().GetLiveImage (image->data, width, height);
+ image->width = (uint32_t) width;
+ image->height = (uint32_t) height;
+ image->encoding = "CV_8UC1";
+ }
+
+ // We need the cloud or mesh
+ if (num_slots<sig_cb_davidsdk_point_cloud> () > 0 || num_slots<sig_cb_davidsdk_mesh> () > 0 || num_slots<sig_cb_davidsdk_point_cloud_image> () > 0
+ || num_slots<sig_cb_davidsdk_mesh_image> () > 0)
+ {
+ mesh.reset (new pcl::PolygonMesh);
+ david_.sls ().Scan (false);
+ david_.fusion ().DeleteAllMeshes ();
+ david_.sls ().AddScanToShapeFusion ();
+ david_.sls ().ExportMesh (remote_path_ + "scan." + file_format_);
+
+ if (file_format_ == "obj")
+ {
+ if (pcl::io::loadPolygonFileOBJ (local_path_ + "scan." + file_format_, *mesh) == 0)
+ return;
+ }
+ else if (file_format_ == "ply")
+ {
+ if (pcl::io::loadPolygonFilePLY (local_path_ + "scan." + file_format_, *mesh) == 0)
+ return;
+ }
+ else if (file_format_ == "stl")
+ {
+ if (pcl::io::loadPolygonFileSTL (local_path_ + "scan." + file_format_, *mesh) == 0)
+ return;
+ }
+ else
+ return;
+
+ if (num_slots<sig_cb_davidsdk_point_cloud> () > 0 || num_slots<sig_cb_davidsdk_point_cloud_image> () > 0)
+ {
+ cloud.reset (new PointCloud<pcl::PointXYZ>);
+ pcl::fromPCLPointCloud2 (mesh->cloud, *cloud);
+ }
+ }
+
+ // Publish signals
+ if (num_slots<sig_cb_davidsdk_point_cloud_image> () > 0)
+ point_cloud_image_signal_->operator () (cloud, image);
+ if (num_slots<sig_cb_davidsdk_mesh_image> () > 0)
+ mesh_image_signal_->operator () (mesh, image);
+ else if (num_slots<sig_cb_davidsdk_point_cloud> () > 0)
+ point_cloud_signal_->operator () (cloud);
+ else if (num_slots<sig_cb_davidsdk_mesh> () > 0)
+ mesh_signal_->operator () (mesh);
+ else if (num_slots<sig_cb_davidsdk_image> () > 0)
+ image_signal_->operator () (image);
+ }
+ continue_grabbing = running_;
+ }
+ catch (david::Exception& e)
+ {
+ e.PrintError ();
+ }
+ }
+}
// padding skip for destination image
unsigned rgb_line_skip = rgb_line_step - width * 3;
- register unsigned yIdx, xIdx;
+ unsigned yIdx, xIdx;
// first two pixel values for first two lines
// Bayer 0 1 2
// 0 G r g
// padding skip for destination image
unsigned rgb_line_skip = rgb_line_step - width * 3;
- register unsigned yIdx, xIdx;
+ unsigned yIdx, xIdx;
int dh, dv;
// first two pixel values for first two lines
// padding skip for destination image
unsigned rgb_line_skip = rgb_line_step - width * 3;
- register unsigned yIdx, xIdx;
+ unsigned yIdx, xIdx;
int dh, dv;
// first two pixel values for first two lines
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/io/io_exception.h>
+
+#include <pcl/io/depth_sense/depth_sense_grabber_impl.h>
+#include <pcl/io/depth_sense/depth_sense_device_manager.h>
+
+boost::mutex pcl::io::depth_sense::DepthSenseDeviceManager::mutex_;
+
+pcl::io::depth_sense::DepthSenseDeviceManager::DepthSenseDeviceManager ()
+{
+ try
+ {
+ context_ = DepthSense::Context::create ("localhost");
+ depth_sense_thread_ = boost::thread (&DepthSense::Context::run, &context_);
+ }
+ catch (...)
+ {
+ THROW_IO_EXCEPTION ("failed to initialize DepthSense context");
+ }
+}
+
+pcl::io::depth_sense::DepthSenseDeviceManager::~DepthSenseDeviceManager ()
+{
+ try
+ {
+ context_.quit();
+ depth_sense_thread_.join();
+ }
+ catch (DepthSense::InvalidOperationException& e)
+ {
+ // do nothing
+ }
+}
+
+std::string
+pcl::io::depth_sense::DepthSenseDeviceManager::captureDevice (DepthSenseGrabberImpl* grabber)
+{
+ boost::mutex::scoped_lock lock (mutex_);
+ std::vector<DepthSense::Device> devices = context_.getDevices ();
+ if (devices.size () == 0)
+ THROW_IO_EXCEPTION ("no connected devices");
+ for (size_t i = 0; i < devices.size (); ++i)
+ if (!isCaptured (devices[i].getSerialNumber ()))
+ return (captureDevice (grabber, devices[i]));
+ THROW_IO_EXCEPTION ("all connected devices are captured by other grabbers");
+ return (""); // never reached, needed just to silence -Wreturn-type warning
+}
+
+std::string
+pcl::io::depth_sense::DepthSenseDeviceManager::captureDevice (DepthSenseGrabberImpl* grabber, size_t index)
+{
+ boost::mutex::scoped_lock lock (mutex_);
+ if (index >= context_.getDevices ().size ())
+ THROW_IO_EXCEPTION ("device with index %i is not connected", index + 1);
+ if (isCaptured (context_.getDevices ().at (index).getSerialNumber ()))
+ THROW_IO_EXCEPTION ("device with index %i is captured by another grabber", index);
+ return (captureDevice (grabber, context_.getDevices ().at (index)));
+}
+
+std::string
+pcl::io::depth_sense::DepthSenseDeviceManager::captureDevice (DepthSenseGrabberImpl* grabber, const std::string& sn)
+{
+ boost::mutex::scoped_lock lock (mutex_);
+ std::vector<DepthSense::Device> devices = context_.getDevices ();
+ for (size_t i = 0; i < devices.size (); ++i)
+ {
+ if (devices[i].getSerialNumber () == sn)
+ {
+ if (isCaptured (sn))
+ THROW_IO_EXCEPTION ("device with serial number %s is captured by another grabber", sn.c_str ());
+ return (captureDevice (grabber, devices[i]));
+ }
+ }
+ THROW_IO_EXCEPTION ("device with serial number %s is not connected", sn.c_str ());
+ return (""); // never reached, needed just to silence -Wreturn-type warning
+}
+
+void
+pcl::io::depth_sense::DepthSenseDeviceManager::reconfigureDevice (const std::string& sn)
+{
+ boost::mutex::scoped_lock lock (mutex_);
+ const CapturedDevice& dev = captured_devices_[sn];
+ context_.requestControl (dev.depth_node, 0);
+ dev.grabber->configureDepthNode (dev.depth_node);
+ context_.releaseControl (dev.depth_node);
+ context_.requestControl (dev.color_node, 0);
+ dev.grabber->configureColorNode (dev.color_node);
+ context_.releaseControl (dev.color_node);
+}
+
+void
+pcl::io::depth_sense::DepthSenseDeviceManager::startDevice (const std::string& sn)
+{
+ const CapturedDevice& dev = captured_devices_[sn];
+ try
+ {
+ context_.registerNode (dev.depth_node);
+ context_.registerNode (dev.color_node);
+ context_.startNodes ();
+ }
+ catch (DepthSense::ArgumentException& e)
+ {
+ THROW_IO_EXCEPTION ("unable to start device %s, possibly disconnected", sn.c_str ());
+ }
+}
+
+void
+pcl::io::depth_sense::DepthSenseDeviceManager::stopDevice (const std::string& sn)
+{
+ const CapturedDevice& dev = captured_devices_[sn];
+ try
+ {
+ context_.unregisterNode (dev.depth_node);
+ context_.unregisterNode (dev.color_node);
+ if (context_.getRegisteredNodes ().size () == 0)
+ context_.stopNodes ();
+ }
+ catch (DepthSense::ArgumentException& e)
+ {
+ THROW_IO_EXCEPTION ("unable to stop device %s, possibly disconnected", sn.c_str ());
+ }
+}
+
+void
+pcl::io::depth_sense::DepthSenseDeviceManager::releaseDevice (const std::string& sn)
+{
+ boost::mutex::scoped_lock lock (mutex_);
+ const CapturedDevice& dev = captured_devices_[sn];
+ dev.depth_node.newSampleReceivedEvent ().disconnect (dev.grabber, &DepthSenseGrabberImpl::onDepthDataReceived);
+ dev.color_node.newSampleReceivedEvent ().disconnect (dev.grabber, &DepthSenseGrabberImpl::onColorDataReceived);
+ captured_devices_.erase (sn);
+}
+
+std::string
+pcl::io::depth_sense::DepthSenseDeviceManager::captureDevice (DepthSenseGrabberImpl* grabber, DepthSense::Device device)
+{
+ // This is called from public captureDevice() functions and should already be
+ // under scoped lock
+ CapturedDevice dev;
+ dev.grabber = grabber;
+ std::vector<DepthSense::Node> nodes = device.getNodes ();
+ for (size_t i = 0; i < nodes.size (); ++i)
+ {
+ if (nodes[i].is<DepthSense::DepthNode> ())
+ {
+ dev.depth_node = nodes[i].as<DepthSense::DepthNode> ();
+ dev.depth_node.newSampleReceivedEvent ().connect (grabber, &DepthSenseGrabberImpl::onDepthDataReceived);
+ grabber->setCameraParameters (device.getStereoCameraParameters ());
+ }
+ if (nodes[i].is<DepthSense::ColorNode> ())
+ {
+ dev.color_node = nodes[i].as<DepthSense::ColorNode> ();
+ dev.color_node.newSampleReceivedEvent ().connect (grabber, &DepthSenseGrabberImpl::onColorDataReceived);
+ }
+ }
+ captured_devices_.insert (std::make_pair (device.getSerialNumber (), dev));
+ return (device.getSerialNumber ());
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <boost/lexical_cast.hpp>
+
+#include <pcl/common/io.h>
+#include <pcl/io/depth_sense_grabber.h>
+#include <pcl/io/depth_sense/depth_sense_grabber_impl.h>
+#include <pcl/io/depth_sense/depth_sense_device_manager.h>
+
+pcl::io::depth_sense::DepthSenseGrabberImpl::DepthSenseGrabberImpl (DepthSenseGrabber* parent, const std::string& device_id)
+: p_ (parent)
+, is_running_ (false)
+, confidence_threshold_ (50)
+, temporal_filtering_type_ (DepthSenseGrabber::DepthSense_None)
+, color_data_ (COLOR_SIZE * 3)
+, depth_buffer_ (new pcl::io::SingleBuffer<float> (SIZE))
+{
+ if (device_id == "")
+ device_id_ = DepthSenseDeviceManager::getInstance ()->captureDevice (this);
+ else if (device_id[0] == '#')
+ device_id_ = DepthSenseDeviceManager::getInstance ()->captureDevice (this, boost::lexical_cast<int> (device_id.substr (1)) - 1);
+ else
+ device_id_ = DepthSenseDeviceManager::getInstance ()->captureDevice (this, device_id);
+
+ point_cloud_signal_ = p_->createSignal<sig_cb_depth_sense_point_cloud> ();
+ point_cloud_rgba_signal_ = p_->createSignal<sig_cb_depth_sense_point_cloud_rgba> ();
+}
+
+pcl::io::depth_sense::DepthSenseGrabberImpl::~DepthSenseGrabberImpl () throw ()
+{
+ stop ();
+
+ DepthSenseDeviceManager::getInstance ()->releaseDevice (device_id_);
+
+ p_->disconnect_all_slots<sig_cb_depth_sense_point_cloud> ();
+ p_->disconnect_all_slots<sig_cb_depth_sense_point_cloud_rgba> ();
+}
+
+
+void
+pcl::io::depth_sense::DepthSenseGrabberImpl::start ()
+{
+ need_xyz_ = p_->num_slots<sig_cb_depth_sense_point_cloud> () > 0;
+ need_xyzrgba_ = p_->num_slots<sig_cb_depth_sense_point_cloud_rgba> () > 0;
+
+ if (!is_running_)
+ {
+ DepthSenseDeviceManager::getInstance ()->reconfigureDevice (device_id_);
+ DepthSenseDeviceManager::getInstance ()->startDevice (device_id_);
+ frequency_.reset ();
+ is_running_ = true;
+ }
+}
+
+void
+pcl::io::depth_sense::DepthSenseGrabberImpl::stop ()
+{
+ if (is_running_)
+ {
+ DepthSenseDeviceManager::getInstance ()->stopDevice (device_id_);
+ is_running_ = false;
+ }
+}
+
+float
+pcl::io::depth_sense::DepthSenseGrabberImpl::getFramesPerSecond () const
+{
+ boost::mutex::scoped_lock lock (fps_mutex_);
+ return (frequency_.getFrequency ());
+}
+
+void
+pcl::io::depth_sense::DepthSenseGrabberImpl::setConfidenceThreshold (int threshold)
+{
+ confidence_threshold_ = threshold;
+ DepthSenseDeviceManager::getInstance ()->reconfigureDevice (device_id_);
+}
+
+void
+pcl::io::depth_sense::DepthSenseGrabberImpl::enableTemporalFiltering (DepthSenseGrabber::TemporalFilteringType type, size_t window_size)
+{
+ if (temporal_filtering_type_ != type ||
+ (type != DepthSenseGrabber::DepthSense_None && depth_buffer_->size () != window_size))
+ {
+ bool was_running = is_running_;
+ if (was_running)
+ stop ();
+ switch (type)
+ {
+ case DepthSenseGrabber::DepthSense_None:
+ {
+ depth_buffer_.reset (new pcl::io::SingleBuffer<float> (SIZE));
+ break;
+ }
+ case DepthSenseGrabber::DepthSense_Median:
+ {
+ depth_buffer_.reset (new pcl::io::MedianBuffer<float> (SIZE, window_size));
+ break;
+ }
+ case DepthSenseGrabber::DepthSense_Average:
+ {
+ depth_buffer_.reset (new pcl::io::AverageBuffer<float> (SIZE, window_size));
+ break;
+ }
+ }
+ temporal_filtering_type_ = type;
+ if (was_running)
+ start ();
+ }
+}
+
+void
+pcl::io::depth_sense::DepthSenseGrabberImpl::setCameraParameters (const DepthSense::StereoCameraParameters& parameters)
+{
+ projection_.reset (new DepthSense::ProjectionHelper (parameters));
+}
+
+void
+pcl::io::depth_sense::DepthSenseGrabberImpl::configureDepthNode (DepthSense::DepthNode node) const
+{
+ DepthSense::DepthNode::Configuration config = node.getConfiguration ();
+ config.frameFormat = DepthSense::FRAME_FORMAT_QVGA;
+ config.framerate = FRAMERATE;
+ config.mode = DepthSense::DepthNode::CAMERA_MODE_CLOSE_MODE;
+ config.saturation = false;
+ node.setEnableDepthMapFloatingPoint (true);
+ node.setEnableUvMap (true);
+ node.setEnableConfidenceMap (true);
+ node.setConfiguration (config);
+}
+
+void
+pcl::io::depth_sense::DepthSenseGrabberImpl::configureColorNode (DepthSense::ColorNode node) const
+{
+ DepthSense::ColorNode::Configuration config = node.getConfiguration ();
+ config.frameFormat = DepthSense::FRAME_FORMAT_VGA;
+ config.compression = DepthSense::COMPRESSION_TYPE_MJPEG;
+ config.powerLineFrequency = DepthSense::POWER_LINE_FREQUENCY_50HZ;
+ config.framerate = FRAMERATE;
+ node.setEnableColorMap (true);
+ node.setConfiguration (config);
+}
+
+void
+pcl::io::depth_sense::DepthSenseGrabberImpl::onDepthDataReceived (DepthSense::DepthNode, DepthSense::DepthNode::NewSampleReceivedData data)
+{
+ fps_mutex_.lock ();
+ frequency_.event ();
+ fps_mutex_.unlock ();
+
+ static const float nan = std::numeric_limits<float>::quiet_NaN ();
+
+ std::vector<float> depth_data (SIZE);
+ memcpy (depth_data.data (), &data.depthMapFloatingPoint[0], SIZE * sizeof (float));
+ for (int i = 0; i < SIZE; i++)
+ if (data.confidenceMap[i] < confidence_threshold_)
+ depth_data[i] = nan;
+ depth_buffer_->push (depth_data);
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_cloud;
+ pcl::PointCloud<pcl::PointXYZRGBA>::Ptr xyzrgba_cloud;
+
+ if (need_xyz_)
+ {
+ xyz_cloud.reset (new pcl::PointCloud<pcl::PointXYZ> (WIDTH, HEIGHT));
+ xyz_cloud->is_dense = false;
+
+ computeXYZ (*xyz_cloud);
+
+ point_cloud_signal_->operator () (xyz_cloud);
+ }
+
+ if (need_xyzrgba_)
+ {
+ xyzrgba_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBA> (WIDTH, HEIGHT));
+ xyzrgba_cloud->is_dense = false;
+
+ if (need_xyz_)
+ copyPointCloud (*xyz_cloud, *xyzrgba_cloud);
+ else
+ computeXYZ (*xyzrgba_cloud);
+
+ for (int i = 0; i < SIZE; i++)
+ {
+ const DepthSense::UV& uv = data.uvMap[i];
+ int row = static_cast<int> (uv.v * COLOR_HEIGHT);
+ int col = static_cast<int> (uv.u * COLOR_WIDTH);
+ int pixel = row * COLOR_WIDTH + col;
+ if (pixel >=0 && pixel < COLOR_WIDTH * COLOR_HEIGHT)
+ memcpy (&xyzrgba_cloud->points[i].rgba, &color_data_[pixel * 3], 3);
+ }
+
+ point_cloud_rgba_signal_->operator () (xyzrgba_cloud);
+ }
+}
+
+void
+pcl::io::depth_sense::DepthSenseGrabberImpl::onColorDataReceived (DepthSense::ColorNode, DepthSense::ColorNode::NewSampleReceivedData data)
+{
+ if (need_xyzrgba_)
+ memcpy (&color_data_[0], data.colorMap, color_data_.size ());
+}
+
+template <typename Point> void
+pcl::io::depth_sense::DepthSenseGrabberImpl::computeXYZ (PointCloud<Point>& cloud)
+{
+ static const float nan = std::numeric_limits<float>::quiet_NaN ();
+
+ int i = 0;
+ DepthSense::FPExtended2DPoint point (DepthSense::Point2D (0, 0), 0);
+ DepthSense::FPVertex vertex;
+ while (point.point.y < HEIGHT)
+ {
+ point.point.x = 0;
+ while (point.point.x < WIDTH)
+ {
+ point.depth = (*depth_buffer_)[i];
+ if (pcl_isnan (point.depth))
+ {
+ cloud.points[i].x = nan;
+ cloud.points[i].y = nan;
+ cloud.points[i].z = nan;
+ }
+ else
+ {
+ projection_->get3DCoordinates (&point, &vertex, 1);
+ cloud.points[i].x = vertex.x;
+ cloud.points[i].y = vertex.y;
+ cloud.points[i].z = vertex.z;
+ }
+ point.point.x += 1;
+ ++i;
+ }
+ point.point.y += 1;
+ }
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/io/depth_sense_grabber.h>
+#include <pcl/io/depth_sense/depth_sense_grabber_impl.h>
+
+pcl::DepthSenseGrabber::DepthSenseGrabber (const std::string& device_id)
+: Grabber ()
+, p_ (new pcl::io::depth_sense::DepthSenseGrabberImpl (this, device_id))
+{
+}
+
+pcl::DepthSenseGrabber::~DepthSenseGrabber () throw ()
+{
+ delete p_;
+}
+
+void
+pcl::DepthSenseGrabber::start ()
+{
+ p_->start();
+}
+
+void
+pcl::DepthSenseGrabber::stop ()
+{
+ p_->stop();
+}
+
+bool
+pcl::DepthSenseGrabber::isRunning () const
+{
+ return (p_->is_running_);
+}
+
+float
+pcl::DepthSenseGrabber::getFramesPerSecond () const
+{
+ return (p_->getFramesPerSecond());
+}
+
+void
+pcl::DepthSenseGrabber::setConfidenceThreshold (int threshold)
+{
+ p_->setConfidenceThreshold (threshold);
+}
+
+void
+pcl::DepthSenseGrabber::enableTemporalFiltering (TemporalFilteringType type, size_t window_size)
+{
+ p_->enableTemporalFiltering (type, window_size);
+}
+
+void
+pcl::DepthSenseGrabber::disableTemporalFiltering ()
+{
+ p_->enableTemporalFiltering (DepthSense_None, 1);
+}
+
+std::string
+pcl::DepthSenseGrabber::getDeviceSerialNumber () const
+{
+ return (p_->device_id_);
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Victor Lamoine (victor.lamoine@gmail.com)
+ */
+
+#include <pcl/pcl_config.h>
+#include <pcl/io/ensenso_grabber.h>
+#include <pcl/exceptions.h>
+#include <pcl/common/io.h>
+#include <pcl/console/print.h>
+#include <pcl/point_types.h>
+#include <boost/format.hpp>
+#include <boost/algorithm/string/predicate.hpp>
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Handle Ensenso SDK exceptions
+// This function is called whenever an exception is raised to provide details about the error
+void
+ensensoExceptionHandling (const NxLibException &ex,
+ std::string func_nam)
+{
+ PCL_ERROR ("%s: NxLib error %s (%d) occurred while accessing item %s.\n", func_nam.c_str (), ex.getErrorText ().c_str (), ex.getErrorCode (),
+ ex.getItemPath ().c_str ());
+ if (ex.getErrorCode () == NxLibExecutionFailed)
+ {
+ NxLibCommand cmd ("");
+ PCL_WARN ("\n%s\n", cmd.result ().asJson (true, 4, false).c_str ());
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::EnsensoGrabber::EnsensoGrabber () :
+ device_open_ (false),
+ tcp_open_ (false),
+ running_ (false)
+{
+ point_cloud_signal_ = createSignal<sig_cb_ensenso_point_cloud> ();
+ images_signal_ = createSignal<sig_cb_ensenso_images> ();
+ point_cloud_images_signal_ = createSignal<sig_cb_ensenso_point_cloud_images> ();
+ PCL_INFO ("Initialising nxLib\n");
+
+ try
+ {
+ nxLibInitialize ();
+ root_.reset (new NxLibItem);
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "EnsensoGrabber");
+ PCL_THROW_EXCEPTION (pcl::IOException, "Could not initialise NxLib."); // If constructor fails; throw exception
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::EnsensoGrabber::~EnsensoGrabber () throw ()
+{
+ try
+ {
+ stop ();
+ root_.reset ();
+
+ disconnect_all_slots<sig_cb_ensenso_point_cloud> ();
+ disconnect_all_slots<sig_cb_ensenso_images> ();
+ disconnect_all_slots<sig_cb_ensenso_point_cloud_images> ();
+
+ if (tcp_open_)
+ closeTcpPort ();
+ nxLibFinalize ();
+ }
+ catch (...)
+ {
+ // destructor never throws
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+int
+pcl::EnsensoGrabber::enumDevices () const
+{
+ int camera_count = 0;
+
+ try
+ {
+ NxLibItem cams = NxLibItem ("/Cameras/BySerialNo");
+ camera_count = cams.count ();
+
+ // Print information for all cameras in the tree
+ PCL_INFO ("Number of connected cameras: %d\n", camera_count);
+ PCL_INFO ("Serial No Model Status\n");
+
+ for (int n = 0; n < cams.count (); ++n)
+ {
+ PCL_INFO ("%s %s %s\n", cams[n][itmSerialNumber].asString ().c_str (),
+ cams[n][itmModelName].asString ().c_str (),
+ cams[n][itmStatus].asString ().c_str ());
+ }
+ PCL_INFO ("\n");
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "enumDevices");
+ }
+
+ return (camera_count);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::openDevice (const int device)
+{
+ if (device_open_)
+ PCL_THROW_EXCEPTION (pcl::IOException, "Cannot open multiple devices!");
+
+ PCL_INFO ("Opening Ensenso stereo camera id = %d\n", device);
+
+ try
+ {
+ // Create a pointer referencing the camera's tree item, for easier access:
+ camera_ = (*root_)[itmCameras][itmBySerialNo][device];
+
+ if (!camera_.exists () || camera_[itmType] != valStereo)
+ {
+ PCL_THROW_EXCEPTION (pcl::IOException, "Please connect a single stereo camera to your computer!");
+ }
+
+ NxLibCommand open (cmdOpen);
+ open.parameters ()[itmCameras] = camera_[itmSerialNumber].asString ();
+ open.execute ();
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "openDevice");
+ return (false);
+ }
+
+ device_open_ = true;
+ return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::closeDevice ()
+{
+ if (!device_open_)
+ return (false);
+
+ stop ();
+ PCL_INFO ("Closing Ensenso stereo camera\n");
+
+ try
+ {
+ NxLibCommand (cmdClose).execute ();
+ device_open_ = false;
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "closeDevice");
+ return (false);
+ }
+ return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::EnsensoGrabber::start ()
+{
+ if (isRunning ())
+ return;
+
+ if (!device_open_)
+ openDevice (0);
+
+ frequency_.reset ();
+ running_ = true;
+ grabber_thread_ = boost::thread (&pcl::EnsensoGrabber::processGrabbing, this);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::EnsensoGrabber::stop ()
+{
+ if (running_)
+ {
+ running_ = false; // Stop processGrabbing () callback
+
+ grabber_thread_.join (); // join () waits for the thread to finish it's last iteration
+ // See: http://www.boost.org/doc/libs/1_54_0/doc/html/thread/thread_management.html#thread.thread_management.thread.join
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::isRunning () const
+{
+ return (running_);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::isTcpPortOpen () const
+{
+ return (tcp_open_);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+std::string
+pcl::EnsensoGrabber::getName () const
+{
+ return ("EnsensoGrabber");
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::configureCapture (const bool auto_exposure,
+ const bool auto_gain,
+ const int bining,
+ const float exposure,
+ const bool front_light,
+ const int gain,
+ const bool gain_boost,
+ const bool hardware_gamma,
+ const bool hdr,
+ const int pixel_clock,
+ const bool projector,
+ const int target_brightness,
+ const std::string trigger_mode,
+ const bool use_disparity_map_area_of_interest) const
+{
+ if (!device_open_)
+ return (false);
+
+ try
+ {
+ NxLibItem captureParams = camera_[itmParameters][itmCapture];
+ captureParams[itmAutoExposure].set (auto_exposure);
+ captureParams[itmAutoGain].set (auto_gain);
+ captureParams[itmBinning].set (bining);
+ captureParams[itmExposure].set (exposure);
+ captureParams[itmFrontLight].set (front_light);
+ captureParams[itmGain].set (gain);
+ captureParams[itmGainBoost].set (gain_boost);
+ captureParams[itmHardwareGamma].set (hardware_gamma);
+ captureParams[itmHdr].set (hdr);
+ captureParams[itmPixelClock].set (pixel_clock);
+ captureParams[itmProjector].set (projector);
+ captureParams[itmTargetBrightness].set (target_brightness);
+ captureParams[itmTriggerMode].set (trigger_mode);
+ captureParams[itmUseDisparityMapAreaOfInterest].set (use_disparity_map_area_of_interest);
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "configureCapture");
+ return (false);
+ }
+ return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::grabSingleCloud (pcl::PointCloud<pcl::PointXYZ> &cloud)
+{
+ if (!device_open_)
+ return (false);
+
+ if (running_)
+ return (false);
+
+ try
+ {
+ NxLibCommand (cmdCapture).execute ();
+
+ // Stereo matching task
+ NxLibCommand (cmdComputeDisparityMap).execute ();
+
+ // Convert disparity map into XYZ data for each pixel
+ NxLibCommand (cmdComputePointMap).execute ();
+
+ // Get info about the computed point map and copy it into a std::vector
+ double timestamp;
+ std::vector<float> pointMap;
+ int width, height;
+ camera_[itmImages][itmRaw][itmLeft].getBinaryDataInfo (0, 0, 0, 0, 0, ×tamp); // Get raw image timestamp
+ camera_[itmImages][itmPointMap].getBinaryDataInfo (&width, &height, 0, 0, 0, 0);
+ camera_[itmImages][itmPointMap].getBinaryData (pointMap, 0);
+
+ // Copy point cloud and convert in meters
+ cloud.header.stamp = getPCLStamp (timestamp);
+ cloud.resize (height * width);
+ cloud.width = width;
+ cloud.height = height;
+ cloud.is_dense = false;
+
+ // Copy data in point cloud (and convert milimeters in meters)
+ for (size_t i = 0; i < pointMap.size (); i += 3)
+ {
+ cloud.points[i / 3].x = pointMap[i] / 1000.0;
+ cloud.points[i / 3].y = pointMap[i + 1] / 1000.0;
+ cloud.points[i / 3].z = pointMap[i + 2] / 1000.0;
+ }
+
+ return (true);
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "grabSingleCloud");
+ return (false);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::initExtrinsicCalibration (const int grid_spacing) const
+{
+ if (!device_open_)
+ return (false);
+
+ if (running_)
+ return (false);
+
+ try
+ {
+ if (!clearCalibrationPatternBuffer ())
+ return (false);
+ (*root_)[itmParameters][itmPattern][itmGridSpacing].set (grid_spacing); // GridSize can't be changed, it's protected in the tree
+ // With the speckle projector on it is nearly impossible to recognize the pattern
+ // (the 3D calibration is based on stereo images, not on 3D depth map)
+
+ // Most important parameters are: projector=off, front_light=on
+ configureCapture (true, true, 1, 0.32, true, 1, false, false, false, 10, false, 80, "Software", false);
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "initExtrinsicCalibration");
+ return (false);
+ }
+ return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::clearCalibrationPatternBuffer () const
+{
+ if (!device_open_)
+ return (false);
+
+ if (running_)
+ return (false);
+
+ try
+ {
+ NxLibCommand (cmdDiscardPatterns).execute ();
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "clearCalibrationPatternBuffer");
+ return (false);
+ }
+ return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+int
+pcl::EnsensoGrabber::captureCalibrationPattern () const
+{
+ if (!device_open_)
+ return (-1);
+
+ if (running_)
+ return (-1);
+
+ try
+ {
+ NxLibCommand (cmdCapture).execute ();
+ NxLibCommand (cmdCollectPattern).execute ();
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "captureCalibrationPattern");
+ return (-1);
+ }
+
+ return ( (*root_)[itmParameters][itmPatternCount].asInt ());
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::estimateCalibrationPatternPose (Eigen::Affine3d &pattern_pose) const
+{
+ if (!device_open_)
+ return (false);
+
+ if (running_)
+ return (false);
+
+ try
+ {
+ NxLibCommand estimate_pattern_pose (cmdEstimatePatternPose);
+ estimate_pattern_pose.execute ();
+ NxLibItem tf = estimate_pattern_pose.result ()[itmPatternPose];
+ // Convert tf into a matrix
+ if (!jsonTransformationToMatrix (tf.asJson (), pattern_pose))
+ return (false);
+ pattern_pose.translation () /= 1000.0; // Convert translation in meters (Ensenso API returns milimeters)
+ return (true);
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "estimateCalibrationPatternPoses");
+ return (false);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::computeCalibrationMatrix (const std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d> > &robot_poses,
+ std::string &json,
+ const std::string setup,
+ const std::string target,
+ const Eigen::Affine3d &guess_tf,
+ const bool pretty_format) const
+{
+ if ( (*root_)[itmVersion][itmMajor] < 2 && (*root_)[itmVersion][itmMinor] < 3)
+ PCL_WARN ("EnsensoSDK 1.3.x fixes bugs into extrinsic calibration matrix optimization, please update your SDK!\n"
+ "http://www.ensenso.de/support/sdk-download/\n");
+
+ try
+ {
+ std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d> > robot_poses_mm (robot_poses);
+ std::vector<std::string> robot_poses_json;
+ robot_poses_json.resize (robot_poses.size ());
+ for (uint i = 0; i < robot_poses_json.size (); ++i)
+ {
+ robot_poses_mm[i].translation () *= 1000.0; // Convert meters in millimeters
+ if (!matrixTransformationToJson (robot_poses_mm[i], robot_poses_json[i]))
+ return (false);
+ }
+
+ NxLibCommand calibrate (cmdCalibrateHandEye);
+
+ // Set Hand-Eye calibration parameters
+ if (boost::iequals (setup, "Fixed"))
+ calibrate.parameters ()[itmSetup].set (valFixed);
+ else
+ calibrate.parameters ()[itmSetup].set (valMoving);
+
+ calibrate.parameters ()[itmTarget].set (target);
+
+ // Set guess matrix
+ if (guess_tf.matrix () != Eigen::Matrix4d::Identity ())
+ {
+ // Matrix > JSON > Angle axis
+ NxLibItem tf ("/tmpTF");
+ if (!matrixTransformationToJson (guess_tf, json))
+ return (false);
+ tf.setJson (json);
+
+ // Rotation
+ double theta = tf[itmRotation][itmAngle].asDouble (); // Angle of rotation
+ double x = tf[itmRotation][itmAxis][0].asDouble (); // X component of Euler vector
+ double y = tf[itmRotation][itmAxis][1].asDouble (); // Y component of Euler vector
+ double z = tf[itmRotation][itmAxis][2].asDouble (); // Z component of Euler vector
+ tf.erase(); // Delete tmpTF node
+
+ calibrate.parameters ()[itmLink][itmRotation][itmAngle].set (theta);
+ calibrate.parameters ()[itmLink][itmRotation][itmAxis][0].set (x);
+ calibrate.parameters ()[itmLink][itmRotation][itmAxis][1].set (y);
+ calibrate.parameters ()[itmLink][itmRotation][itmAxis][2].set (z);
+
+ // Translation
+ calibrate.parameters ()[itmLink][itmTranslation][0].set (guess_tf.translation ()[0] * 1000.0);
+ calibrate.parameters ()[itmLink][itmTranslation][1].set (guess_tf.translation ()[1] * 1000.0);
+ calibrate.parameters ()[itmLink][itmTranslation][2].set (guess_tf.translation ()[2] * 1000.0);
+ }
+
+ // Feed all robot poses into the calibration command
+ for (uint i = 0; i < robot_poses_json.size (); ++i)
+ {
+ // Very weird behavior here:
+ // If you modify this loop, check that all the transformations are still here in the [itmExecute][itmParameters] node
+ // because for an unknown reason sometimes the old transformations are erased in the tree ("null" in the tree)
+ // Ensenso SDK 2.3.348: If not moved after guess calibration matrix, the vector is empty.
+ calibrate.parameters ()[itmTransformations][i].setJson (robot_poses_json[i], false);
+ }
+
+ calibrate.execute (); // Might take up to 120 sec (maximum allowed by Ensenso API)
+
+ if (calibrate.successful ())
+ {
+ json = calibrate.result ().asJson (pretty_format);
+ return (true);
+ }
+ else
+ {
+ json.clear ();
+ return (false);
+ }
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "computeCalibrationMatrix");
+ return (false);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::storeEEPROMExtrinsicCalibration () const
+{
+ try
+ {
+ NxLibCommand store (cmdStoreCalibration);
+ store.execute ();
+ return (true);
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "storeEEPROMExtrinsicCalibration");
+ return (false);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::clearEEPROMExtrinsicCalibration ()
+{
+ try
+ {
+ setExtrinsicCalibration("");
+ NxLibCommand store (cmdStoreCalibration);
+ store.execute ();
+ return (true);
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "clearEEPROMExtrinsicCalibration");
+ return (false);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::setExtrinsicCalibration (const double euler_angle,
+ Eigen::Vector3d &rotation_axis,
+ const Eigen::Vector3d &translation,
+ const std::string target)
+{
+ if (!device_open_)
+ return (false);
+
+ if (rotation_axis != Eigen::Vector3d (0, 0, 0)) // Otherwise the vector becomes NaN
+ rotation_axis.normalize ();
+
+ try
+ {
+ NxLibItem calibParams = camera_[itmLink];
+ calibParams[itmTarget].set (target);
+ calibParams[itmRotation][itmAngle].set (euler_angle);
+
+ calibParams[itmRotation][itmAxis][0].set (rotation_axis[0]);
+ calibParams[itmRotation][itmAxis][1].set (rotation_axis[1]);
+ calibParams[itmRotation][itmAxis][2].set (rotation_axis[2]);
+
+ calibParams[itmTranslation][0].set (translation[0] * 1000.0); // Convert in millimeters
+ calibParams[itmTranslation][1].set (translation[1] * 1000.0);
+ calibParams[itmTranslation][2].set (translation[2] * 1000.0);
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "setExtrinsicCalibration");
+ return (false);
+ }
+ return (true);
+}
+
+bool
+pcl::EnsensoGrabber::setExtrinsicCalibration (const std::string target)
+{
+ if (!device_open_)
+ return (false);
+
+ Eigen::Vector3d rotation (Eigen::Vector3d::Zero ());
+ Eigen::Vector3d translation (Eigen::Vector3d::Zero ());
+ return (setExtrinsicCalibration (0.0, rotation, translation, target));
+}
+
+bool
+pcl::EnsensoGrabber::setExtrinsicCalibration (const Eigen::Affine3d &transformation,
+ const std::string target)
+{
+ std::string json;
+ if (!matrixTransformationToJson (transformation, json))
+ return (false);
+
+ double euler_angle;
+ Eigen::Vector3d rotation_axis;
+ Eigen::Vector3d translation;
+
+ if (!jsonTransformationToAngleAxis (json, euler_angle, rotation_axis, translation))
+ return (false);
+
+ return (setExtrinsicCalibration (euler_angle, rotation_axis, translation, target));
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+float
+pcl::EnsensoGrabber::getFramesPerSecond () const
+{
+ boost::mutex::scoped_lock lock (fps_mutex_);
+ return (frequency_.getFrequency ());
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::openTcpPort (const int port)
+{
+ try
+ {
+ nxLibOpenTcpPort (port);
+ tcp_open_ = true;
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "openTcpPort");
+ return (false);
+ }
+ return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::closeTcpPort ()
+{
+ try
+ {
+ nxLibCloseTcpPort ();
+ tcp_open_ = false;
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "closeTcpPort");
+ return (false);
+ }
+ return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+std::string
+pcl::EnsensoGrabber::getTreeAsJson (const bool pretty_format) const
+{
+ try
+ {
+ return (root_->asJson (pretty_format));
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "getTreeAsJson");
+ return ("");
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+std::string
+pcl::EnsensoGrabber::getResultAsJson (const bool pretty_format) const
+{
+ try
+ {
+ NxLibCommand cmd ("");
+ return (cmd.result ().asJson (pretty_format));
+ }
+
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "getResultAsJson");
+ return ("");
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::jsonTransformationToEulerAngles (const std::string &json,
+ double &x,
+ double &y,
+ double &z,
+ double &w,
+ double &p,
+ double &r) const
+{
+ try
+ {
+ NxLibCommand convert (cmdConvertTransformation);
+ convert.parameters ()[itmTransformation].setJson (json, false);
+ convert.parameters ()[itmSplitRotation].set (valXYZ);
+
+ convert.execute ();
+
+ NxLibItem tf = convert.result ()[itmTransformations];
+ x = tf[0][itmTranslation][0].asDouble ();
+ y = tf[0][itmTranslation][1].asDouble ();
+ z = tf[0][itmTranslation][2].asDouble ();
+ r = tf[0][itmRotation][itmAngle].asDouble (); // Roll
+ p = tf[1][itmRotation][itmAngle].asDouble (); // Pitch
+ w = tf[2][itmRotation][itmAngle].asDouble (); // yaW
+ return (true);
+ }
+
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "jsonTransformationToEulerAngles");
+ return (false);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::jsonTransformationToAngleAxis (const std::string json,
+ double &alpha,
+ Eigen::Vector3d &axis,
+ Eigen::Vector3d &translation) const
+{
+ try
+ {
+ NxLibItem tf ("/tmpTF");
+ tf.setJson(json);
+ translation[0] = tf[itmTranslation][0].asDouble ();
+ translation[1] = tf[itmTranslation][1].asDouble ();
+ translation[2] = tf[itmTranslation][2].asDouble ();
+
+ alpha = tf[itmRotation][itmAngle].asDouble (); // Angle of rotation
+ axis[0] = tf[itmRotation][itmAxis][0].asDouble (); // X component of Euler vector
+ axis[1] = tf[itmRotation][itmAxis][1].asDouble (); // Y component of Euler vector
+ axis[2] = tf[itmRotation][itmAxis][2].asDouble (); // Z component of Euler vector
+ tf.erase(); // Delete tmpTF node
+ return (true);
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "jsonTransformationToAngleAxis");
+ return (false);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::jsonTransformationToMatrix (const std::string transformation,
+ Eigen::Affine3d &matrix) const
+{
+ try
+ {
+ NxLibCommand convert_transformation (cmdConvertTransformation);
+ convert_transformation.parameters ()[itmTransformation].setJson (transformation);
+ convert_transformation.execute ();
+ Eigen::Affine3d tmp (Eigen::Affine3d::Identity ());
+
+ // Rotation
+ tmp.linear ().col (0) = Eigen::Vector3d (convert_transformation.result ()[itmTransformation][0][0].asDouble (),
+ convert_transformation.result ()[itmTransformation][0][1].asDouble (),
+ convert_transformation.result ()[itmTransformation][0][2].asDouble ());
+
+ tmp.linear ().col (1) = Eigen::Vector3d (convert_transformation.result ()[itmTransformation][1][0].asDouble (),
+ convert_transformation.result ()[itmTransformation][1][1].asDouble (),
+ convert_transformation.result ()[itmTransformation][1][2].asDouble ());
+
+ tmp.linear ().col (2) = Eigen::Vector3d (convert_transformation.result ()[itmTransformation][2][0].asDouble (),
+ convert_transformation.result ()[itmTransformation][2][1].asDouble (),
+ convert_transformation.result ()[itmTransformation][2][2].asDouble ());
+
+ // Translation
+ tmp.translation () = Eigen::Vector3d (convert_transformation.result ()[itmTransformation][3][0].asDouble (),
+ convert_transformation.result ()[itmTransformation][3][1].asDouble (),
+ convert_transformation.result ()[itmTransformation][3][2].asDouble ());
+ matrix = tmp;
+ return (true);
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "jsonTransformationToMatrix");
+ return (false);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::eulerAnglesTransformationToJson (const double x,
+ const double y,
+ const double z,
+ const double w,
+ const double p,
+ const double r,
+ std::string &json,
+ const bool pretty_format) const
+{
+ try
+ {
+ NxLibCommand chain (cmdChainTransformations);
+ NxLibItem tf = chain.parameters ()[itmTransformations];
+
+ if (!angleAxisTransformationToJson (x, y, z, 0, 0, 1, r, json))
+ return (false);
+ tf[0].setJson (json, false); // Roll
+
+ if (!angleAxisTransformationToJson (0, 0, 0, 0, 1, 0, p, json))
+ return (false);
+ tf[1].setJson (json, false); // Pitch
+
+ if (!angleAxisTransformationToJson (0, 0, 0, 1, 0, 0, w, json))
+ return (false);
+ tf[2].setJson (json, false); // yaW
+
+ chain.execute ();
+ json = chain.result ()[itmTransformation].asJson (pretty_format);
+ return (true);
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "eulerAnglesTransformationToJson");
+ return (false);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::angleAxisTransformationToJson (const double x,
+ const double y,
+ const double z,
+ const double rx,
+ const double ry,
+ const double rz,
+ const double alpha,
+ std::string &json,
+ const bool pretty_format) const
+{
+ try
+ {
+ NxLibItem tf ("/tmpTF");
+ tf[itmTranslation][0].set (x);
+ tf[itmTranslation][1].set (y);
+ tf[itmTranslation][2].set (z);
+
+ tf[itmRotation][itmAngle].set (alpha); // Angle of rotation
+ tf[itmRotation][itmAxis][0].set (rx); // X component of Euler vector
+ tf[itmRotation][itmAxis][1].set (ry); // Y component of Euler vector
+ tf[itmRotation][itmAxis][2].set (rz); // Z component of Euler vector
+
+ json = tf.asJson (pretty_format);
+ tf.erase ();
+ return (true);
+ }
+
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "angleAxisTransformationToJson");
+ return (false);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::EnsensoGrabber::matrixTransformationToJson (const Eigen::Affine3d &matrix,
+ std::string &json,
+ const bool pretty_format) const
+{
+ try
+ {
+ NxLibCommand convert_transformation (cmdConvertTransformation);
+ // Rotation
+ convert_transformation.parameters ()[itmTransformation][0][0].set (matrix.linear ().col (0)[0]);
+ convert_transformation.parameters ()[itmTransformation][0][1].set (matrix.linear ().col (0)[1]);
+ convert_transformation.parameters ()[itmTransformation][0][2].set (matrix.linear ().col (0)[2]);
+ convert_transformation.parameters ()[itmTransformation][0][3].set (0.0);
+
+ convert_transformation.parameters ()[itmTransformation][1][0].set (matrix.linear ().col (1)[0]);
+ convert_transformation.parameters ()[itmTransformation][1][1].set (matrix.linear ().col (1)[1]);
+ convert_transformation.parameters ()[itmTransformation][1][2].set (matrix.linear ().col (1)[2]);
+ convert_transformation.parameters ()[itmTransformation][1][3].set (0.0);
+
+ convert_transformation.parameters ()[itmTransformation][2][0].set (matrix.linear ().col (2)[0]);
+ convert_transformation.parameters ()[itmTransformation][2][1].set (matrix.linear ().col (2)[1]);
+ convert_transformation.parameters ()[itmTransformation][2][2].set (matrix.linear ().col (2)[2]);
+ convert_transformation.parameters ()[itmTransformation][2][3].set (0.0);
+
+ // Translation
+ convert_transformation.parameters ()[itmTransformation][3][0].set (matrix.translation ()[0]);
+ convert_transformation.parameters ()[itmTransformation][3][1].set (matrix.translation ()[1]);
+ convert_transformation.parameters ()[itmTransformation][3][2].set (matrix.translation ()[2]);
+ convert_transformation.parameters ()[itmTransformation][3][3].set (1.0);
+
+ convert_transformation.execute ();
+ json = convert_transformation.result ()[itmTransformation].asJson (pretty_format);
+ return (true);
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "matrixTransformationToJson");
+ return (false);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::uint64_t
+pcl::EnsensoGrabber::getPCLStamp (const double ensenso_stamp)
+{
+#if defined _WIN32 || defined _WIN64
+ return (ensenso_stamp * 1000000.0);
+#else
+ return ( (ensenso_stamp - 11644473600.0) * 1000000.0);
+#endif
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+std::string
+pcl::EnsensoGrabber::getOpenCVType (const int channels,
+ const int bpe,
+ const bool isFlt)
+{
+ int bits = bpe * 8;
+ char type = isFlt ? 'F' : (bpe > 3 ? 'S' : 'U');
+ return (boost::str (boost::format ("CV_%i%cC%i") % bits % type % channels));
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::EnsensoGrabber::processGrabbing ()
+{
+ bool continue_grabbing = running_;
+ while (continue_grabbing)
+ {
+ try
+ {
+ // Publish cloud / images
+ if (num_slots<sig_cb_ensenso_point_cloud> () > 0 || num_slots<sig_cb_ensenso_images> () > 0 || num_slots<sig_cb_ensenso_point_cloud_images> () > 0)
+ {
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ boost::shared_ptr<PairOfImages> images (new PairOfImages);
+
+ fps_mutex_.lock ();
+ frequency_.event ();
+ fps_mutex_.unlock ();
+
+ NxLibCommand (cmdCapture).execute ();
+ double timestamp;
+ camera_[itmImages][itmRaw][itmLeft].getBinaryDataInfo (0, 0, 0, 0, 0, ×tamp);
+
+ // Gather images
+ if (num_slots<sig_cb_ensenso_images> () > 0 || num_slots<sig_cb_ensenso_point_cloud_images> () > 0)
+ {
+ // Rectify images
+ NxLibCommand (cmdRectifyImages).execute ();
+ int width, height, channels, bpe;
+ bool isFlt, collected_pattern = false;
+
+ try // Try to collect calibration pattern, if not possible, publish RAW images instead
+ {
+ NxLibCommand collect_pattern (cmdCollectPattern);
+ collect_pattern.parameters ()[itmBuffer].set (false); // Do NOT store the pattern into the buffer!
+ collect_pattern.execute ();
+ collected_pattern = true;
+ }
+ catch (const NxLibException &ex)
+ {
+ // We failed to collect the pattern but the RAW images are available!
+ }
+
+ if (collected_pattern)
+ {
+ camera_[itmImages][itmWithOverlay][itmLeft].getBinaryDataInfo (&width, &height, &channels, &bpe, &isFlt, 0);
+ images->first.header.stamp = images->second.header.stamp = getPCLStamp (timestamp);
+ images->first.width = images->second.width = width;
+ images->first.height = images->second.height = height;
+ images->first.data.resize (width * height * sizeof(float));
+ images->second.data.resize (width * height * sizeof(float));
+ images->first.encoding = images->second.encoding = getOpenCVType (channels, bpe, isFlt);
+
+ camera_[itmImages][itmWithOverlay][itmLeft].getBinaryData (images->first.data.data (), images->first.data.size (), 0, 0);
+ camera_[itmImages][itmWithOverlay][itmRight].getBinaryData (images->second.data.data (), images->second.data.size (), 0, 0);
+ }
+ else
+ {
+ camera_[itmImages][itmRaw][itmLeft].getBinaryDataInfo (&width, &height, &channels, &bpe, &isFlt, 0);
+ images->first.header.stamp = images->second.header.stamp = getPCLStamp (timestamp);
+ images->first.width = images->second.width = width;
+ images->first.height = images->second.height = height;
+ images->first.data.resize (width * height * sizeof(float));
+ images->second.data.resize (width * height * sizeof(float));
+ images->first.encoding = images->second.encoding = getOpenCVType (channels, bpe, isFlt);
+
+ camera_[itmImages][itmRaw][itmLeft].getBinaryData (images->first.data.data (), images->first.data.size (), 0, 0);
+ camera_[itmImages][itmRaw][itmRight].getBinaryData (images->second.data.data (), images->second.data.size (), 0, 0);
+ }
+ }
+
+ // Gather point cloud
+ if (num_slots<sig_cb_ensenso_point_cloud> () > 0 || num_slots<sig_cb_ensenso_point_cloud_images> () > 0)
+ {
+ // Stereo matching task
+ NxLibCommand (cmdComputeDisparityMap).execute ();
+
+ // Convert disparity map into XYZ data for each pixel
+ NxLibCommand (cmdComputePointMap).execute ();
+
+ // Get info about the computed point map and copy it into a std::vector
+ std::vector<float> pointMap;
+ int width, height;
+ camera_[itmImages][itmPointMap].getBinaryDataInfo (&width, &height, 0, 0, 0, 0);
+ camera_[itmImages][itmPointMap].getBinaryData (pointMap, 0);
+
+ // Copy point cloud and convert in meters
+ cloud->header.stamp = getPCLStamp (timestamp);
+ cloud->points.resize (height * width);
+ cloud->width = width;
+ cloud->height = height;
+ cloud->is_dense = false;
+
+ // Copy data in point cloud (and convert milimeters in meters)
+ for (size_t i = 0; i < pointMap.size (); i += 3)
+ {
+ cloud->points[i / 3].x = pointMap[i] / 1000.0;
+ cloud->points[i / 3].y = pointMap[i + 1] / 1000.0;
+ cloud->points[i / 3].z = pointMap[i + 2] / 1000.0;
+ }
+ }
+
+ // Publish signals
+ if (num_slots<sig_cb_ensenso_point_cloud_images> () > 0)
+ point_cloud_images_signal_->operator () (cloud, images);
+ else if (num_slots<sig_cb_ensenso_point_cloud> () > 0)
+ point_cloud_signal_->operator () (cloud);
+ else if (num_slots<sig_cb_ensenso_images> () > 0)
+ images_signal_->operator () (images);
+ }
+ continue_grabbing = running_;
+ }
+ catch (NxLibException &ex)
+ {
+ ensensoExceptionHandling (ex, "processGrabbing");
+ }
+ }
+}
+
*/
#include <pcl/io/file_io.h>
-#include <pcl/io/boost.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/io/ply_io.h>
-#include <pcl/io/ifs_io.h>
-#include <pcl/io/obj_io.h>
-#include <pcl/io/vtk_io.h>
-namespace pcl
-{
- namespace io
- {
- template<typename PointT> int
- load (const std::string& file_name, pcl::PointCloud<PointT>& cloud)
- {
- boost::filesystem::path p (file_name.c_str ());
- std::string extension = p.extension ().string ();
- int result = -1;
- if (extension == ".pcd")
- result = pcl::io::loadPCDFile (file_name, cloud);
- else if (extension == ".ply")
- result = pcl::io::loadPLYFile (file_name, cloud);
- else if (extension == ".ifs")
- result = pcl::io::loadIFSFile (file_name, cloud);
- else
- {
- PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s", extension.c_str ());
- result = -1;
- }
- return (result);
- }
-
- template<typename PointT> int
- save (const std::string& file_name, const pcl::PointCloud<PointT>& cloud)
- {
- boost::filesystem::path p (file_name.c_str ());
- std::string extension = p.extension ().string ();
- int result = -1;
- if (extension == ".pcd")
- result = pcl::io::savePCDFile (file_name, cloud, true);
- else if (extension == ".ply")
- result = pcl::io::savePLYFile (file_name, cloud, true);
- else if (extension == ".ifs")
- result = pcl::io::saveIFSFile (file_name, cloud);
- else
- {
- PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s", extension.c_str ());
- result = -1;
- }
- return (result);
- }
- }
-}
-
-int
-pcl::io::load (const std::string& file_name, pcl::PCLPointCloud2& blob)
-{
- boost::filesystem::path p (file_name.c_str ());
- std::string extension = p.extension ().string ();
- int result = -1;
- if (extension == ".pcd")
- result = pcl::io::loadPCDFile (file_name, blob);
- else if (extension == ".ply")
- result = pcl::io::loadPLYFile (file_name, blob);
- else if (extension == ".ifs")
- result = pcl::io::loadIFSFile (file_name, blob);
- else if (extension == ".obj")
- result = pcl::io::loadOBJFile (file_name, blob);
- else
- {
- PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s", extension.c_str ());
- result = -1;
- }
- return (result);
-}
-
-int
-pcl::io::load (const std::string& file_name, pcl::PolygonMesh& mesh)
-{
- boost::filesystem::path p (file_name.c_str ());
- std::string extension = p.extension ().string ();
- int result = -1;
- if (extension == ".ply")
- result = pcl::io::loadPLYFile (file_name, mesh);
- else if (extension == ".ifs")
- result = pcl::io::loadIFSFile (file_name, mesh);
- else if (extension == ".obj")
- result = pcl::io::loadOBJFile (file_name, mesh);
- else
- {
- PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s", extension.c_str ());
- result = -1;
- }
- return (result);
-}
-
-int
-pcl::io::load (const std::string& file_name, pcl::TextureMesh& mesh)
-{
- boost::filesystem::path p (file_name.c_str ());
- std::string extension = p.extension ().string ();
- int result = -1;
- if (extension == ".obj")
- result = pcl::io::loadOBJFile (file_name, mesh);
- else
- {
- PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s", extension.c_str ());
- result = -1;
- }
- return (result);
-}
-
-int
-pcl::io::save (const std::string& file_name, const pcl::PCLPointCloud2& blob, unsigned precision)
-{
- boost::filesystem::path p (file_name.c_str ());
- std::string extension = p.extension ().string ();
- int result = -1;
- if (extension == ".pcd")
- {
- Eigen::Vector4f origin = Eigen::Vector4f::Zero ();
- Eigen::Quaternionf orientation = Eigen::Quaternionf::Identity ();
- result = pcl::io::savePCDFile (file_name, blob, origin, orientation, true);
- }
- else if (extension == ".ply")
- {
- Eigen::Vector4f origin = Eigen::Vector4f::Zero ();
- Eigen::Quaternionf orientation = Eigen::Quaternionf::Identity ();
- result = pcl::io::savePLYFile (file_name, blob, origin, orientation, true);
- }
- else if (extension == ".ifs")
- result = pcl::io::saveIFSFile (file_name, blob);
- else if (extension == ".vtk")
- result = pcl::io::saveVTKFile (file_name, blob, precision);
- else
- {
- PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s", extension.c_str ());
- result = -1;
- }
- return (result);
-}
-
-int
-pcl::io::save (const std::string &file_name, const pcl::TextureMesh &tex_mesh, unsigned precision)
-{
- boost::filesystem::path p (file_name.c_str ());
- std::string extension = p.extension ().string ();
- int result = -1;
- if (extension == ".obj")
- result = pcl::io::saveOBJFile (file_name, tex_mesh, precision);
- else
- {
- PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s", extension.c_str ());
- result = -1;
- }
- return (result);
-}
-
-int
-pcl::io::save (const std::string &file_name, const pcl::PolygonMesh &poly_mesh, unsigned precision)
-{
- boost::filesystem::path p (file_name.c_str ());
- std::string extension = p.extension ().string ();
- int result = -1;
- if (extension == ".ply")
- result = pcl::io::savePLYFileBinary (file_name, poly_mesh);
- else if (extension == ".obj")
- result = pcl::io::saveOBJFile (file_name, poly_mesh, precision);
- else if (extension == ".vtk")
- result = pcl::io::saveVTKFile (file_name, poly_mesh, precision);
- else
- {
- PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s", extension.c_str ());
- result = -1;
- }
- return (result);
-}
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
- * Copyright (c) 2012 The MITRE Corporation
+ * Copyright (c) 2012-, Open Perception, Inc.
+ * Copyright (c) 2012,2015 The MITRE Corporation
*
* All rights reserved.
*
#include <pcap.h>
#endif // #ifdef HAVE_PCAP
-const boost::asio::ip::address pcl::HDLGrabber::HDL_DEFAULT_NETWORK_ADDRESS = boost::asio::ip::address::from_string ("192.168.3.255");
double *pcl::HDLGrabber::cos_lookup_table_ = NULL;
double *pcl::HDLGrabber::sin_lookup_table_ = NULL;
/////////////////////////////////////////////////////////////////////////////
pcl::HDLGrabber::HDLGrabber (const std::string& correctionsFile,
- const std::string& pcapFile)
- : hdl_data_ ()
- , udp_listener_endpoint_ (HDL_DEFAULT_NETWORK_ADDRESS, HDL_DATA_PORT)
- , source_address_filter_ ()
- , source_port_filter_ (443)
- , hdl_read_socket_service_ ()
- , hdl_read_socket_ (NULL)
- , pcap_file_name_ (pcapFile)
- , queue_consumer_thread_ (NULL)
- , hdl_read_packet_thread_ (NULL)
- , current_scan_xyz_ (new pcl::PointCloud<pcl::PointXYZ> ())
- , current_sweep_xyz_ (new pcl::PointCloud<pcl::PointXYZ> ())
- , current_scan_xyzi_ (new pcl::PointCloud<pcl::PointXYZI> ())
- , current_sweep_xyzi_ (new pcl::PointCloud<pcl::PointXYZI> ())
- , current_scan_xyzrgb_ (new pcl::PointCloud<pcl::PointXYZRGBA> ())
- , current_sweep_xyzrgb_ (new pcl::PointCloud<pcl::PointXYZRGBA> ())
- , last_azimuth_ (65000)
- , sweep_xyz_signal_ ()
- , sweep_xyzrgb_signal_ ()
- , sweep_xyzi_signal_ ()
- , scan_xyz_signal_ ()
- , scan_xyzrgb_signal_ ()
- , scan_xyzi_signal_ ()
- , min_distance_threshold_(0.0)
- , max_distance_threshold_(10000.0)
+ const std::string& pcapFile) :
+ last_azimuth_ (65000),
+ current_scan_xyz_ (new pcl::PointCloud<pcl::PointXYZ> ()),
+ current_sweep_xyz_ (new pcl::PointCloud<pcl::PointXYZ> ()),
+ current_scan_xyzi_ (new pcl::PointCloud<pcl::PointXYZI> ()),
+ current_sweep_xyzi_ (new pcl::PointCloud<pcl::PointXYZI> ()),
+ current_scan_xyzrgb_ (new pcl::PointCloud<pcl::PointXYZRGBA> ()),
+ current_sweep_xyzrgb_ (new pcl::PointCloud<pcl::PointXYZRGBA> ()),
+ sweep_xyz_signal_ (),
+ sweep_xyzrgb_signal_ (),
+ sweep_xyzi_signal_ (),
+ scan_xyz_signal_ (),
+ scan_xyzrgb_signal_ (),
+ scan_xyzi_signal_ (),
+ hdl_data_ (),
+ udp_listener_endpoint_ (),
+ source_address_filter_ (),
+ source_port_filter_ (443),
+ hdl_read_socket_service_ (),
+ hdl_read_socket_ (NULL),
+ pcap_file_name_ (pcapFile),
+ queue_consumer_thread_ (NULL),
+ hdl_read_packet_thread_ (NULL),
+ min_distance_threshold_ (0.0),
+ max_distance_threshold_ (10000.0)
{
initialize (correctionsFile);
}
/////////////////////////////////////////////////////////////////////////////
pcl::HDLGrabber::HDLGrabber (const boost::asio::ip::address& ipAddress,
- const unsigned short int port,
- const std::string& correctionsFile)
- : hdl_data_ ()
- , udp_listener_endpoint_ (ipAddress, port)
- , source_address_filter_ ()
- , source_port_filter_ (443)
- , hdl_read_socket_service_ ()
- , hdl_read_socket_ (NULL)
- , pcap_file_name_ ()
- , queue_consumer_thread_ (NULL)
- , hdl_read_packet_thread_ (NULL)
- , current_scan_xyz_ (new pcl::PointCloud<pcl::PointXYZ> ())
- , current_sweep_xyz_ (new pcl::PointCloud<pcl::PointXYZ> ())
- , current_scan_xyzi_ (new pcl::PointCloud<pcl::PointXYZI> ())
- , current_sweep_xyzi_ (new pcl::PointCloud<pcl::PointXYZI> ())
- , current_scan_xyzrgb_ (new pcl::PointCloud<pcl::PointXYZRGBA> ())
- , current_sweep_xyzrgb_ (new pcl::PointCloud<pcl::PointXYZRGBA> ())
- , last_azimuth_ (65000)
- , sweep_xyz_signal_ ()
- , sweep_xyzrgb_signal_ ()
- , sweep_xyzi_signal_ ()
- , scan_xyz_signal_ ()
- , scan_xyzrgb_signal_ ()
- , scan_xyzi_signal_ ()
- , min_distance_threshold_(0.0)
- , max_distance_threshold_(10000.0)
+ const unsigned short int port,
+ const std::string& correctionsFile) :
+ last_azimuth_ (65000),
+ current_scan_xyz_ (new pcl::PointCloud<pcl::PointXYZ> ()),
+ current_sweep_xyz_ (new pcl::PointCloud<pcl::PointXYZ> ()),
+ current_scan_xyzi_ (new pcl::PointCloud<pcl::PointXYZI> ()),
+ current_sweep_xyzi_ (new pcl::PointCloud<pcl::PointXYZI> ()),
+ current_scan_xyzrgb_ (new pcl::PointCloud<pcl::PointXYZRGBA> ()),
+ current_sweep_xyzrgb_ (new pcl::PointCloud<pcl::PointXYZRGBA> ()),
+ sweep_xyz_signal_ (),
+ sweep_xyzrgb_signal_ (),
+ sweep_xyzi_signal_ (),
+ scan_xyz_signal_ (),
+ scan_xyzrgb_signal_ (),
+ scan_xyzi_signal_ (),
+ hdl_data_ (),
+ udp_listener_endpoint_ (ipAddress, port),
+ source_address_filter_ (),
+ source_port_filter_ (443),
+ hdl_read_socket_service_ (),
+ hdl_read_socket_ (NULL),
+ pcap_file_name_ (),
+ queue_consumer_thread_ (NULL),
+ hdl_read_packet_thread_ (NULL),
+ min_distance_threshold_ (0.0),
+ max_distance_threshold_ (10000.0)
{
initialize (correctionsFile);
}
{
cos_lookup_table_ = static_cast<double *> (malloc (HDL_NUM_ROT_ANGLES * sizeof (*cos_lookup_table_)));
sin_lookup_table_ = static_cast<double *> (malloc (HDL_NUM_ROT_ANGLES * sizeof (*sin_lookup_table_)));
- for (unsigned int i = 0; i < HDL_NUM_ROT_ANGLES; i++)
+ for (int i = 0; i < HDL_NUM_ROT_ANGLES; i++)
{
double rad = (M_PI / 180.0) * (static_cast<double> (i) / 100.0);
cos_lookup_table_[i] = std::cos (rad);
for (int i = 0; i < HDL_MAX_NUM_LASERS; i++)
{
HDLLaserCorrection correction = laser_corrections_[i];
- laser_corrections_[i].sinVertOffsetCorrection = correction.verticalOffsetCorrection
- * correction.sinVertCorrection;
- laser_corrections_[i].cosVertOffsetCorrection = correction.verticalOffsetCorrection
- * correction.cosVertCorrection;
+ laser_corrections_[i].sinVertOffsetCorrection = correction.verticalOffsetCorrection * correction.sinVertCorrection;
+ laser_corrections_[i].cosVertOffsetCorrection = correction.verticalOffsetCorrection * correction.cosVertCorrection;
}
sweep_xyz_signal_ = createSignal<sig_cb_velodyne_hdl_sweep_point_cloud_xyz> ();
sweep_xyzrgb_signal_ = createSignal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb> ();
- sweep_xyzi_signal_ =createSignal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzi> ();
+ sweep_xyzi_signal_ = createSignal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzi> ();
scan_xyz_signal_ = createSignal<sig_cb_velodyne_hdl_scan_point_cloud_xyz> ();
scan_xyzrgb_signal_ = createSignal<sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb> ();
scan_xyzi_signal_ = createSignal<sig_cb_velodyne_hdl_scan_point_cloud_xyzi> ();
{
if (px.first == "px")
{
- boost::property_tree::ptree calibrationData = px.second;
+ boost::property_tree::ptree calibration_data = px.second;
int index = -1;
- double azimuth = 0, vertCorrection = 0, distCorrection = 0,
- vertOffsetCorrection = 0, horizOffsetCorrection = 0;
+ double azimuth = 0, vert_correction = 0, dist_correction = 0, vert_offset_correction = 0, horiz_offset_correction = 0;
- BOOST_FOREACH (boost::property_tree::ptree::value_type &item, calibrationData)
+ BOOST_FOREACH (boost::property_tree::ptree::value_type &item, calibration_data)
{
if (item.first == "id_")
index = atoi (item.second.data ().c_str ());
if (item.first == "rotCorrection_")
azimuth = atof (item.second.data ().c_str ());
if (item.first == "vertCorrection_")
- vertCorrection = atof (item.second.data ().c_str ());
+ vert_correction = atof (item.second.data ().c_str ());
if (item.first == "distCorrection_")
- distCorrection = atof (item.second.data ().c_str ());
+ dist_correction = atof (item.second.data ().c_str ());
if (item.first == "vertOffsetCorrection_")
- vertOffsetCorrection = atof (item.second.data ().c_str ());
+ vert_offset_correction = atof (item.second.data ().c_str ());
if (item.first == "horizOffsetCorrection_")
- horizOffsetCorrection = atof (item.second.data ().c_str ());
+ horiz_offset_correction = atof (item.second.data ().c_str ());
}
if (index != -1)
{
laser_corrections_[index].azimuthCorrection = azimuth;
- laser_corrections_[index].verticalCorrection = vertCorrection;
- laser_corrections_[index].distanceCorrection = distCorrection / 100.0;
- laser_corrections_[index].verticalOffsetCorrection = vertOffsetCorrection / 100.0;
- laser_corrections_[index].horizontalOffsetCorrection = horizOffsetCorrection / 100.0;
+ laser_corrections_[index].verticalCorrection = vert_correction;
+ laser_corrections_[index].distanceCorrection = dist_correction / 100.0;
+ laser_corrections_[index].verticalOffsetCorrection = vert_offset_correction / 100.0;
+ laser_corrections_[index].horizontalOffsetCorrection = horiz_offset_correction / 100.0;
laser_corrections_[index].cosVertCorrection = std::cos (HDL_Grabber_toRadians(laser_corrections_[index].verticalCorrection));
laser_corrections_[index].sinVertCorrection = std::sin (HDL_Grabber_toRadians(laser_corrections_[index].verticalCorrection));
void
pcl::HDLGrabber::loadHDL32Corrections ()
{
- double hdl32VerticalCorrections[] = {
- -30.67, -9.3299999, -29.33, -8, -28,
- -6.6700001, -26.67, -5.3299999, -25.33, -4, -24, -2.6700001, -22.67,
- -1.33, -21.33, 0, -20, 1.33, -18.67, 2.6700001, -17.33, 4, -16, 5.3299999,
- -14.67, 6.6700001, -13.33, 8, -12, 9.3299999, -10.67, 10.67 };
+ double hdl32_vertical_corrections[] = { -30.67, -9.3299999, -29.33, -8, -28, -6.6700001, -26.67, -5.3299999, -25.33, -4, -24, -2.6700001, -22.67, -1.33, -21.33,
+ 0, -20, 1.33, -18.67, 2.6700001, -17.33, 4, -16, 5.3299999, -14.67, 6.6700001, -13.33, 8, -12, 9.3299999, -10.67, 10.67 };
for (int i = 0; i < HDL_LASER_PER_FIRING; i++)
{
laser_corrections_[i].azimuthCorrection = 0.0;
laser_corrections_[i].distanceCorrection = 0.0;
laser_corrections_[i].horizontalOffsetCorrection = 0.0;
laser_corrections_[i].verticalOffsetCorrection = 0.0;
- laser_corrections_[i].verticalCorrection = hdl32VerticalCorrections[i];
- laser_corrections_[i].sinVertCorrection = std::sin (HDL_Grabber_toRadians(hdl32VerticalCorrections[i]));
- laser_corrections_[i].cosVertCorrection = std::cos (HDL_Grabber_toRadians(hdl32VerticalCorrections[i]));
+ laser_corrections_[i].verticalCorrection = hdl32_vertical_corrections[i];
+ laser_corrections_[i].sinVertCorrection = std::sin (HDL_Grabber_toRadians(hdl32_vertical_corrections[i]));
+ laser_corrections_[i].cosVertCorrection = std::cos (HDL_Grabber_toRadians(hdl32_vertical_corrections[i]));
}
for (int i = HDL_LASER_PER_FIRING; i < HDL_MAX_NUM_LASERS; i++)
{
}
}
+/////////////////////////////////////////////////////////////////////////////
+boost::asio::ip::address
+pcl::HDLGrabber::getDefaultNetworkAddress ()
+{
+ return (boost::asio::ip::address::from_string ("192.168.3.255"));
+}
+
/////////////////////////////////////////////////////////////////////////////
void
pcl::HDLGrabber::processVelodynePackets ()
void
pcl::HDLGrabber::toPointClouds (HDLDataPacket *dataPacket)
{
- static uint32_t scanCounter = 0;
- static uint32_t sweepCounter = 0;
- if (sizeof (HDLLaserReturn) != 3)
+ static uint32_t scan_counter = 0;
+ static uint32_t sweep_counter = 0;
+ if (sizeof(HDLLaserReturn) != 3)
return;
current_scan_xyz_.reset (new pcl::PointCloud<pcl::PointXYZ> ());
current_scan_xyzrgb_.reset (new pcl::PointCloud<pcl::PointXYZRGBA> ());
current_scan_xyzi_.reset (new pcl::PointCloud<pcl::PointXYZI> ());
- time_t time_;
- time(&time_);
- time_t velodyneTime = (time_ & 0x00000000ffffffffl) << 32 | dataPacket->gpsTimestamp;
+ time_t system_time;
+ time (&system_time);
+ time_t velodyne_time = (system_time & 0x00000000ffffffffl) << 32 | dataPacket->gpsTimestamp;
- current_scan_xyz_->header.stamp = velodyneTime;
- current_scan_xyzrgb_->header.stamp = velodyneTime;
- current_scan_xyzi_->header.stamp = velodyneTime;
- current_scan_xyz_->header.seq = scanCounter;
- current_scan_xyzrgb_->header.seq = scanCounter;
- current_scan_xyzi_->header.seq = scanCounter;
- scanCounter++;
+ current_scan_xyz_->header.stamp = velodyne_time;
+ current_scan_xyzrgb_->header.stamp = velodyne_time;
+ current_scan_xyzi_->header.stamp = velodyne_time;
+ current_scan_xyz_->header.seq = scan_counter;
+ current_scan_xyzrgb_->header.seq = scan_counter;
+ current_scan_xyzi_->header.seq = scan_counter;
+ scan_counter++;
for (int i = 0; i < HDL_FIRING_PER_PKT; ++i)
{
- HDLFiringData firingData = dataPacket->firingData[i];
- int offset = (firingData.blockIdentifier == BLOCK_0_TO_31) ? 0 : 32;
+ HDLFiringData firing_data = dataPacket->firingData[i];
+ int offset = (firing_data.blockIdentifier == BLOCK_0_TO_31) ? 0 : 32;
for (int j = 0; j < HDL_LASER_PER_FIRING; j++)
{
- if (firingData.rotationalPosition < last_azimuth_)
+ if (firing_data.rotationalPosition < last_azimuth_)
{
if (current_sweep_xyzrgb_->size () > 0)
{
current_sweep_xyz_->is_dense = current_sweep_xyzrgb_->is_dense = current_sweep_xyzi_->is_dense = false;
- current_sweep_xyz_->header.stamp = velodyneTime;
- current_sweep_xyzrgb_->header.stamp = velodyneTime;
- current_sweep_xyzi_->header.stamp = velodyneTime;
- current_sweep_xyz_->header.seq = sweepCounter;
- current_sweep_xyzrgb_->header.seq = sweepCounter;
- current_sweep_xyzi_->header.seq = sweepCounter;
+ current_sweep_xyz_->header.stamp = velodyne_time;
+ current_sweep_xyzrgb_->header.stamp = velodyne_time;
+ current_sweep_xyzi_->header.stamp = velodyne_time;
+ current_sweep_xyz_->header.seq = sweep_counter;
+ current_sweep_xyzrgb_->header.seq = sweep_counter;
+ current_sweep_xyzi_->header.seq = sweep_counter;
- sweepCounter++;
+ sweep_counter++;
fireCurrentSweep ();
}
PointXYZI xyzi;
PointXYZRGBA xyzrgb;
- computeXYZI (xyzi, firingData.rotationalPosition, firingData.laserReturns[j], laser_corrections_[j + offset]);
+ computeXYZI (xyzi, firing_data.rotationalPosition, firing_data.laserReturns[j], laser_corrections_[j + offset]);
xyz.x = xyzrgb.x = xyzi.x;
xyz.y = xyzrgb.y = xyzi.y;
xyz.z = xyzrgb.z = xyzi.z;
xyzrgb.rgba = laser_rgb_mapping_[j + offset].rgba;
- if ((boost::math::isnan)(xyz.x) ||
- (boost::math::isnan)(xyz.y) ||
- (boost::math::isnan)(xyz.z)) {
+ if ( (boost::math::isnan) (xyz.x) || (boost::math::isnan) (xyz.y) || (boost::math::isnan) (xyz.z))
+ {
continue;
}
current_sweep_xyzi_->push_back (xyzi);
current_sweep_xyzrgb_->push_back (xyzrgb);
- last_azimuth_ = firingData.rotationalPosition;
+ last_azimuth_ = firing_data.rotationalPosition;
}
}
current_scan_xyz_->is_dense = current_scan_xyzrgb_->is_dense = current_scan_xyzi_->is_dense = true;
- fireCurrentScan (dataPacket->firingData[0].rotationalPosition,
- dataPacket->firingData[11].rotationalPosition);
+ fireCurrentScan (dataPacket->firingData[0].rotationalPosition, dataPacket->firingData[11].rotationalPosition);
}
/////////////////////////////////////////////////////////////////////////////
void
-pcl::HDLGrabber::computeXYZI (pcl::PointXYZI& point, int azimuth,
- HDLLaserReturn laserReturn, HDLLaserCorrection correction)
+pcl::HDLGrabber::computeXYZI (pcl::PointXYZI& point,
+ int azimuth,
+ HDLLaserReturn laserReturn,
+ HDLLaserCorrection correction)
{
- double cosAzimuth, sinAzimuth;
+ double cos_azimuth, sin_azimuth;
double distanceM = laserReturn.distance * 0.002;
- if (distanceM < min_distance_threshold_ || distanceM > max_distance_threshold_) {
- point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN();
- point.intensity = static_cast<float> (laserReturn.intensity);
+ point.intensity = static_cast<float> (laserReturn.intensity);
+ if (distanceM < min_distance_threshold_ || distanceM > max_distance_threshold_)
+ {
+ point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
return;
}
if (correction.azimuthCorrection == 0)
{
- cosAzimuth = cos_lookup_table_[azimuth];
- sinAzimuth = sin_lookup_table_[azimuth];
+ cos_azimuth = cos_lookup_table_[azimuth];
+ sin_azimuth = sin_lookup_table_[azimuth];
}
else
{
- double azimuthInRadians = HDL_Grabber_toRadians ((static_cast<double> (azimuth) / 100.0) - correction.azimuthCorrection);
- cosAzimuth = std::cos (azimuthInRadians);
- sinAzimuth = std::sin (azimuthInRadians);
+ double azimuthInRadians = HDL_Grabber_toRadians( (static_cast<double> (azimuth) / 100.0) - correction.azimuthCorrection);
+ cos_azimuth = std::cos (azimuthInRadians);
+ sin_azimuth = std::sin (azimuthInRadians);
}
distanceM += correction.distanceCorrection;
- double xyDistance = distanceM * correction.cosVertCorrection - correction.sinVertOffsetCorrection;
+ double xyDistance = distanceM * correction.cosVertCorrection;
- point.x = static_cast<float> (xyDistance * sinAzimuth - correction.horizontalOffsetCorrection * cosAzimuth);
- point.y = static_cast<float> (xyDistance * cosAzimuth + correction.horizontalOffsetCorrection * sinAzimuth);
- point.z = static_cast<float> (distanceM * correction.sinVertCorrection + correction.cosVertOffsetCorrection);
- point.intensity = static_cast<float> (laserReturn.intensity);
+ point.x = static_cast<float> (xyDistance * sin_azimuth - correction.horizontalOffsetCorrection * cos_azimuth);
+ point.y = static_cast<float> (xyDistance * cos_azimuth + correction.horizontalOffsetCorrection * sin_azimuth);
+ point.z = static_cast<float> (distanceM * correction.sinVertCorrection + correction.verticalOffsetCorrection);
+ if (point.x == 0 && point.y == 0 && point.z == 0)
+ {
+ point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
+ }
}
/////////////////////////////////////////////////////////////////////////////
void
pcl::HDLGrabber::fireCurrentSweep ()
{
- if (sweep_xyz_signal_->num_slots () > 0)
+ if (sweep_xyz_signal_ != NULL && sweep_xyz_signal_->num_slots () > 0)
sweep_xyz_signal_->operator() (current_sweep_xyz_);
-
- if (sweep_xyzrgb_signal_->num_slots () > 0)
+
+ if (sweep_xyzrgb_signal_ != NULL && sweep_xyzrgb_signal_->num_slots () > 0)
sweep_xyzrgb_signal_->operator() (current_sweep_xyzrgb_);
- if (sweep_xyzi_signal_->num_slots () > 0)
+ if (sweep_xyzi_signal_ != NULL && sweep_xyzi_signal_->num_slots () > 0)
sweep_xyzi_signal_->operator() (current_sweep_xyzi_);
}
/////////////////////////////////////////////////////////////////////////////
void
pcl::HDLGrabber::fireCurrentScan (const unsigned short startAngle,
- const unsigned short endAngle)
+ const unsigned short endAngle)
{
const float start = static_cast<float> (startAngle) / 100.0f;
const float end = static_cast<float> (endAngle) / 100.0f;
/////////////////////////////////////////////////////////////////////////////
void
pcl::HDLGrabber::enqueueHDLPacket (const unsigned char *data,
- std::size_t bytesReceived)
+ std::size_t bytesReceived)
{
if (bytesReceived == 1206)
{
{
try
{
- try {
- hdl_read_socket_ = new udp::socket (hdl_read_socket_service_, udp_listener_endpoint_);
- }
- catch (std::exception bind) {
- delete hdl_read_socket_;
- hdl_read_socket_ = new udp::socket (hdl_read_socket_service_, udp::endpoint(boost::asio::ip::address_v4::any(), udp_listener_endpoint_.port()));
- }
+ try
+ {
+ if (isAddressUnspecified (udp_listener_endpoint_.address ()))
+ {
+ udp_listener_endpoint_.address (getDefaultNetworkAddress ());
+ }
+ if (udp_listener_endpoint_.port () == 0)
+ {
+ udp_listener_endpoint_.port (HDL_DATA_PORT);
+ }
+ hdl_read_socket_ = new udp::socket (hdl_read_socket_service_, udp_listener_endpoint_);
+ }
+ catch (const std::exception& bind)
+ {
+ delete hdl_read_socket_;
+ hdl_read_socket_ = new udp::socket (hdl_read_socket_service_, udp::endpoint (boost::asio::ip::address_v4::any (), udp_listener_endpoint_.port ()));
+ }
hdl_read_socket_service_.run ();
}
catch (std::exception &e)
{
- PCL_ERROR ("[pcl::HDLGrabber::start] Unable to bind to socket! %s\n", e.what());
- return;
+ PCL_ERROR("[pcl::HDLGrabber::start] Unable to bind to socket! %s\n", e.what ());
+ return;
}
hdl_read_packet_thread_ = new boost::thread (boost::bind (&HDLGrabber::readPacketsFromSocket, this));
}
bool
pcl::HDLGrabber::isRunning () const
{
- return (!hdl_data_.isEmpty() || (hdl_read_packet_thread_ != NULL &&
- !hdl_read_packet_thread_->timed_join (boost::posix_time::milliseconds (10))));
+ return (!hdl_data_.isEmpty () || (hdl_read_packet_thread_ != NULL && !hdl_read_packet_thread_->timed_join (boost::posix_time::milliseconds (10))));
}
/////////////////////////////////////////////////////////////////////////////
pcl::HDLGrabber::setLaserColorRGB (const pcl::RGB& color,
unsigned int laserNumber)
{
- if (laserNumber >= HDL_MAX_NUM_LASERS)
+ if (laserNumber >= (unsigned int) HDL_MAX_NUM_LASERS)
return;
laser_rgb_mapping_[laserNumber] = color;
/////////////////////////////////////////////////////////////////////////////
void
-pcl::HDLGrabber::setMaximumDistanceThreshold(float &maxThreshold) {
+pcl::HDLGrabber::setMaximumDistanceThreshold (float &maxThreshold)
+{
max_distance_threshold_ = maxThreshold;
}
/////////////////////////////////////////////////////////////////////////////
void
-pcl::HDLGrabber::setMinimumDistanceThreshold(float &minThreshold) {
+pcl::HDLGrabber::setMinimumDistanceThreshold (float &minThreshold)
+{
min_distance_threshold_ = minThreshold;
}
/////////////////////////////////////////////////////////////////////////////
float
-pcl::HDLGrabber::getMaximumDistanceThreshold() {
- return(max_distance_threshold_);
+pcl::HDLGrabber::getMaximumDistanceThreshold ()
+{
+ return (max_distance_threshold_);
}
/////////////////////////////////////////////////////////////////////////////
float
-pcl::HDLGrabber::getMinimumDistanceThreshold() {
- return(min_distance_threshold_);
+pcl::HDLGrabber::getMinimumDistanceThreshold ()
+{
+ return (min_distance_threshold_);
}
/////////////////////////////////////////////////////////////////////////////
unsigned char data[1500];
udp::endpoint sender_endpoint;
- while (!terminate_read_packet_thread_ && hdl_read_socket_->is_open())
+ while (!terminate_read_packet_thread_ && hdl_read_socket_->is_open ())
{
size_t length = hdl_read_socket_->receive_from (boost::asio::buffer (data, 1500), sender_endpoint);
- if (isAddressUnspecified (source_address_filter_) ||
- (source_address_filter_ == sender_endpoint.address () && source_port_filter_ == sender_endpoint.port ()))
- {
+ if (isAddressUnspecified (source_address_filter_)
+ || (source_address_filter_ == sender_endpoint.address () && source_port_filter_ == sender_endpoint.port ()))
+ {
enqueueHDLPacket (data, length);
- }
+ }
}
}
pcap_t *pcap = pcap_open_offline (pcap_file_name_.c_str (), errbuff);
struct bpf_program filter;
- std::ostringstream stringStream;
+ std::ostringstream string_stream;
- stringStream << "udp ";
+ string_stream << "udp ";
if (!isAddressUnspecified(source_address_filter_))
{
- stringStream << " and src port " << source_port_filter_ << " and src host " << source_address_filter_.to_string();
+ string_stream << " and src port " << source_port_filter_ << " and src host " << source_address_filter_.to_string();
}
// PCAP_NETMASK_UNKNOWN should be 0xffffffff, but it's undefined in older PCAP versions
- if (pcap_compile (pcap, &filter, stringStream.str ().c_str(), 0, 0xffffffff) == -1)
+ if (pcap_compile (pcap, &filter, string_stream.str ().c_str(), 0, 0xffffffff) == -1)
{
PCL_WARN ("[pcl::HDLGrabber::readPacketsFromPcap] Issue compiling filter: %s.\n", pcap_geterr (pcap));
}
}
struct timeval lasttime;
- unsigned long long uSecDelay;
+ unsigned long long usec_delay;
lasttime.tv_sec = 0;
lasttime.tv_usec -= 1000000;
lasttime.tv_sec++;
}
- uSecDelay = ((header->ts.tv_sec - lasttime.tv_sec) * 1000000) +
- (header->ts.tv_usec - lasttime.tv_usec);
+ usec_delay = ((header->ts.tv_sec - lasttime.tv_sec) * 1000000) +
+ (header->ts.tv_usec - lasttime.tv_usec);
- boost::this_thread::sleep(boost::posix_time::microseconds(uSecDelay));
+ boost::this_thread::sleep(boost::posix_time::microseconds(usec_delay));
lasttime.tv_sec = header->ts.tv_sec;
lasttime.tv_usec = header->ts.tv_usec;
THROW_IO_EXCEPTION ("Downsampling only possible for power of two scale in both dimensions. Request was %d x %d -> %d x %d.", wrapper_->getWidth (), wrapper_->getHeight (), width, height);
}
- register const uint8_t* yuv_buffer = (uint8_t*) wrapper_->getData ();
+ const uint8_t* yuv_buffer = (uint8_t*) wrapper_->getData ();
unsigned rgb_line_skip = 0;
if (rgb_line_step != 0)
if (wrapper_->getWidth () == width && wrapper_->getHeight () == height)
{
- for ( register unsigned yIdx = 0; yIdx < height; ++yIdx, rgb_buffer += rgb_line_skip )
+ for (unsigned yIdx = 0; yIdx < height; ++yIdx, rgb_buffer += rgb_line_skip)
{
- for ( register unsigned xIdx = 0; xIdx < width; xIdx += 2, rgb_buffer += 6, yuv_buffer += 4 )
+ for (unsigned xIdx = 0; xIdx < width; xIdx += 2, rgb_buffer += 6, yuv_buffer += 4)
{
int v = yuv_buffer[2] - 128;
int u = yuv_buffer[0] - 128;
}
else
{
- register unsigned yuv_step = wrapper_->getWidth () / width;
- register unsigned yuv_x_step = yuv_step << 1;
- register unsigned yuv_skip = (wrapper_->getHeight () / height - 1) * ( wrapper_->getWidth () << 1 );
+ unsigned yuv_step = wrapper_->getWidth () / width;
+ unsigned yuv_x_step = yuv_step << 1;
+ unsigned yuv_skip = (wrapper_->getHeight () / height - 1) * ( wrapper_->getWidth () << 1 );
- for ( register unsigned yIdx = 0; yIdx < wrapper_->getHeight (); yIdx += yuv_step, yuv_buffer += yuv_skip, rgb_buffer += rgb_line_skip )
+ for (unsigned yIdx = 0; yIdx < wrapper_->getHeight (); yIdx += yuv_step, yuv_buffer += yuv_skip, rgb_buffer += rgb_line_skip)
{
- for ( register unsigned xIdx = 0; xIdx < wrapper_->getWidth (); xIdx += yuv_step, rgb_buffer += 3, yuv_buffer += yuv_x_step )
+ for (unsigned xIdx = 0; xIdx < wrapper_->getWidth (); xIdx += yuv_step, rgb_buffer += 3, yuv_buffer += yuv_x_step)
{
int v = yuv_buffer[2] - 128;
int u = yuv_buffer[0] - 128;
if (gray_line_step != 0)
gray_line_skip = gray_line_step - width;
- register unsigned yuv_step = wrapper_->getWidth () / width;
- register unsigned yuv_x_step = yuv_step << 1;
- register unsigned yuv_skip = (wrapper_->getHeight () / height - 1) * ( wrapper_->getWidth () << 1 );
- register const uint8_t* yuv_buffer = ( (uint8_t*) wrapper_->getData () + 1);
+ unsigned yuv_step = wrapper_->getWidth () / width;
+ unsigned yuv_x_step = yuv_step << 1;
+ unsigned yuv_skip = (wrapper_->getHeight () / height - 1) * ( wrapper_->getWidth () << 1 );
+ const uint8_t* yuv_buffer = ( (uint8_t*) wrapper_->getData () + 1);
- for ( register unsigned yIdx = 0; yIdx < wrapper_->getHeight (); yIdx += yuv_step, yuv_buffer += yuv_skip, gray_buffer += gray_line_skip )
+ for (unsigned yIdx = 0; yIdx < wrapper_->getHeight (); yIdx += yuv_step, yuv_buffer += yuv_skip, gray_buffer += gray_line_skip)
{
- for ( register unsigned xIdx = 0; xIdx < wrapper_->getWidth (); xIdx += yuv_step, ++gray_buffer, yuv_buffer += yuv_x_step )
+ for (unsigned xIdx = 0; xIdx < wrapper_->getWidth (); xIdx += yuv_step, ++gray_buffer, yuv_buffer += yuv_x_step)
{
*gray_buffer = *yuv_buffer;
}
{
try
{
- materials_.back ().tex_d = boost::lexical_cast<float> (st[1]);
+ materials_.back ().tex_Ns = boost::lexical_cast<float> (st[1]);
}
catch (boost::bad_lexical_cast &)
{
std::size_t f_idx = 0;
std::string line;
std::vector<std::string> st;
- std::vector<Eigen::Vector2f> coordinates;
+ std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > coordinates;
try
{
while (!fs.eof ())
// Set default resolution if not reading a file
if (!openni_device_->isFile ())
{
- setColorVideoMode (getDefaultColorMode ());
+ if (openni_device_->hasSensor (openni::SENSOR_COLOR))
+ {
+ setColorVideoMode (getDefaultColorMode ());
+ }
setDepthVideoMode (getDefaultDepthMode ());
setIRVideoMode (getDefaultIRMode ());
}
{
// Search for and return VGA@30 Hz mode
vector<OpenNI2VideoMode> modeList = getSupportedIRVideoModes ();
- for (vector<OpenNI2VideoMode>::iterator modeItr = modeList.begin (); modeItr != modeList.end (); modeItr++)
+ for (vector<OpenNI2VideoMode>::iterator modeItr = modeList.begin (); modeItr != modeList.end (); ++modeItr)
{
OpenNI2VideoMode mode = *modeItr;
if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ == 30.0) )
{
// Search for and return VGA@30 Hz mode
vector<OpenNI2VideoMode> modeList = getSupportedColorVideoModes ();
- for (vector<OpenNI2VideoMode>::iterator modeItr = modeList.begin (); modeItr != modeList.end (); modeItr++)
+ for (vector<OpenNI2VideoMode>::iterator modeItr = modeList.begin (); modeItr != modeList.end (); ++modeItr)
{
OpenNI2VideoMode mode = *modeItr;
if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ == 30.0) )
{
// Search for and return VGA@30 Hz mode
vector<OpenNI2VideoMode> modeList = getSupportedDepthVideoModes ();
- for (vector<OpenNI2VideoMode>::iterator modeItr = modeList.begin (); modeItr != modeList.end (); modeItr++)
+ for (vector<OpenNI2VideoMode>::iterator modeItr = modeList.begin (); modeItr != modeList.end (); ++modeItr)
{
OpenNI2VideoMode mode = *modeItr;
if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ == 30.0) )
&& resizingSupported (modeIt->x_resolution_, modeIt->y_resolution_, requested_mode.x_resolution_, requested_mode.y_resolution_))
{
if (found)
- { // check wheter the new mode is better -> smaller than the current one.
+ { // check whether the new mode is better -> smaller than the current one.
if (actual_mode.x_resolution_ * actual_mode.x_resolution_ > modeIt->x_resolution_ * modeIt->y_resolution_ )
actual_mode = *modeIt;
}
class OpenNI2DeviceInfoComparator
{
public:
- bool operator ()(const OpenNI2DeviceInfo& di1, const OpenNI2DeviceInfo& di2)
+ bool operator ()(const OpenNI2DeviceInfo& di1, const OpenNI2DeviceInfo& di2) const
{
return (di1.uri_.compare (di2.uri_) < 0);
}
try
{
// check if we need to start/stop any stream
- if (image_required_ && !device_->isColorStreamStarted () )
+ if (image_required_ && !device_->isColorStreamStarted () && device_->hasColorSensor ())
{
block_signals ();
device_->startColorStream ();
{
try
{
- if (device_->isSynchronizationSupported () && !device_->isSynchronized () && !device_->isFile () &&
- device_->getColorVideoMode ().frame_rate_ == device_->getDepthVideoMode ().frame_rate_)
+ if (device_->hasColorSensor () && (device_->isSynchronizationSupported () && !device_->isSynchronized () && !device_->isFile () &&
+ device_->getColorVideoMode ().frame_rate_ == device_->getDepthVideoMode ().frame_rate_))
device_->setSynchronization (true);
}
catch (const IOException& exception)
// Load pre-calibrated camera parameters if they exist
if (pcl_isfinite (depth_parameters_.focal_length_x))
- fx = 1.0f / static_cast<float> (depth_parameters_.focal_length_x);
+ fx = static_cast<float> (depth_parameters_.focal_length_x);
if (pcl_isfinite (depth_parameters_.focal_length_y))
- fy = 1.0f / static_cast<float> (depth_parameters_.focal_length_y);
+ fy = static_cast<float> (depth_parameters_.focal_length_y);
if (pcl_isfinite (depth_parameters_.principal_point_x))
cx = static_cast<float> (depth_parameters_.principal_point_x);
// fill in XYZ values
unsigned step = cloud->width / depth_width_;
- unsigned skip = cloud->width * step - cloud->width;
+ unsigned skip = cloud->width - (depth_width_ * step);
int value_idx = 0;
int point_idx = 0;
// fill in the RGB values
step = cloud->width / image_width_;
- skip = cloud->width * step - cloud->width;
+ skip = cloud->width - (image_width_ * step);
value_idx = 0;
point_idx = 0;
RGBValue color;
- color.Alpha = 0;
+ color.Alpha = 0xff;
for (unsigned yIdx = 0; yIdx < image_height_; ++yIdx, point_idx += skip)
{
if (modeIt->nFPS == output_mode.nFPS && isImageResizeSupported (modeIt->nXRes, modeIt->nYRes, output_mode.nXRes, output_mode.nYRes))
{
if (found)
- { // check wheter the new mode is better -> smaller than the current one.
+ { // check whether the new mode is better -> smaller than the current one.
if (mode.nXRes * mode.nYRes > modeIt->nXRes * modeIt->nYRes )
mode = *modeIt;
}
if (modeIt->nFPS == output_mode.nFPS && isImageResizeSupported (modeIt->nXRes, modeIt->nYRes, output_mode.nXRes, output_mode.nYRes))
{
if (found)
- { // check wheter the new mode is better -> smaller than the current one.
+ { // check whether the new mode is better -> smaller than the current one.
if (mode.nXRes * mode.nYRes > modeIt->nXRes * modeIt->nYRes )
mode = *modeIt;
}
boost::char_separator<char> sep ("#");
boost::tokenizer<boost::char_separator<char> > tokens (info, sep);
boost::tokenizer<boost::char_separator<char> >::iterator itr = tokens.begin ();
- itr++;
- itr++;
+ ++itr;
+ ++itr;
std::string sn = *itr;
device_context_[index].device_node.SetInstanceName(sn.c_str ());
return (device_context_[index].device_node.GetInstanceName ());
#include <pcl/exceptions.h>
#include <iostream>
-namespace pcl
-{
- typedef union
- {
- struct
- {
- unsigned char Blue;
- unsigned char Green;
- unsigned char Red;
- unsigned char Alpha;
- };
- float float_value;
- uint32_t long_value;
- } RGBValue;
-}
-
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
pcl::OpenNIGrabber::OpenNIGrabber (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode)
: rgb_sync_ ()
device_->unregisterImageCallback (image_callback_handle);
if (device_->hasIRStream ())
- device_->unregisterIRCallback (image_callback_handle);
+ device_->unregisterIRCallback (ir_callback_handle);
// release the pointer to the device object
device_.reset ();
pt.y = (static_cast<float> (v) - centerY) * pt.z * constant_y;
}
}
- cloud->sensor_origin_.setZero ();
- cloud->sensor_orientation_.w () = 1.0f;
- cloud->sensor_orientation_.x () = 0.0f;
- cloud->sensor_orientation_.y () = 0.0f;
- cloud->sensor_orientation_.z () = 0.0f;
return (cloud);
}
cloud->points.resize (cloud->height * cloud->width);
//float constant = 1.0f / device_->getImageFocalLength (depth_width_);
- register float constant_x = 1.0f / device_->getImageFocalLength (depth_width_);
- register float constant_y = 1.0f / device_->getImageFocalLength (depth_width_);
+ register float constant_x = 1.0f / device_->getDepthFocalLength (depth_width_);
+ register float constant_y = 1.0f / device_->getDepthFocalLength (depth_width_);
register float centerX = ((float)cloud->width - 1.f) / 2.f;
register float centerY = ((float)cloud->height - 1.f) / 2.f;
- if (pcl_isfinite (rgb_focal_length_x_))
- constant_x = 1.0f / static_cast<float> (rgb_focal_length_x_);
+ if (pcl_isfinite (depth_focal_length_x_))
+ constant_x = 1.0f / static_cast<float> (depth_focal_length_x_);
- if (pcl_isfinite (rgb_focal_length_y_))
- constant_y = 1.0f / static_cast<float> (rgb_focal_length_y_);
+ if (pcl_isfinite (depth_focal_length_y_))
+ constant_y = 1.0f / static_cast<float> (depth_focal_length_y_);
- if (pcl_isfinite (rgb_principal_point_x_))
- centerX = static_cast<float> (rgb_principal_point_x_);
+ if (pcl_isfinite (depth_principal_point_x_))
+ centerX = static_cast<float> (depth_principal_point_x_);
- if (pcl_isfinite (rgb_principal_point_y_))
- centerY = static_cast<float> (rgb_principal_point_y_);
+ if (pcl_isfinite (depth_principal_point_y_))
+ centerY = static_cast<float> (depth_principal_point_y_);
register const XnDepthPixel* depth_map = depth_image->getDepthMetaData ().Data ();
if (depth_image->getWidth () != depth_width_ || depth_image->getHeight() != depth_height_)
PointT pt;
pt.x = pt.y = pt.z = bad_point;
pt.b = pt.g = pt.r = 0;
- pt.a = 255; // point has no color info -> alpha = max => transparent
+ pt.a = 0; // point has no color info -> alpha = min => transparent
cloud->points.assign (cloud->points.size (), pt);
}
value_idx = 0;
point_idx = 0;
- RGBValue color;
- color.Alpha = 0;
for (unsigned yIdx = 0; yIdx < image_height_; ++yIdx, point_idx += skip)
{
{
PointT& pt = cloud->points[point_idx];
- color.Red = rgb_buffer[value_idx];
- color.Green = rgb_buffer[value_idx + 1];
- color.Blue = rgb_buffer[value_idx + 2];
-
- pt.rgba = color.long_value;
+ pt.r = rgb_buffer[value_idx];
+ pt.g = rgb_buffer[value_idx + 1];
+ pt.b = rgb_buffer[value_idx + 2];
}
}
- cloud->sensor_origin_.setZero ();
- cloud->sensor_orientation_.w () = 1.0;
- cloud->sensor_orientation_.x () = 0.0;
- cloud->sensor_orientation_.y () = 0.0;
- cloud->sensor_orientation_.z () = 0.0;
return (cloud);
}
pt.intensity = static_cast<float> (ir_map[depth_idx]);
}
}
- cloud->sensor_origin_.setZero ();
- cloud->sensor_orientation_.w () = 1.0;
- cloud->sensor_orientation_.x () = 0.0;
- cloud->sensor_orientation_.y () = 0.0;
- cloud->sensor_orientation_.z () = 0.0;
return (cloud);
}
pcl::PCLPointCloud2 next_cloud_;
Eigen::Vector4f origin_;
Eigen::Quaternionf orientation_;
+ std::string next_file_name_;
bool valid_;
// TAR reading I/O
, next_cloud_ ()
, origin_ ()
, orientation_ ()
+ , next_file_name_ ()
, valid_ (false)
, tar_fd_ (-1)
, tar_offset_ (0)
{
pcd_files_.push_back (pcd_path);
pcd_iterator_ = pcd_files_.begin ();
+ next_file_name_ = *pcd_iterator_;
readAhead ();
}
, next_cloud_ ()
, origin_ ()
, orientation_ ()
+ , next_file_name_ ()
, valid_ (false)
, tar_fd_ (-1)
, tar_offset_ (0)
{
pcd_files_ = pcd_files;
pcd_iterator_ = pcd_files_.begin ();
+ next_file_name_ = *pcd_iterator_;
readAhead ();
}
}
}
+ next_file_name_ = *pcd_iterator_;
if (++pcd_iterator_ == pcd_files_.end () && repeat_)
pcd_iterator_ = pcd_files_.begin ();
}
{
boost::mutex::scoped_lock read_ahead_lock(read_ahead_mutex_);
if (valid_)
- grabber_.publish (next_cloud_,origin_,orientation_);
+ grabber_.publish (next_cloud_,origin_,orientation_, next_file_name_);
// use remaining time, if there is time left!
readAhead ();
///////////////////////////////////////////////////////////////////////////////////////////
pcl::PCDGrabberBase::~PCDGrabberBase () throw ()
{
- stop ();
delete impl_;
}
PCL_DEBUG ("[pcl::PCDWriter::setLockingPermissions] File %s could not be locked!\n", file_name.c_str ());
namespace fs = boost::filesystem;
- fs::permissions (fs::path (file_name), fs::add_perms | fs::set_gid_on_exe);
+ try
+ {
+ fs::permissions (fs::path (file_name), fs::add_perms | fs::set_gid_on_exe);
+ }
+ catch (const std::exception &e)
+ {
+ PCL_DEBUG ("[pcl::PCDWriter::setLockingPermissions] Permissions on %s could not be set!\n", file_name.c_str ());
+ }
#endif
#endif
}
#if BOOST_VERSION >= 104900
(void)file_name;
namespace fs = boost::filesystem;
- fs::permissions (fs::path (file_name), fs::remove_perms | fs::set_gid_on_exe);
+ try
+ {
+ fs::permissions (fs::path (file_name), fs::remove_perms | fs::set_gid_on_exe);
+ }
+ catch (const std::exception &e)
+ {
+ PCL_DEBUG ("[pcl::PCDWriter::resetLockingPermissions] Permissions on %s could not be reset!\n", file_name.c_str ());
+ }
lock.unlock ();
#endif
#endif
format_type format = pcl::io::ply::unknown;
std::vector< boost::shared_ptr<element> > elements;
+ char line_delim = '\n';
+ int char_ignore_count = 0;
+
// magic
char magic[4];
istream.read (magic, 4);
- if (magic[3] == '\r') // Check if CR/LF
+
+ // Check if CR/LF, setup delim and char skip
+ if (magic[3] == '\r')
+ {
istream.ignore (1);
+ line_delim = '\r';
+ char_ignore_count = 1;
+ }
+
++line_number_;
if (!istream)
{
if (error_callback_)
- error_callback_ (line_number_, "parse error");
+ error_callback_ (line_number_, "parse error: couldn't read the magic string");
return false;
}
if ((magic[0] != 'p') || (magic[1] != 'l') || (magic[2] != 'y'))
{
if (error_callback_)
- error_callback_ (line_number_, "parse error");
+ error_callback_ (line_number_, "parse error: wrong magic string");
return false;
}
if (magic_callback_)
magic_callback_ ();
- while (std::getline (istream, line))
+ while (std::getline (istream, line, line_delim))
{
+ istream.ignore (char_ignore_count);
++line_number_;
std::istringstream stringstream (line);
stringstream.unsetf (std::ios_base::skipws);
{
std::string format_string, version;
char space_format_format_string, space_format_string_version;
- stringstream >> space_format_format_string >> std::ws >> format_string >> space_format_string_version >> std::ws >> version >> std::ws;
+ stringstream >> space_format_format_string >> std::ws >> format_string >> space_format_string_version >> std::ws >> version;
if (!stringstream ||
!stringstream.eof () ||
!isspace (space_format_format_string) ||
!isspace (space_format_string_version))
{
if (error_callback_)
- error_callback_ (line_number_, "parse error");
+ error_callback_ (line_number_, "parse error: failed to parse the format statement");
return false;
}
if (format_string == "ascii")
{
if (error_callback_)
{
- error_callback_ (line_number_, "parse error");
+ error_callback_ (line_number_, "parse error: unknown format");
}
return false;
}
{
if (error_callback_)
{
- error_callback_ (line_number_, "parse error");
+ error_callback_ (line_number_, "parse error: more than 1 format statement");
}
return false;
}
std::string name;
std::size_t count;
char space_element_name, space_name_count;
- stringstream >> space_element_name >> std::ws >> name >> space_name_count >> std::ws >> count >> std::ws;
+ stringstream >> space_element_name >> std::ws >> name >> space_name_count >> std::ws >> count;
if (!stringstream ||
!stringstream.eof () ||
!isspace (space_element_name) ||
{
if (error_callback_)
{
- error_callback_ (line_number_, "parse error");
+ error_callback_ (line_number_, "parse error: invalid element statement");
}
return false;
}
{
if (error_callback_)
{
- error_callback_ (line_number_, "parse error");
+ error_callback_ (line_number_, "parse error: invalid elements");
}
return false;
}
{
if (error_callback_)
{
- error_callback_ (line_number_, "parse error");
+ error_callback_ (line_number_, "parse error: invalid property statement");
}
return false;
}
std::string name;
std::string& type = type_or_list;
char space_type_name;
- stringstream >> space_type_name >> std::ws >> name >> std::ws;
+ stringstream >> space_type_name >> std::ws >> name;
if (!stringstream || !isspace (space_type_name))
{
if (error_callback_)
{
if (error_callback_)
{
- error_callback_ (line_number_, "parse error");
+ error_callback_ (line_number_, "parse error: unknown type");
}
return false;
}
std::string name;
std::string size_type_string, scalar_type_string;
char space_list_size_type, space_size_type_scalar_type, space_scalar_type_name;
- stringstream >> space_list_size_type >> std::ws >> size_type_string >> space_size_type_scalar_type >> std::ws >> scalar_type_string >> space_scalar_type_name >> std::ws >> name >> std::ws;
+ stringstream >> space_list_size_type >> std::ws >> size_type_string >> space_size_type_scalar_type >> std::ws >> scalar_type_string >> space_scalar_type_name >> std::ws >> name;
if (!stringstream ||
!isspace (space_list_size_type) ||
!isspace (space_size_type_scalar_type) ||
!isspace (space_scalar_type_name))
{
if (error_callback_)
- error_callback_ (line_number_, "parse error");
+ error_callback_ (line_number_, "parse error: invalid list statement");
return false;
}
if (number_of_element_statements == 0)
{
if (error_callback_)
{
- error_callback_ (line_number_, "parse error");
+ error_callback_ (line_number_, "parse error: unkonwn scalar type");
}
return false;
}
else
{
if (error_callback_)
- error_callback_ (line_number_, "parse error");
+ error_callback_ (line_number_, "parse error: unknown scalar type");
return false;
}
}
else
{
if (error_callback_)
- error_callback_ (line_number_, "parse error");
+ error_callback_ (line_number_, "parse error: unknown scalar type");
return false;
}
}
if (number_of_format_statements == 0)
{
if (error_callback_)
- error_callback_ (line_number_, "parse error");
+ error_callback_ (line_number_, "parse error: a format statement is required");
return false;
}
{
if (element.begin_element_callback)
element.begin_element_callback ();
- if (!std::getline (istream, line))
+ if (!std::getline (istream, line, line_delim))
{
if (error_callback_)
error_callback_ (line_number_, "parse error");
return false;
}
+ istream.ignore (char_ignore_count);
++line_number_;
std::istringstream stringstream (line);
stringstream.unsetf (std::ios_base::skipws);
}
}
}
- if (istream.fail () || (istream.rdbuf ()->sgetc () != std::char_traits<char>::eof ()) || istream.bad ())
+ if (istream.fail () || istream.bad ())
{
if (error_callback_)
{
- error_callback_ (line_number_, "parse error");
+ error_callback_ (line_number_, "parse error: failed to read from the binary stream");
}
return false;
}
+ if (istream.rdbuf ()->sgetc () != std::char_traits<char>::eof ())
+ {
+ if (warning_callback_)
+ warning_callback_ (line_number_, "ignoring extra data at the end of binary stream");
+ }
return true;
}
}
boost::tuple<boost::function<void (pcl::io::ply::uint8)>, boost::function<void (pcl::io::ply::int32)>, boost::function<void ()> >
pcl::PLYReader::listPropertyDefinitionCallback (const std::string& element_name, const std::string& property_name)
{
- if ((element_name == "range_grid") && (property_name == "vertex_indices") && polygons_)
+ if ((element_name == "range_grid") && (property_name == "vertex_indices"))
{
return boost::tuple<boost::function<void (pcl::io::ply::uint8)>, boost::function<void (pcl::io::ply::int32)>, boost::function<void ()> > (
boost::bind (&pcl::PLYReader::rangeGridVertexIndicesBeginCallback, this, _1),
boost::bind (&pcl::PLYReader::rangeGridVertexIndicesEndCallback, this)
);
}
- else if ((element_name == "face") && (property_name == "vertex_indices"))
+ else if ((element_name == "face") && (property_name == "vertex_indices") && polygons_)
{
return boost::tuple<boost::function<void (pcl::io::ply::uint8)>, boost::function<void (pcl::io::ply::int32)>, boost::function<void ()> > (
boost::bind (&pcl::PLYReader::faceVertexIndicesBeginCallback, this, _1),
"\nproperty uchar green"
"\nproperty uchar blue";
}
+ // Check if we have normal on vertices
+ int normal_x_index = getFieldIndex(mesh.cloud, "normal_x");
+ int normal_y_index = getFieldIndex(mesh.cloud, "normal_y");
+ int normal_z_index = getFieldIndex(mesh.cloud, "normal_z");
+ if (normal_x_index != -1 && normal_y_index != -1 && normal_z_index != -1)
+ {
+ fs << "\nproperty float nx"
+ "\nproperty float ny"
+ "\nproperty float nz";
+ }
+ // Check if we have curvature on vertices
+ int curvature_index = getFieldIndex(mesh.cloud, "curvature");
+ if ( curvature_index != -1)
+ {
+ fs << "\nproperty float curvature";
+ }
// Faces
fs << "\nelement face "<< nr_faces;
fs << "\nproperty list uchar int vertex_indices";
memcpy (&color, &mesh.cloud.data[i * point_size + mesh.cloud.fields[rgba_index].offset + c * sizeof (uint32_t)], sizeof (RGB));
fs << int (color.r) << " " << int (color.g) << " " << int (color.b) << " " << int (color.a);
}
+ else if ((mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
+ mesh.cloud.fields[d].name == "normal_x" ||
+ mesh.cloud.fields[d].name == "normal_y" ||
+ mesh.cloud.fields[d].name == "normal_z"))
+ {
+ float value;
+ memcpy (&value, &mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset + c * sizeof(float)], sizeof(float));
+ fs << value;
+ }
+ else if ((mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
+ mesh.cloud.fields[d].name == "curvature"))
+ {
+ float value;
+ memcpy(&value, &mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset + c * sizeof(float)], sizeof(float));
+ fs << value;
+ }
fs << " ";
}
if (xyz != 3)
"\nproperty uchar green"
"\nproperty uchar blue";
}
+ // Check if we have normal on vertices
+ int normal_x_index = getFieldIndex(mesh.cloud, "normal_x");
+ int normal_y_index = getFieldIndex(mesh.cloud, "normal_y");
+ int normal_z_index = getFieldIndex(mesh.cloud, "normal_z");
+ if (normal_x_index != -1 && normal_y_index != -1 && normal_z_index != -1)
+ {
+ fs << "\nproperty float nx"
+ "\nproperty float ny"
+ "\nproperty float nz";
+ }
+ // Check if we have curvature on vertices
+ int curvature_index = getFieldIndex(mesh.cloud, "curvature");
+ if ( curvature_index != -1)
+ {
+ fs << "\nproperty float curvature";
+ }
// Faces
fs << "\nelement face "<< nr_faces;
fs << "\nproperty list uchar int vertex_indices";
fpout.write (reinterpret_cast<const char*> (&color.b), sizeof (unsigned char));
fpout.write (reinterpret_cast<const char*> (&color.a), sizeof (unsigned char));
}
+ else if ((mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
+ mesh.cloud.fields[d].name == "normal_x" ||
+ mesh.cloud.fields[d].name == "normal_y" ||
+ mesh.cloud.fields[d].name == "normal_z"))
+ {
+ float value;
+ memcpy (&value, &mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset + c * sizeof (float)], sizeof (float));
+ fpout.write (reinterpret_cast<const char*> (&value), sizeof (float));
+ }
+ else if ((mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) &&
+ (mesh.cloud.fields[d].name == "curvature"))
+ {
+ float value;
+ memcpy (&value, &mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset + c * sizeof (float)], sizeof (float));
+ fpout.write (reinterpret_cast<const char*> (&value), sizeof (float));
+ }
}
if (xyz != 3)
{
+++ /dev/null
-/*
- * Software License Agreement (BSD License)
- *
- * Point Cloud Library (PCL) - www.pointclouds.org
- * Copyright (c) 2012-, Open Perception, Inc.
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the copyright holder(s) nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- */
-
-#include <pcl/pcl_config.h>
-#define HAVE_PXCAPI
-#ifdef HAVE_PXCAPI
-
-#include <pcl/io/pxc_grabber.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/common/time.h>
-#include <pcl/common/io.h>
-#include <pcl/console/print.h>
-#include <pcl/io/boost.h>
-#include <pcl/exceptions.h>
-
-#include <boost/thread.hpp>
-
-#include <iostream>
-#include <queue>
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-pcl::PXCGrabber::PXCGrabber ()
- : pp_ ()
- , running_ (false)
- , fps_ (0.0f)
-{
- point_cloud_signal_ = createSignal<sig_cb_pxc_point_cloud> ();
- point_cloud_rgb_signal_ = createSignal<sig_cb_pxc_point_cloud_rgb> ();
- point_cloud_rgba_signal_ = createSignal<sig_cb_pxc_point_cloud_rgba> ();
-}
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-pcl::PXCGrabber::~PXCGrabber () throw ()
-{
- stop ();
-
- disconnect_all_slots<sig_cb_pxc_point_cloud> ();
- disconnect_all_slots<sig_cb_pxc_point_cloud_rgb> ();
- disconnect_all_slots<sig_cb_pxc_point_cloud_rgba> ();
-}
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-bool
-pcl::PXCGrabber::init ()
-{
- // enable rgb and 3d data and initialize
- pp_.EnableImage (PXCImage::COLOR_FORMAT_RGB32);
- pp_.EnableImage (PXCImage::COLOR_FORMAT_VERTICES);
- pp_.Init ();
-
- return (true);
-}
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void
-pcl::PXCGrabber::close ()
-{
- pp_.Close ();
-}
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void
-pcl::PXCGrabber::start ()
-{
- init ();
- running_ = true;
-
- grabber_thread_ = boost::thread (&pcl::PXCGrabber::processGrabbing, this);
-}
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void
-pcl::PXCGrabber::stop ()
-{
- running_ = false;
- grabber_thread_.join ();
-
- close ();
-}
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-bool
-pcl::PXCGrabber::isRunning () const
-{
- return (running_);
-}
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-std::string
-pcl::PXCGrabber::getName () const
-{
- return (std::string ("PXCGrabber"));
-}
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-float
-pcl::PXCGrabber::getFramesPerSecond () const
-{
- fps_mutex_.lock ();
- float fps = fps_;
- fps_mutex_.unlock ();
-
- return (fps);
-}
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void
-pcl::PXCGrabber::processGrabbing ()
-{
- UtilCapture * cap = pp_.QueryCapture ();
- PXCCapture * capture = cap->QueryCapture ();
- PXCSession * session = pp_.QuerySession ();
-
- // special depth values for saturated and low-confidence pixels
- pxcF32 dvalues[2];
-
- // projection serializable identifier
- pxcUID prj_value;
- PXCSmartPtr<PXCProjection> projection;
-
- // setup the projection info for getting a nice depth map
- pxcStatus sts = cap->QueryDevice ()->QueryPropertyAsUID (PXCCapture::Device::PROPERTY_PROJECTION_SERIALIZABLE, &prj_value);
- if (sts >= PXC_STATUS_NO_ERROR)
- {
- // create properties for checking if depth values are bad (by low confidence and by saturation)
- cap->QueryDevice ()->QueryProperty (PXCCapture::Device::PROPERTY_DEPTH_LOW_CONFIDENCE_VALUE, &dvalues[0]);
- cap->QueryDevice ()->QueryProperty (PXCCapture::Device::PROPERTY_DEPTH_SATURATION_VALUE, &dvalues[1]);
-
- session->DynamicCast<PXCMetadata> ()->CreateSerializable<PXCProjection> (prj_value, &projection);
- }
-
- pcl::StopWatch stop_watch;
- std::queue<double> capture_time_queue;
- double total_time = 0.0f;
-
- stop_watch.reset ();
-
- bool continue_grabbing = true;
- while (continue_grabbing)
- {
- if (!pp_.AcquireFrame (true))
- {
- boost::this_thread::sleep (boost::posix_time::milliseconds (1));
- continue;
- }
-
- // query rgb and 3d data
- PXCImage *color_image = pp_.QueryImage (PXCImage::IMAGE_TYPE_COLOR);
- PXCImage *vertex_image = pp_.QueryImage (PXCImage::IMAGE_TYPE_DEPTH);
-
- // acquiure access to data
- PXCImage::ImageData ddata;
- vertex_image->AcquireAccess (PXCImage::ACCESS_READ, &ddata);
- short * vertices = (short*) ddata.planes[0];
-
- PXCImage::ImageData idata;
- color_image->AcquireAccess (PXCImage::ACCESS_READ, &idata);
- unsigned char * rgb_image_data = (unsigned char*) idata.planes[0];
-
- // get image information
- PXCImage::ImageInfo vertImageInfo;
- pxcStatus stat = vertex_image->QueryInfo (&vertImageInfo);
- const int w = vertImageInfo.width;
- const int h = vertImageInfo.height;
-
- PXCImage::ImageInfo rgb_image_info;
- stat = color_image->QueryInfo (&rgb_image_info);
- const int image_width = rgb_image_info.width;
- const int image_height = rgb_image_info.height;
-
- // convert to point cloud
- const float nan_value = std::numeric_limits<float>::quiet_NaN ();
-
- int dwidth2 = ddata.pitches[0] / sizeof (pxcU16);
- float *uvmap = (float*)ddata.planes[2];
- pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA> (w, h));
- for (int j = 0, k = 0; j < h; j++)
- {
- for (int i = 0; i < w; i++, k++)
- {
- int xx = (int) (uvmap[(j*w+i)*2+0]*image_width+0.5f);
- int yy = (int) (uvmap[(j*w+i)*2+1]*image_height+0.5f);
-
- unsigned int idx = 3*(j*w + i);
- short x = vertices[idx];
- short y = vertices[idx+1];
- short z = vertices[idx+2];
-
- unsigned char r = 255;
- unsigned char g = 255;
- unsigned char b = 255;
-
- if (xx < 0 || xx >= image_width || yy < 0 || yy >= image_height)
- {
- r = 255;
- g = 255;
- b = 255;
- }
- else
- {
- unsigned char * rgb_ptr = rgb_image_data + 3*(yy*image_width+xx);
- r = rgb_ptr[2];
- g = rgb_ptr[1];
- b = rgb_ptr[0];
- }
-
- if (z == dvalues[0] || z == dvalues[1])
- {
- cloud->points[k].x = nan_value;
- cloud->points[k].y = nan_value;
- cloud->points[k].z = nan_value;
- cloud->points[k].r = r;
- cloud->points[k].g = g;
- cloud->points[k].b = b;
- cloud->points[k].a = 255;
- }
- else
- {
- // normalize vertices to meters
- cloud->points[k].x = x / 1000.0f;
- cloud->points[k].y = y / 1000.0f;
- cloud->points[k].z = z / 1000.0f;
- cloud->points[k].r = r;
- cloud->points[k].g = g;
- cloud->points[k].b = b;
- cloud->points[k].a = 255;
- }
- }
- }
-
- // publish cloud
- if (num_slots<sig_cb_pxc_point_cloud_rgba> () > 0)
- {
- point_cloud_rgba_signal_->operator() (cloud);
- }
- if (num_slots<sig_cb_pxc_point_cloud_rgb> () > 0)
- {
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
- cloud->resize (w*h);
- cloud->width = w;
- cloud->height = h;
- cloud->is_dense = false;
-
- pcl::copyPointCloud<pcl::PointXYZRGBA, pcl::PointXYZRGB> (*cloud, *tmp_cloud);
-
- point_cloud_rgb_signal_->operator() (tmp_cloud);
- }
- if (num_slots<sig_cb_pxc_point_cloud> () > 0)
- {
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp (new pcl::PointCloud<pcl::PointXYZ> ());
- cloud->resize (w*h);
- cloud->width = w;
- cloud->height = h;
- cloud->is_dense = false;
-
- pcl::copyPointCloud<pcl::PointXYZRGBA, pcl::PointXYZ> (*cloud, *cloud_tmp);
-
- point_cloud_signal_->operator() (cloud_tmp);
- }
-
- vertex_image->ReleaseAccess (&ddata);
- color_image->ReleaseAccess (&idata);
-
- pp_.ReleaseFrame ();
-
- const double capture_time = stop_watch.getTimeSeconds ();
- total_time += capture_time;
-
- capture_time_queue.push (capture_time);
-
- if (capture_time_queue.size () >= 30)
- {
- double removed_time = capture_time_queue.front ();
- capture_time_queue.pop ();
-
- total_time -= removed_time;
- }
-
- fps_mutex_.lock ();
- fps_ = static_cast<float> (total_time / capture_time_queue.size ());
- fps_mutex_.unlock ();
- }
-}
-
-#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2015-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/io/io_exception.h>
+
+#include <pcl/io/real_sense/real_sense_device_manager.h>
+
+boost::mutex pcl::io::real_sense::RealSenseDeviceManager::mutex_;
+
+using namespace pcl::io;
+
+/** Helper function to release a RealSense resource.
+ Useful as a deleter for a shared pointer holding RealSense resource. */
+template <typename T> void
+releasePXCResource (T* resource)
+{
+ if (resource)
+ {
+ resource->Release ();
+ resource = 0;
+ }
+}
+
+template <typename T> boost::shared_ptr<T>
+makePXCSharedPtr (T* resource)
+{
+ return boost::shared_ptr<T> (resource, releasePXCResource<T>);
+}
+
+boost::shared_ptr<PXCSession>
+createPXCSession ()
+{
+ PXCSession* s = PXCSession::CreateInstance ();
+ if (!s)
+ THROW_IO_EXCEPTION ("failed to create RealSense session");
+ return makePXCSharedPtr (s);
+}
+
+boost::shared_ptr<PXCCaptureManager>
+createPXCCaptureManager (PXCSession& session)
+{
+ PXCCaptureManager* cm = session.CreateCaptureManager ();
+ if (!cm)
+ THROW_IO_EXCEPTION ("failed to create RealSense capture manager");
+ return makePXCSharedPtr (cm);
+}
+
+boost::shared_ptr<PXCCapture>
+createPXCCapture (PXCSession& session, pxcUID iuid)
+{
+ PXCCapture* c;
+ if (session.CreateImpl (iuid, &c) < PXC_STATUS_NO_ERROR)
+ THROW_IO_EXCEPTION ("unable to create RealSense capture");
+ return makePXCSharedPtr (c);
+}
+
+boost::shared_ptr<PXCCapture::Device>
+createPXCCaptureDevice (PXCCapture& capture, pxcI32 didx)
+{
+ PXCCapture::Device* d;
+ d = capture.CreateDevice (didx);
+ if (!d)
+ THROW_IO_EXCEPTION ("unable to create RealSense capture device");
+ return makePXCSharedPtr (d);
+}
+
+/** Utility function to convert RealSense-style strings (which happen to
+ * consist of 2-byte chars) into standard library strings. */
+std::string
+toString (const pxcCHAR* pxc_string, size_t max_length)
+{
+ size_t i = 0;
+ while (i + 1 < max_length && pxc_string[i])
+ ++i;
+ std::string out (i + 1, '\0');
+ size_t j = 0;
+ while (j < i)
+ out[j] = pxc_string[j++];
+ return out;
+}
+
+pcl::io::real_sense::RealSenseDeviceManager::RealSenseDeviceManager ()
+{
+ session_ = createPXCSession ();
+ populateDeviceList ();
+}
+
+pcl::io::real_sense::RealSenseDeviceManager::~RealSenseDeviceManager ()
+{
+}
+
+pcl::io::real_sense::RealSenseDevice::Ptr
+pcl::io::real_sense::RealSenseDeviceManager::captureDevice ()
+{
+ boost::mutex::scoped_lock lock (mutex_);
+ if (device_list_.size () == 0)
+ THROW_IO_EXCEPTION ("no connected devices");
+ for (size_t i = 0; i < device_list_.size (); ++i)
+ if (!device_list_[i].isCaptured ())
+ return (capture (device_list_[i]));
+ THROW_IO_EXCEPTION ("all connected devices are captured by other grabbers");
+ return (RealSenseDevice::Ptr ()); // never reached, needed just to silence -Wreturn-type warning
+}
+
+pcl::io::real_sense::RealSenseDevice::Ptr
+pcl::io::real_sense::RealSenseDeviceManager::captureDevice (size_t index)
+{
+ boost::mutex::scoped_lock lock (mutex_);
+ if (index >= device_list_.size ())
+ THROW_IO_EXCEPTION ("device with index %i is not connected", index + 1);
+ if (device_list_[index].isCaptured ())
+ THROW_IO_EXCEPTION ("device with index %i is captured by another grabber", index + 1);
+ return (capture (device_list_[index]));
+}
+
+pcl::io::real_sense::RealSenseDevice::Ptr
+pcl::io::real_sense::RealSenseDeviceManager::captureDevice (const std::string& sn)
+{
+ boost::mutex::scoped_lock lock (mutex_);
+ for (size_t i = 0; i < device_list_.size (); ++i)
+ {
+ if (device_list_[i].serial == sn)
+ {
+ if (device_list_[i].isCaptured ())
+ THROW_IO_EXCEPTION ("device with serial number %s is captured by another grabber", sn.c_str ());
+ return (capture (device_list_[i]));
+ }
+ }
+ THROW_IO_EXCEPTION ("device with serial number %s is not connected", sn.c_str ());
+ return (RealSenseDevice::Ptr ()); // never reached, needed just to silence -Wreturn-type warning
+}
+
+void
+pcl::io::real_sense::RealSenseDeviceManager::populateDeviceList ()
+{
+ device_list_.clear ();
+
+ // Module description to match
+ PXCSession::ImplDesc module_desc = {};
+ module_desc.group = PXCSession::IMPL_GROUP_SENSOR;
+ module_desc.subgroup = PXCSession::IMPL_SUBGROUP_VIDEO_CAPTURE;
+
+ for (int m = 0;; m++)
+ {
+ PXCSession::ImplDesc desc;
+ if (session_->QueryImpl (&module_desc, m, &desc) < PXC_STATUS_NO_ERROR)
+ break;
+ PXCCapture* capture;
+ if (session_->CreateImpl<PXCCapture> (&desc, &capture) < PXC_STATUS_NO_ERROR)
+ continue;
+ for (int j = 0;; j++)
+ {
+ PXCCapture::DeviceInfo device_info;
+ if (capture->QueryDeviceInfo (j, &device_info) < PXC_STATUS_NO_ERROR)
+ break;
+ if (device_info.streams & PXCCapture::STREAM_TYPE_DEPTH)
+ {
+ const size_t MAX_SERIAL_LENGTH = sizeof (device_info.serial) / sizeof (device_info.serial[0]);
+ std::string serial = toString (device_info.serial, MAX_SERIAL_LENGTH);
+ device_list_.push_back (DeviceInfo ());
+ device_list_.back ().serial = serial;
+ device_list_.back ().iuid = desc.iuid;
+ device_list_.back ().didx = j;
+ }
+ }
+ capture->Release ();
+ }
+}
+
+pcl::io::real_sense::RealSenseDevice::Ptr
+pcl::io::real_sense::RealSenseDeviceManager::capture (DeviceInfo& device_info)
+{
+ // This is called from public captureDevice() functions and should already be
+ // under scoped lock
+ if (!device_info.device_ptr.expired ())
+ {
+ return device_info.device_ptr.lock ();
+ }
+ else
+ {
+ RealSenseDevice::Ptr device (new RealSenseDevice (device_info.serial));
+ device->capture_ = createPXCCapture (*session_, device_info.iuid);
+ device->device_ = createPXCCaptureDevice (*device->capture_, device_info.didx);
+ device_info.device_ptr = device;
+ return device;
+ }
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2015-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <boost/lexical_cast.hpp>
+
+#include <pxcimage.h>
+#include <pxccapture.h>
+#include <pxcprojection.h>
+#include <pxcsensemanager.h>
+
+#include <pcl/common/io.h>
+#include <pcl/common/time.h>
+
+#include <pcl/io/buffers.h>
+#include <pcl/io/io_exception.h>
+#include <pcl/io/real_sense_grabber.h>
+#include <pcl/io/real_sense/real_sense_device_manager.h>
+
+using namespace pcl::io;
+using namespace pcl::io::real_sense;
+
+/* Helper function to convert a PXCPoint3DF32 point into a PCL point.
+ * Takes care of unit conversion (PXC point coordinates are in millimeters)
+ * and invalid points. */
+template <typename T> inline void
+convertPoint (const PXCPoint3DF32& src, T& tgt)
+{
+ static const float nan = std::numeric_limits<float>::quiet_NaN ();
+ if (src.z == 0)
+ {
+ tgt.x = tgt.y = tgt.z = nan;
+ }
+ else
+ {
+ tgt.x = -src.x / 1000.0;
+ tgt.y = src.y / 1000.0;
+ tgt.z = src.z / 1000.0;
+ }
+}
+
+pcl::RealSenseGrabber::Mode::Mode ()
+: fps (0), depth_width (0), depth_height (0), color_width (0), color_height (0)
+{
+}
+
+pcl::RealSenseGrabber::Mode::Mode (unsigned int f)
+: fps (f), depth_width (0), depth_height (0), color_width (0), color_height (0)
+{
+}
+
+pcl::RealSenseGrabber::Mode::Mode (unsigned int dw, unsigned int dh)
+: fps (0), depth_width (dw), depth_height (dh), color_width (0), color_height (0)
+{
+}
+
+pcl::RealSenseGrabber::Mode::Mode (unsigned int f, unsigned int dw, unsigned int dh)
+: fps (f), depth_width (dw), depth_height (dh), color_width (0), color_height (0)
+{
+}
+
+pcl::RealSenseGrabber::Mode::Mode (unsigned int dw, unsigned int dh, unsigned int cw, unsigned int ch)
+: fps (0), depth_width (dw), depth_height (dh), color_width (cw), color_height (ch)
+{
+}
+
+pcl::RealSenseGrabber::Mode::Mode (unsigned int f, unsigned int dw, unsigned int dh, unsigned int cw, unsigned int ch)
+: fps (f), depth_width (dw), depth_height (dh), color_width (cw), color_height (ch)
+{
+}
+
+bool
+operator== (const pcl::RealSenseGrabber::Mode& m1, const pcl::RealSenseGrabber::Mode& m2)
+{
+ return (m1.fps == m2.fps &&
+ m1.depth_width == m2.depth_width &&
+ m1.depth_height == m2.depth_height &&
+ m1.color_width == m2.color_width &&
+ m1.color_height == m2.color_height);
+}
+
+pcl::RealSenseGrabber::RealSenseGrabber (const std::string& device_id, const Mode& mode, bool strict)
+: Grabber ()
+, is_running_ (false)
+, confidence_threshold_ (6)
+, temporal_filtering_type_ (RealSense_None)
+, temporal_filtering_window_size_ (1)
+, mode_requested_ (mode)
+, strict_ (strict)
+{
+ if (device_id == "")
+ device_ = RealSenseDeviceManager::getInstance ()->captureDevice ();
+ else if (device_id[0] == '#')
+ device_ = RealSenseDeviceManager::getInstance ()->captureDevice (boost::lexical_cast<int> (device_id.substr (1)) - 1);
+ else
+ device_ = RealSenseDeviceManager::getInstance ()->captureDevice (device_id);
+
+ point_cloud_signal_ = createSignal<sig_cb_real_sense_point_cloud> ();
+ point_cloud_rgba_signal_ = createSignal<sig_cb_real_sense_point_cloud_rgba> ();
+}
+
+pcl::RealSenseGrabber::~RealSenseGrabber () throw ()
+{
+ stop ();
+
+ disconnect_all_slots<sig_cb_real_sense_point_cloud> ();
+ disconnect_all_slots<sig_cb_real_sense_point_cloud_rgba> ();
+}
+
+void
+pcl::RealSenseGrabber::start ()
+{
+ if (!is_running_)
+ {
+ need_xyz_ = num_slots<sig_cb_real_sense_point_cloud> () > 0;
+ need_xyzrgba_ = num_slots<sig_cb_real_sense_point_cloud_rgba> () > 0;
+ if (need_xyz_ || need_xyzrgba_)
+ {
+ selectMode ();
+ PXCCapture::Device::StreamProfileSet profile;
+ memset (&profile, 0, sizeof (profile));
+ profile.depth.frameRate.max = mode_selected_.fps;
+ profile.depth.frameRate.min = mode_selected_.fps;
+ profile.depth.imageInfo.width = mode_selected_.depth_width;
+ profile.depth.imageInfo.height = mode_selected_.depth_height;
+ profile.depth.imageInfo.format = PXCImage::PIXEL_FORMAT_DEPTH;
+ profile.depth.options = PXCCapture::Device::STREAM_OPTION_ANY;
+ if (need_xyzrgba_)
+ {
+ profile.color.frameRate.max = mode_selected_.fps;
+ profile.color.frameRate.min = mode_selected_.fps;
+ profile.color.imageInfo.width = mode_selected_.color_width;
+ profile.color.imageInfo.height = mode_selected_.color_height;
+ profile.color.imageInfo.format = PXCImage::PIXEL_FORMAT_RGB32;
+ profile.color.options = PXCCapture::Device::STREAM_OPTION_ANY;
+ }
+ device_->getPXCDevice ().SetStreamProfileSet (&profile);
+ if (!device_->getPXCDevice ().IsStreamProfileSetValid (&profile))
+ THROW_IO_EXCEPTION ("Invalid stream profile for PXC device");
+ frequency_.reset ();
+ is_running_ = true;
+ thread_ = boost::thread (&RealSenseGrabber::run, this);
+ }
+ }
+}
+
+void
+pcl::RealSenseGrabber::stop ()
+{
+ if (is_running_)
+ {
+ is_running_ = false;
+ thread_.join ();
+ }
+}
+
+bool
+pcl::RealSenseGrabber::isRunning () const
+{
+ return (is_running_);
+}
+
+float
+pcl::RealSenseGrabber::getFramesPerSecond () const
+{
+ boost::mutex::scoped_lock lock (fps_mutex_);
+ return (frequency_.getFrequency ());
+}
+
+void
+pcl::RealSenseGrabber::setConfidenceThreshold (unsigned int threshold)
+{
+ if (threshold > 15)
+ {
+ PCL_WARN ("[pcl::RealSenseGrabber::setConfidenceThreshold] Attempted to set threshold outside valid range (0-15)");
+ }
+ else
+ {
+ confidence_threshold_ = threshold;
+ device_->getPXCDevice ().SetDepthConfidenceThreshold (confidence_threshold_);
+ }
+}
+
+void
+pcl::RealSenseGrabber::enableTemporalFiltering (TemporalFilteringType type, size_t window_size)
+{
+ if (temporal_filtering_type_ != type ||
+ (type != RealSense_None && temporal_filtering_window_size_ != window_size))
+ {
+ temporal_filtering_type_ = type;
+ temporal_filtering_window_size_ = window_size;
+ if (is_running_)
+ {
+ stop ();
+ start ();
+ }
+ }
+}
+
+void
+pcl::RealSenseGrabber::disableTemporalFiltering ()
+{
+ enableTemporalFiltering (RealSense_None, 1);
+}
+
+const std::string&
+pcl::RealSenseGrabber::getDeviceSerialNumber () const
+{
+ return (device_->getSerialNumber ());
+}
+
+std::vector<pcl::RealSenseGrabber::Mode>
+pcl::RealSenseGrabber::getAvailableModes (bool only_depth) const
+{
+ std::vector<Mode> modes;
+ PXCCapture::StreamType streams = only_depth
+ ? PXCCapture::STREAM_TYPE_DEPTH
+ : PXCCapture::STREAM_TYPE_DEPTH | PXCCapture::STREAM_TYPE_COLOR;
+ for (int p = 0;; p++)
+ {
+ PXCCapture::Device::StreamProfileSet profiles = {};
+ if (device_->getPXCDevice ().QueryStreamProfileSet (streams, p, &profiles) == PXC_STATUS_NO_ERROR)
+ {
+ if (!only_depth && profiles.depth.frameRate.max != profiles.color.frameRate.max)
+ continue; // we need both streams to have the same framerate
+ Mode mode;
+ mode.fps = profiles.depth.frameRate.max;
+ mode.depth_width = profiles.depth.imageInfo.width;
+ mode.depth_height = profiles.depth.imageInfo.height;
+ mode.color_width = profiles.color.imageInfo.width;
+ mode.color_height = profiles.color.imageInfo.height;
+ bool duplicate = false;
+ for (size_t i = 0; i < modes.size (); ++i)
+ duplicate |= modes[i] == mode;
+ if (!duplicate)
+ modes.push_back (mode);
+ }
+ else
+ {
+ break;
+ }
+ }
+ return modes;
+}
+
+void
+pcl::RealSenseGrabber::setMode (const Mode& mode, bool strict)
+{
+ if (mode == mode_requested_ && strict == strict_)
+ return;
+ mode_requested_ = mode;
+ strict_ = strict;
+ if (is_running_)
+ {
+ stop ();
+ start ();
+ }
+}
+
+void
+pcl::RealSenseGrabber::run ()
+{
+ const int WIDTH = mode_selected_.depth_width;
+ const int HEIGHT = mode_selected_.depth_height;
+ const int SIZE = WIDTH * HEIGHT;
+
+ PXCProjection* projection = device_->getPXCDevice ().CreateProjection ();
+ PXCCapture::Sample sample;
+ std::vector<PXCPoint3DF32> vertices (SIZE);
+ createDepthBuffer ();
+
+ while (is_running_)
+ {
+ pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_cloud;
+ pcl::PointCloud<pcl::PointXYZRGBA>::Ptr xyzrgba_cloud;
+
+ pxcStatus status;
+ if (need_xyzrgba_)
+ status = device_->getPXCDevice ().ReadStreams (PXCCapture::STREAM_TYPE_DEPTH | PXCCapture::STREAM_TYPE_COLOR, &sample);
+ else
+ status = device_->getPXCDevice ().ReadStreams (PXCCapture::STREAM_TYPE_DEPTH, &sample);
+
+ uint64_t timestamp = pcl::getTime () * 1.0e+6;
+
+ switch (status)
+ {
+ case PXC_STATUS_NO_ERROR:
+ {
+ fps_mutex_.lock ();
+ frequency_.event ();
+ fps_mutex_.unlock ();
+
+ /* We preform the following steps to convert received data into point clouds:
+ *
+ * 1. Push depth image to the depth buffer
+ * 2. Pull filtered depth image from the depth buffer
+ * 3. Project (filtered) depth image into 3D
+ * 4. Fill XYZ point cloud with computed points
+ * 5. Fill XYZRGBA point cloud with computed points
+ * 7. Project color image into 3D
+ * 6. Assign colors to points in XYZRGBA point cloud
+ *
+ * Steps 1-2 are skipped if temporal filtering is disabled.
+ * Step 4 is skipped if there are no subscribers for XYZ clouds.
+ * Steps 5-7 are skipped if there are no subscribers for XYZRGBA clouds. */
+
+ if (temporal_filtering_type_ != RealSense_None)
+ {
+ PXCImage::ImageData data;
+ sample.depth->AcquireAccess (PXCImage::ACCESS_READ, &data);
+ std::vector<unsigned short> data_copy (SIZE);
+ memcpy (data_copy.data (), data.planes[0], SIZE * sizeof (unsigned short));
+ sample.depth->ReleaseAccess (&data);
+
+ depth_buffer_->push (data_copy);
+
+ sample.depth->AcquireAccess (PXCImage::ACCESS_WRITE, &data);
+ unsigned short* d = reinterpret_cast<unsigned short*> (data.planes[0]);
+ for (size_t i = 0; i < SIZE; i++)
+ d[i] = (*depth_buffer_)[i];
+ sample.depth->ReleaseAccess (&data);
+ }
+
+ projection->QueryVertices (sample.depth, vertices.data ());
+
+ if (need_xyz_)
+ {
+ xyz_cloud.reset (new pcl::PointCloud<pcl::PointXYZ> (WIDTH, HEIGHT));
+ xyz_cloud->header.stamp = timestamp;
+ xyz_cloud->is_dense = false;
+ for (int i = 0; i < SIZE; i++)
+ convertPoint (vertices[i], xyz_cloud->points[i]);
+ }
+
+ if (need_xyzrgba_)
+ {
+ PXCImage::ImageData data;
+ PXCImage* mapped = projection->CreateColorImageMappedToDepth (sample.depth, sample.color);
+ mapped->AcquireAccess (PXCImage::ACCESS_READ, &data);
+ uint32_t* d = reinterpret_cast<uint32_t*> (data.planes[0]);
+ if (need_xyz_)
+ {
+ // We can fill XYZ coordinates more efficiently using pcl::copyPointCloud,
+ // given that they were already computed for XYZ point cloud.
+ xyzrgba_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBA>);
+ pcl::copyPointCloud (*xyz_cloud, *xyzrgba_cloud);
+ for (int i = 0; i < HEIGHT; i++)
+ {
+ pcl::PointXYZRGBA* cloud_row = &xyzrgba_cloud->points[i * WIDTH];
+ uint32_t* color_row = &d[i * data.pitches[0] / sizeof (uint32_t)];
+ for (int j = 0; j < WIDTH; j++)
+ memcpy (&cloud_row[j].rgba, &color_row[j], sizeof (uint32_t));
+ }
+ }
+ else
+ {
+ xyzrgba_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBA> (WIDTH, HEIGHT));
+ xyzrgba_cloud->header.stamp = timestamp;
+ xyzrgba_cloud->is_dense = false;
+ for (int i = 0; i < HEIGHT; i++)
+ {
+ PXCPoint3DF32* vertices_row = &vertices[i * WIDTH];
+ pcl::PointXYZRGBA* cloud_row = &xyzrgba_cloud->points[i * WIDTH];
+ uint32_t* color_row = &d[i * data.pitches[0] / sizeof (uint32_t)];
+ for (int j = 0; j < WIDTH; j++)
+ {
+ convertPoint (vertices_row[j], cloud_row[j]);
+ memcpy (&cloud_row[j].rgba, &color_row[j], sizeof (uint32_t));
+ }
+ }
+ }
+ mapped->ReleaseAccess (&data);
+ mapped->Release ();
+ }
+
+ if (need_xyzrgba_)
+ point_cloud_rgba_signal_->operator () (xyzrgba_cloud);
+ if (need_xyz_)
+ point_cloud_signal_->operator () (xyz_cloud);
+ break;
+ }
+ case PXC_STATUS_DEVICE_LOST:
+ THROW_IO_EXCEPTION ("failed to read data stream from PXC device: device lost");
+ case PXC_STATUS_ALLOC_FAILED:
+ THROW_IO_EXCEPTION ("failed to read data stream from PXC device: alloc failed");
+ }
+ sample.ReleaseImages ();
+ }
+ projection->Release ();
+ RealSenseDevice::reset (device_);
+}
+
+float
+pcl::RealSenseGrabber::computeModeScore (const Mode& mode)
+{
+ const float FPS_WEIGHT = 100000;
+ const float DEPTH_WEIGHT = 1000;
+ const float COLOR_WEIGHT = 1;
+ int f = mode.fps - mode_requested_.fps;
+ int dw = mode.depth_width - mode_requested_.depth_width;
+ int dh = mode.depth_height - mode_requested_.depth_height;
+ int cw = mode.color_width - mode_requested_.color_width;
+ int ch = mode.color_height - mode_requested_.color_height;
+ float penalty;
+ penalty = std::abs (FPS_WEIGHT * f * (mode_requested_.fps != 0));
+ penalty += std::abs (DEPTH_WEIGHT * dw * (mode_requested_.depth_width != 0));
+ penalty += std::abs (DEPTH_WEIGHT * dh * (mode_requested_.depth_height != 0));
+ penalty += std::abs (COLOR_WEIGHT * cw * (mode_requested_.color_width != 0 && need_xyzrgba_));
+ penalty += std::abs (COLOR_WEIGHT * ch * (mode_requested_.color_height != 0 && need_xyzrgba_));
+ return penalty;
+}
+
+void
+pcl::RealSenseGrabber::selectMode ()
+{
+ if (mode_requested_ == Mode ())
+ mode_requested_ = Mode (30, 640, 480, 640, 480);
+ float best_score = std::numeric_limits<float>::max ();
+ std::vector<Mode> modes = getAvailableModes (!need_xyzrgba_);
+ for (size_t i = 0; i < modes.size (); ++i)
+ {
+ Mode mode = modes[i];
+ float score = computeModeScore (mode);
+ if (score < best_score)
+ {
+ best_score = score;
+ mode_selected_ = mode;
+ }
+ }
+ if (strict_ && best_score > 0)
+ THROW_IO_EXCEPTION ("PXC device does not support requested mode");
+}
+
+void
+pcl::RealSenseGrabber::createDepthBuffer ()
+{
+ size_t size = mode_selected_.depth_width * mode_selected_.depth_height;
+ switch (temporal_filtering_type_)
+ {
+ case RealSense_None:
+ {
+ depth_buffer_.reset (new pcl::io::SingleBuffer<unsigned short> (size));
+ break;
+ }
+ case RealSense_Median:
+ {
+ depth_buffer_.reset (new pcl::io::MedianBuffer<unsigned short> (size, temporal_filtering_window_size_));
+ break;
+ }
+ case RealSense_Average:
+ {
+ depth_buffer_.reset (new pcl::io::AverageBuffer<unsigned short> (size, temporal_filtering_window_size_));
+ break;
+ }
+ }
+}
+
boost::shared_array<unsigned char> data;
if (!packet_queue_.dequeue (data))
return;
-
- convertPacketData (data.get(), 464);
+ convertPacketData (data.get(), data_size_);
}
}
/////////////////////////////////////////////////////////////////////////////
void
-pcl::RobotEyeGrabber::convertPacketData (unsigned char *dataPacket, size_t length)
+pcl::RobotEyeGrabber::convertPacketData (unsigned char *data_packet, size_t length)
{
- const size_t bytesPerPoint = 8;
- const size_t totalPoints = length / bytesPerPoint;
+ //Check for the presence of the header
+ size_t offset = 0;
+ //The old packet data format does not start with an E since it just starts with laser data
+ if(data_packet[0] != 'E')
+ {
+ //old packet data format (just laser data)
+ offset = 0;
+ }
+ else
+ {
+ //The new packet data format contains this as a header
+ //char[6] "EBRBEP"
+ //uint32_t Timestamp // counts of a 66 MHz clock since power-on of eye.
+ size_t response_size = 6; //"EBRBEP"
+ if( !strncmp((char*)(data_packet), "EBRBEP", response_size) )
+ {
+ boost::uint32_t timestamp; // counts of a 66 MHz clock since power-on of eye.
+ computeTimestamp(timestamp, data_packet + response_size);
+ //std::cout << "Timestamp: " << timestamp << std::endl;
+ offset = (response_size + sizeof(timestamp));
+ }
+ else
+ {
+ //Invalid packet received, ignore it.
+ return;
+ }
+ }
- for (size_t i = 0; i < totalPoints; ++i)
+ const size_t bytes_per_point = 8;
+ const size_t total_points = (length - offset)/ bytes_per_point;
+
+ for (size_t i = 0; i < total_points; ++i)
{
PointXYZI xyzi;
- computeXYZI (xyzi, dataPacket + i*bytesPerPoint);
+ computeXYZI (xyzi, data_packet + i*bytes_per_point + offset);
if (pcl::isFinite(xyzi))
{
}
}
-
if (point_cloud_xyzi_->size () > signal_point_cloud_size_)
{
if (point_cloud_signal_->num_slots () > 0)
/////////////////////////////////////////////////////////////////////////////
void
-pcl::RobotEyeGrabber::computeXYZI (pcl::PointXYZI& point, unsigned char* pointData)
+pcl::RobotEyeGrabber::computeXYZI (pcl::PointXYZI& point, unsigned char* point_data)
{
uint16_t buffer = 0;
double az = 0.0;
uint16_t intensity = 0;
buffer = 0x00;
- buffer = pointData[0] << 8;
- buffer |= pointData[1]; // First 2-byte read will be Azimuth
+ buffer = point_data[0] << 8;
+ buffer |= point_data[1]; // First 2-byte read will be Azimuth
az = (buffer / 100.0);
buffer = 0x00;
- buffer = pointData[2] << 8;
- buffer |= pointData[3]; // Second 2-byte read will be Elevation
+ buffer = point_data[2] << 8;
+ buffer |= point_data[3]; // Second 2-byte read will be Elevation
el = (signed short int)buffer / 100.0;
buffer = 0x00;
- buffer = pointData[4] << 8;
- buffer |= pointData[5]; // Third 2-byte read will be Range
+ buffer = point_data[4] << 8;
+ buffer |= point_data[5]; // Third 2-byte read will be Range
range = (signed short int)buffer / 100.0;
buffer = 0x00;
- buffer = pointData[6] << 8;
- buffer |= pointData[7]; // Fourth 2-byte read will be Intensity
+ buffer = point_data[6] << 8;
+ buffer |= point_data[7]; // Fourth 2-byte read will be Intensity
intensity = buffer;
point.x = range * std::cos (el * M_PI/180) * std::sin (az * M_PI/180);
/////////////////////////////////////////////////////////////////////////////
void
-pcl::RobotEyeGrabber::socketThreadLoop()
+pcl::RobotEyeGrabber::computeTimestamp (boost::uint32_t& timestamp, unsigned char* point_data)
+{
+ boost::uint32_t buffer = 0;
+
+ buffer = 0x00;
+ buffer = point_data[0] << 24;
+ buffer |= point_data[1] << 16;
+ buffer |= point_data[2] << 8;
+ buffer |= point_data[3];
+ timestamp = buffer;
+}
+
+
+
+/////////////////////////////////////////////////////////////////////////////
+void
+pcl::RobotEyeGrabber::socketThreadLoop ()
{
asyncSocketReceive();
io_service_.reset();
/////////////////////////////////////////////////////////////////////////////
void
-pcl::RobotEyeGrabber::asyncSocketReceive()
+pcl::RobotEyeGrabber::asyncSocketReceive ()
{
- // expecting exactly 464 bytes, using a larger buffer so that if a
- // larger packet arrives unexpectedly we'll notice it.
- socket_->async_receive_from(boost::asio::buffer(receive_buffer_, 500), sender_endpoint_,
- boost::bind(&RobotEyeGrabber::socketCallback, this,
- boost::asio::placeholders::error,
- boost::asio::placeholders::bytes_transferred));
+ // expecting at most max_length bytes (UDP packet).
+ socket_->async_receive_from(boost::asio::buffer(receive_buffer_, MAX_LENGTH), sender_endpoint_,
+ boost::bind(&RobotEyeGrabber::socketCallback, this, boost::asio::placeholders::error,
+ boost::asio::placeholders::bytes_transferred));
}
/////////////////////////////////////////////////////////////////////////////
void
-pcl::RobotEyeGrabber::socketCallback(const boost::system::error_code&, std::size_t numberOfBytes)
+pcl::RobotEyeGrabber::socketCallback (const boost::system::error_code&, std::size_t number_of_bytes)
{
if (terminate_thread_)
return;
if (sensor_address_ == boost::asio::ip::address_v4::any ()
- || sensor_address_ == sender_endpoint_.address ())
+ || sensor_address_ == sender_endpoint_.address ())
{
- if (numberOfBytes == 464)
- {
- unsigned char *dup = new unsigned char[numberOfBytes];
- memcpy (dup, receive_buffer_, numberOfBytes);
- packet_queue_.enqueue (boost::shared_array<unsigned char>(dup));
- }
+ data_size_ = number_of_bytes;
+ unsigned char *dup = new unsigned char[number_of_bytes];
+ memcpy (dup, receive_buffer_, number_of_bytes);
+ packet_queue_.enqueue (boost::shared_array<unsigned char>(dup));
}
asyncSocketReceive ();
void
pcl::RobotEyeGrabber::start ()
{
+ resetPointCloud ();
+
if (isRunning ())
return;
try
{
- socket_.reset (new boost::asio::ip::udp::socket (io_service_, destinationEndpoint));
+ socket_.reset (new boost::asio::ip::udp::socket (io_service_, destinationEndpoint));
}
catch (std::exception &e)
{
- PCL_ERROR ("[pcl::RobotEyeGrabber::start] Unable to bind to socket! %s\n", e.what ());
+ PCL_ERROR ("[pcl::RobotEyeGrabber::start] Unable to bind to socket! %s\n", e.what ());
return;
}
terminate_thread_ = true;
-
socket_->close ();
io_service_.stop ();
socket_thread_->join ();
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2015-, Open Perception, Inc.
+ * Copyright (c) 2015 The MITRE Corporation
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/io/vlp_grabber.h>
+
+using boost::asio::ip::udp;
+
+#define VLP_MAX_NUM_LASERS 16
+#define VLP_DUAL_MODE 0x39
+
+/////////////////////////////////////////////////////////////////////////////
+pcl::VLPGrabber::VLPGrabber (const std::string& pcapFile) :
+ HDLGrabber ("", pcapFile)
+{
+ loadVLP16Corrections ();
+}
+
+/////////////////////////////////////////////////////////////////////////////
+pcl::VLPGrabber::VLPGrabber (const boost::asio::ip::address& ipAddress,
+ const unsigned short int port) :
+ HDLGrabber (ipAddress, port)
+{
+ loadVLP16Corrections ();
+}
+
+/////////////////////////////////////////////////////////////////////////////
+pcl::VLPGrabber::~VLPGrabber () throw ()
+{
+}
+
+/////////////////////////////////////////////////////////////////////////////
+void
+pcl::VLPGrabber::loadVLP16Corrections ()
+{
+ double vlp16_vertical_corrections[] = { -15, 1, -13, 3, -11, 5, -9, 7, -7, 9, -5, 11, -3, 13, -1, 15
+
+ };
+ for (int i = 0; i < VLP_MAX_NUM_LASERS; i++)
+ {
+ HDLGrabber::laser_corrections_[i].azimuthCorrection = 0.0;
+ HDLGrabber::laser_corrections_[i].distanceCorrection = 0.0;
+ HDLGrabber::laser_corrections_[i].horizontalOffsetCorrection = 0.0;
+ HDLGrabber::laser_corrections_[i].verticalOffsetCorrection = 0.0;
+ HDLGrabber::laser_corrections_[i].verticalCorrection = vlp16_vertical_corrections[i];
+ HDLGrabber::laser_corrections_[i].sinVertCorrection = std::sin (HDL_Grabber_toRadians(vlp16_vertical_corrections[i]));
+ HDLGrabber::laser_corrections_[i].cosVertCorrection = std::cos (HDL_Grabber_toRadians(vlp16_vertical_corrections[i]));
+ }
+}
+
+/////////////////////////////////////////////////////////////////////////////
+boost::asio::ip::address
+pcl::VLPGrabber::getDefaultNetworkAddress ()
+{
+ return (boost::asio::ip::address::from_string ("255.255.255.255"));
+}
+
+/////////////////////////////////////////////////////////////////////////////
+void
+pcl::VLPGrabber::toPointClouds (HDLDataPacket *dataPacket)
+{
+ static uint32_t scan_counter = 0;
+ static uint32_t sweep_counter = 0;
+ if (sizeof(HDLLaserReturn) != 3)
+ return;
+
+ time_t system_time;
+ time (&system_time);
+ time_t velodyne_time = (system_time & 0x00000000ffffffffl) << 32 | dataPacket->gpsTimestamp;
+
+ scan_counter++;
+
+ double interpolated_azimuth_delta;
+
+ int index = 1;
+ if (dataPacket->mode == VLP_DUAL_MODE)
+ {
+ index = 2;
+ }
+ if (dataPacket->firingData[index].rotationalPosition < dataPacket->firingData[0].rotationalPosition)
+ {
+ interpolated_azimuth_delta = ( (dataPacket->firingData[index].rotationalPosition + 36000) - dataPacket->firingData[0].rotationalPosition) / 2.0;
+ }
+ else
+ {
+ interpolated_azimuth_delta = (dataPacket->firingData[index].rotationalPosition - dataPacket->firingData[0].rotationalPosition) / 2.0;
+ }
+
+ for (int i = 0; i < HDL_FIRING_PER_PKT; ++i)
+ {
+ HDLFiringData firing_data = dataPacket->firingData[i];
+
+ for (int j = 0; j < HDL_LASER_PER_FIRING; j++)
+ {
+ double current_azimuth = firing_data.rotationalPosition;
+ if (j > VLP_MAX_NUM_LASERS)
+ {
+ current_azimuth += interpolated_azimuth_delta;
+ }
+ if (current_azimuth > 36000)
+ {
+ current_azimuth -= 36000;
+ }
+ if (current_azimuth < HDLGrabber::last_azimuth_)
+ {
+ if (current_sweep_xyz_->size () > 0)
+ {
+ current_sweep_xyz_->is_dense = current_sweep_xyzi_->is_dense = false;
+ current_sweep_xyz_->header.stamp = velodyne_time;
+ current_sweep_xyzi_->header.stamp = velodyne_time;
+ current_sweep_xyz_->header.seq = sweep_counter;
+ current_sweep_xyzi_->header.seq = sweep_counter;
+
+ sweep_counter++;
+
+ HDLGrabber::fireCurrentSweep ();
+ }
+ current_sweep_xyz_.reset (new pcl::PointCloud<pcl::PointXYZ> ());
+ current_sweep_xyzi_.reset (new pcl::PointCloud<pcl::PointXYZI> ());
+ }
+
+ PointXYZ xyz;
+ PointXYZI xyzi;
+ PointXYZ dual_xyz;
+ PointXYZI dual_xyzi;
+
+ HDLGrabber::computeXYZI (xyzi, current_azimuth, firing_data.laserReturns[j], laser_corrections_[j % VLP_MAX_NUM_LASERS]);
+
+ xyz.x = xyzi.x;
+ xyz.y = xyzi.y;
+ xyz.z = xyzi.z;
+
+ if (dataPacket->mode == VLP_DUAL_MODE)
+ {
+ HDLGrabber::computeXYZI (dual_xyzi, current_azimuth, dataPacket->firingData[i + 1].laserReturns[j], laser_corrections_[j % VLP_MAX_NUM_LASERS]);
+
+ dual_xyz.x = dual_xyzi.x;
+ dual_xyz.y = dual_xyzi.y;
+ dual_xyz.z = dual_xyzi.z;
+
+ }
+
+ if (! (boost::math::isnan (xyz.x) || boost::math::isnan (xyz.y) || boost::math::isnan (xyz.z)))
+ {
+ current_sweep_xyz_->push_back (xyz);
+ current_sweep_xyzi_->push_back (xyzi);
+
+ last_azimuth_ = current_azimuth;
+ }
+ if (dataPacket->mode == VLP_DUAL_MODE)
+ {
+ if ( (dual_xyz.x != xyz.x || dual_xyz.y != xyz.y || dual_xyz.z != xyz.z)
+ && ! (boost::math::isnan (dual_xyz.x) || boost::math::isnan (dual_xyz.y) || boost::math::isnan (dual_xyz.z)))
+ {
+ current_sweep_xyz_->push_back (dual_xyz);
+ current_sweep_xyzi_->push_back (dual_xyzi);
+ }
+ }
+ }
+ if (dataPacket->mode == VLP_DUAL_MODE)
+ {
+ i++;
+ }
+ }
+}
+
+/////////////////////////////////////////////////////////////////////////////
+std::string
+pcl::VLPGrabber::getName () const
+{
+ return (std::string ("Velodyne LiDAR (VLP) Grabber"));
+}
+
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-int
-pcl::io::savePolygonFile (const std::string &file_name, const pcl::PolygonMesh& mesh)
+bool
+pcl::io::savePolygonFile (const std::string &file_name,
+ const pcl::PolygonMesh& mesh,
+ const bool binary_format)
{
- // TODO: what about binary/ASCII modes?!?!?!
// TODO: what about sensor position and orientation?!?!?!?
- // TODO: how to adequately catch exceptions thrown by the vtk writers?!
std::string extension = file_name.substr (file_name.find_last_of (".") + 1);
- if (extension == "pcd") // no Polygon, but only a point cloud
- {
- int error_code = pcl::io::savePCDFile (file_name, mesh.cloud);
- if (error_code != 0)
- return (0);
- return (static_cast<int> (mesh.cloud.width * mesh.cloud.height));
- }
+ if (extension == "pcd") // no Polygon, but only a point cloud
+ return (pcl::io::savePCDFile (file_name, mesh.cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_format) == 0);
else if (extension == "vtk")
- return (pcl::io::savePolygonFileVTK (file_name, mesh));
+ return (pcl::io::savePolygonFileVTK (file_name, mesh, binary_format));
else if (extension == "ply")
- return (pcl::io::savePolygonFilePLY (file_name, mesh));
- else if (extension == "stl" )
- return (pcl::io::savePolygonFileSTL (file_name, mesh));
+ return (pcl::io::savePolygonFilePLY (file_name, mesh, binary_format));
+ else if (extension == "stl")
+ return (pcl::io::savePolygonFileSTL (file_name, mesh, binary_format));
else
{
PCL_ERROR ("[pcl::io::savePolygonFile]: Unsupported file type (%s)\n", extension.c_str ());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-int
-pcl::io::savePolygonFileVTK (const std::string &file_name, const pcl::PolygonMesh& mesh)
+bool
+pcl::io::savePolygonFileVTK (const std::string &file_name,
+ const pcl::PolygonMesh& mesh,
+ const bool binary_format)
{
vtkSmartPointer<vtkPolyData> poly_data = vtkSmartPointer<vtkPolyData>::New ();
#else
poly_writer->SetInputData (poly_data);
#endif
- poly_writer->SetFileName (file_name.c_str ());
- poly_writer->Write ();
- return (static_cast<int> (mesh.cloud.width * mesh.cloud.height));
+ if (binary_format)
+ poly_writer->SetFileTypeToBinary ();
+ else
+ poly_writer->SetFileTypeToASCII ();
+
+ poly_writer->SetFileName (file_name.c_str ());
+ return (poly_writer->Write ());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-int
-pcl::io::savePolygonFilePLY (const std::string &file_name, const pcl::PolygonMesh& mesh)
+bool
+pcl::io::savePolygonFilePLY (const std::string &file_name,
+ const pcl::PolygonMesh& mesh,
+ const bool binary_format)
{
vtkSmartPointer<vtkPolyData> poly_data = vtkSmartPointer<vtkPolyData>::New ();
#else
poly_writer->SetInputData (poly_data);
#endif
+
+ if (binary_format)
+ poly_writer->SetFileTypeToBinary ();
+ else
+ poly_writer->SetFileTypeToASCII ();
+
poly_writer->SetFileName (file_name.c_str ());
poly_writer->SetArrayName ("Colors");
- poly_writer->Write ();
-
- return (static_cast<int> (mesh.cloud.width * mesh.cloud.height));
+ return (poly_writer->Write ());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-int
-pcl::io::savePolygonFileSTL (const std::string &file_name, const pcl::PolygonMesh& mesh)
+bool
+pcl::io::savePolygonFileSTL (const std::string &file_name,
+ const pcl::PolygonMesh& mesh,
+ const bool binary_format)
{
vtkSmartPointer<vtkPolyData> poly_data = vtkSmartPointer<vtkPolyData>::New ();
#else
poly_writer->SetInputData (poly_data);
#endif
- poly_writer->SetFileName (file_name.c_str ());
- poly_writer->Write ();
- return (static_cast<int> (mesh.cloud.width * mesh.cloud.height));
+ if (binary_format)
+ poly_writer->SetFileTypeToBinary ();
+ else
+ poly_writer->SetFileTypeToASCII ();
+
+ poly_writer->SetFileName (file_name.c_str ());
+ return (poly_writer->Write ());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Add dummy material
mesh.tex_materials.push_back (pcl::TexMaterial ());
- std::vector<Eigen::Vector2f> dummy;
+ std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > dummy;
mesh.tex_coordinates.push_back (dummy);
vtkIdType nr_points = poly_data->GetNumberOfPoints ();
-if(OPENNI_FOUND)
+if(WITH_OPENNI)
PCL_ADD_EXECUTABLE(pcl_openni_grabber_example ${SUBSYS_NAME} openni_grabber_example.cpp)
target_link_libraries(pcl_openni_grabber_example pcl_common pcl_io)
PCL_ADD_EXECUTABLE(pcl_openni_pcd_recorder ${SUBSYS_NAME} openni_pcd_recorder.cpp)
target_link_libraries(pcl_openni_pcd_recorder pcl_common pcl_io)
-
-endif(OPENNI_FOUND)
+endif()
PCL_ADD_EXECUTABLE(pcl_pcd_convert_NaN_nan ${SUBSYS_NAME} pcd_convert_NaN_nan.cpp)
+PCL_ADD_EXECUTABLE(pcl_pcd_introduce_nan ${SUBSYS_NAME} pcd_introduce_nan.cpp)
PCL_ADD_EXECUTABLE(pcl_convert_pcd_ascii_binary ${SUBSYS_NAME} convert_pcd_ascii_binary.cpp)
+if (VTK_FOUND)
+ PCL_ADD_EXECUTABLE(pcl_converter ${SUBSYS_NAME} converter.cpp)
+ target_link_libraries(pcl_converter pcl_common pcl_io)
+endif ()
PCL_ADD_EXECUTABLE(pcl_hdl_grabber ${SUBSYS_NAME} hdl_grabber_example.cpp)
target_link_libraries(pcl_convert_pcd_ascii_binary pcl_common pcl_io)
target_link_libraries(pcl_hdl_grabber pcl_common pcl_io)
+target_link_libraries(pcl_pcd_introduce_nan pcl_common pcl_io)
#libply inherited tools
add_subdirectory(ply)
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ \author Victor Lamoine
+ @b convert_point_clouds_meshes converts OBJ, PCD, PLY, STL, VTK files containing point clouds or meshes into every other format.
+ This tool allows to specify the file output type between ASCII, binary and binary compressed.
+ **/
+
+//TODO: Inform user about loss of color/alpha during conversion?
+// STL does not support color at all
+// OBJ does not support color in PCL (the format DOES support color)
+
+#include <vector>
+
+#include <pcl/console/parse.h>
+#include <pcl/io/auto_io.h>
+#include <pcl/io/obj_io.h>
+#include <pcl/io/vtk_lib_io.h>
+
+#include <boost/make_shared.hpp>
+
+#define ASCII 0
+#define BINARY 1
+#define BINARY_COMPRESSED 2
+
+/**
+ * Display help for this program
+ * @param argc[in]
+ * @param argv[in]
+ */
+void
+displayHelp (int argc,
+ char** argv)
+{
+ PCL_INFO ("\nUsage: %s [OPTION] SOURCE DEST\n", argv[0]);
+ PCL_INFO ("Convert SOURCE point cloud or mesh to DEST.\n\n");
+
+ PCL_INFO ("Available formats types for SOURCE and DEST:\n"
+ "\tOBJ (Wavefront)\n"
+ "\tPCD (Point Cloud Library)\n"
+ "\tPLY (Polygon File Format)\n"
+ "\tSTL (STereoLithography)\n"
+ "\tVTK (The Visualization Toolkit)\n\n");
+
+ PCL_INFO ("Available options:\n"
+ "\t-f, --format Specify DEST output type, available formats are ascii, binary and binary_compressed.\n"
+ "\t When not specified, binary is used as default.\n"
+ "\t OBJ only supports ascii format.\n"
+ "\t binary_compressed is only supported by the PCD file format.\n\n"
+ "\t-c --cloud Output DEST as a point cloud, delete all faces.\n\n");
+}
+
+bool
+saveMesh (pcl::PolygonMesh& input,
+ std::string output_file,
+ int output_type);
+
+/**
+ * Saves a cloud into the specified file and output type. The file format is automatically parsed.
+ * @param input[in] The cloud to be saved
+ * @param output_file[out] The output file to be written
+ * @param output_type[in] The output file type
+ * @return True on success, false otherwise.
+ */
+bool
+savePointCloud (pcl::PCLPointCloud2::Ptr input,
+ std::string output_file,
+ int output_type)
+{
+ if (boost::filesystem::path (output_file).extension () == ".pcd")
+ {
+ //TODO Support precision, origin, orientation
+ pcl::PCDWriter w;
+ if (output_type == ASCII)
+ {
+ PCL_INFO ("Saving file %s as ASCII.\n", output_file.c_str ());
+ if (w.writeASCII (output_file, *input) != 0)
+ return (false);
+ }
+ else if (output_type == BINARY)
+ {
+ PCL_INFO ("Saving file %s as binary.\n", output_file.c_str ());
+ if (w.writeBinary (output_file, *input) != 0)
+ return (false);
+ }
+ else if (output_type == BINARY_COMPRESSED)
+ {
+ PCL_INFO ("Saving file %s as binary compressed.\n", output_file.c_str ());
+ if (w.writeBinaryCompressed (output_file, *input) != 0)
+ return (false);
+ }
+ }
+ else if (boost::filesystem::path (output_file).extension () == ".stl")
+ {
+ PCL_ERROR ("STL file format does not support point clouds! Aborting.\n");
+ return (false);
+ }
+ else // OBJ, PLY and VTK
+ {
+ //TODO: Support precision
+ //FIXME: Color is lost during OBJ conversion (OBJ supports color)
+ pcl::PolygonMesh mesh;
+ mesh.cloud = *input;
+ if (!saveMesh (mesh, output_file, output_type))
+ return (false);
+ }
+
+ return (true);
+}
+
+/**
+ * Saves a mesh into the specified file and output type. The file format is automatically parsed.
+ * @param input[in] The mesh to be saved
+ * @param output_file[out] The output file to be written
+ * @param output_type[in] The output file type
+ * @return True on success, false otherwise.
+ */
+bool
+saveMesh (pcl::PolygonMesh& input,
+ std::string output_file,
+ int output_type)
+{
+ if (boost::filesystem::path (output_file).extension () == ".obj")
+ {
+ if (output_type == BINARY || output_type == BINARY_COMPRESSED)
+ PCL_WARN ("OBJ file format only supports ASCII.\n");
+
+ //TODO: Support precision
+ //FIXME: Color is lost during conversion (OBJ supports color)
+ PCL_INFO ("Saving file %s as ASCII.\n", output_file.c_str ());
+ if (pcl::io::saveOBJFile (output_file, input) != 0)
+ return (false);
+ }
+ else if (boost::filesystem::path (output_file).extension () == ".pcd")
+ {
+ if (!input.polygons.empty ())
+ PCL_WARN ("PCD file format does not support meshes! Only points be saved.\n");
+ pcl::PCLPointCloud2::Ptr cloud = boost::make_shared<pcl::PCLPointCloud2> (input.cloud);
+ if (!savePointCloud (cloud, output_file, output_type))
+ return (false);
+ }
+ else // PLY, STL and VTK
+ {
+ if (output_type == BINARY_COMPRESSED)
+ PCL_WARN ("PLY, STL and VTK file formats only supports ASCII and binary output file types.\n");
+
+ if (input.polygons.empty() && boost::filesystem::path (output_file).extension () == ".stl")
+ {
+ PCL_ERROR ("STL file format does not support point clouds! Aborting.\n");
+ return (false);
+ }
+
+ PCL_INFO ("Saving file %s as %s.\n", output_file.c_str (), (output_type == ASCII) ? "ASCII" : "binary");
+ if (!pcl::io::savePolygonFile (output_file, input, (output_type == ASCII) ? false : true))
+ return (false);
+ }
+
+ return (true);
+}
+
+/**
+ * Parse input files and options. Calls the right conversion function.
+ * @param argc[in]
+ * @param argv[in]
+ * @return 0 on success, any other value on failure.
+ */
+int
+main (int argc,
+ char** argv)
+{
+ // Display help
+ if (pcl::console::find_switch (argc, argv, "-h") != 0 || pcl::console::find_switch (argc, argv, "--help") != 0)
+ {
+ displayHelp (argc, argv);
+ return (0);
+ }
+
+ // Parse all files and options
+ std::vector<std::string> supported_extensions;
+ supported_extensions.push_back("obj");
+ supported_extensions.push_back("pcd");
+ supported_extensions.push_back("ply");
+ supported_extensions.push_back("stl");
+ supported_extensions.push_back("vtk");
+ std::vector<int> file_args;
+ for (int i = 1; i < argc; ++i)
+ for (size_t j = 0; j < supported_extensions.size(); ++j)
+ if (boost::algorithm::ends_with(argv[i], supported_extensions[j]))
+ {
+ file_args.push_back(i);
+ break;
+ }
+
+ std::string parsed_output_type;
+ pcl::console::parse_argument (argc, argv, "-f", parsed_output_type);
+ pcl::console::parse_argument (argc, argv, "--format", parsed_output_type);
+ bool cloud_output (false);
+ if (pcl::console::find_switch (argc, argv, "-c") != 0 ||
+ pcl::console::find_switch (argc, argv, "--cloud") != 0)
+ cloud_output = true;
+
+ // Make sure that we have one input and one output file only
+ if (file_args.size() != 2)
+ {
+ PCL_ERROR ("Wrong input/output file count!\n");
+ displayHelp (argc, argv);
+ return (-1);
+ }
+
+ // Convert parsed output type to output type
+ int output_type (BINARY);
+ if (!parsed_output_type.empty ())
+ {
+ if (parsed_output_type == "ascii")
+ output_type = ASCII;
+ else if (parsed_output_type == "binary")
+ output_type = BINARY;
+ else if (parsed_output_type == "binary_compressed")
+ output_type = BINARY_COMPRESSED;
+ else
+ {
+ PCL_ERROR ("Wrong output type!\n");
+ displayHelp (argc, argv);
+ return (-1);
+ }
+ }
+
+ // Try to load as mesh
+ pcl::PolygonMesh mesh;
+ if (boost::filesystem::path (argv[file_args[0]]).extension () != ".pcd" &&
+ pcl::io::loadPolygonFile (argv[file_args[0]], mesh) != 0)
+ {
+ PCL_INFO ("Loaded a mesh with %d points (total size is %d) and the following channels:\n%s\n",
+ mesh.cloud.width * mesh.cloud.height, mesh.cloud.data.size (), pcl::getFieldsList (mesh.cloud).c_str ());
+
+ if (cloud_output)
+ mesh.polygons.clear();
+
+ if (saveMesh (mesh, argv[file_args[1]], output_type))
+ return (-1);
+ }
+ else if (boost::filesystem::path (argv[file_args[0]]).extension () == ".stl")
+ {
+ PCL_ERROR ("Unable to load %s.\n", argv[file_args[0]]);
+ return (-1);
+ }
+ else
+ {
+ // PCD, OBJ, PLY or VTK
+ if (boost::filesystem::path (argv[file_args[0]]).extension () != ".pcd")
+ PCL_WARN ("Could not load %s as a mesh, trying as a point cloud instead.\n", argv[file_args[0]]);
+
+ //Eigen::Vector4f origin; // TODO: Support origin/orientation
+ //Eigen::Quaternionf orientation;
+ pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2);
+ if (pcl::io::load (argv[file_args[0]], *cloud) < 0)
+ {
+ PCL_ERROR ("Unable to load %s.\n", argv[file_args[0]]);
+ return (-1);
+ }
+
+ PCL_INFO ("Loaded a point cloud with %d points (total size is %d) and the following channels:\n%s\n", cloud->width * cloud->height, cloud->data.size (),
+ pcl::getFieldsList (*cloud).c_str ());
+
+ if (!savePointCloud (cloud, argv[file_args[1]], output_type))
+ {
+ PCL_ERROR ("Failed to save %s.\n", argv[file_args[1]]);
+ return (-1);
+ }
+ }
+ return (0);
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/common/common.h>
+#include <pcl/io/pcd_io.h>
+
+/** @brief PCL point object */
+typedef pcl::PointXYZRGBA PointT;
+
+/** @brief PCL Point cloud object */
+typedef pcl::PointCloud<PointT> PointCloudT;
+
+int
+main (int argc,
+ char** argv)
+{
+ if (argc != 3 && argc != 4)
+ {
+ PCL_ERROR ("Usage: %s cloud_in.pcd cloud_out_ascii.pcd percentage_of_NaN \n", argv[0]);
+ return (-1);
+ }
+
+ int percentage_of_NaN = 20;
+ if (argc == 4)
+ percentage_of_NaN = boost::lexical_cast<int>(argv[3]);
+
+ PCL_INFO ("Replacing approximately %d%% of the cloud with NaN values (already existing NaN values are conserved)\n", percentage_of_NaN);
+ PointCloudT::Ptr cloud (new PointCloudT);
+ if (pcl::io::loadPCDFile (argv[1], *cloud) != 0)
+ return (-1);
+
+ for (PointCloudT::iterator cloud_it (cloud->begin ()); cloud_it != cloud->end (); ++cloud_it)
+ {
+ int random = 1 + (rand () % (int) (100));
+ int random_xyz = 1 + (rand () % (int) (3 - 1 + 1));
+
+ if (random < percentage_of_NaN)
+ {
+ if (random_xyz == 1)
+ cloud_it->x = std::numeric_limits<double>::quiet_NaN ();
+ else if (random_xyz == 2)
+ cloud_it->y = std::numeric_limits<double>::quiet_NaN ();
+ else
+ cloud_it->z = std::numeric_limits<double>::quiet_NaN ();
+ }
+ }
+
+ pcl::io::savePCDFile (argv[2], *cloud);
+ return (0);
+}
+
if(build)
set(srcs
src/narf_keypoint.cpp
- src/uniform_sampling.cpp
src/sift_keypoint.cpp
src/smoothed_surfaces_keypoint.cpp
src/harris_3d.cpp
src/agast_2d.cpp
src/susan.cpp
src/iss_3d.cpp
+ src/brisk_2d.cpp
)
set(incs
"include/pcl/${SUBSYS_NAME}/keypoint.h"
"include/pcl/${SUBSYS_NAME}/uniform_sampling.h"
"include/pcl/${SUBSYS_NAME}/smoothed_surfaces_keypoint.h"
"include/pcl/${SUBSYS_NAME}/agast_2d.h"
+ "include/pcl/${SUBSYS_NAME}/harris_2d.h"
"include/pcl/${SUBSYS_NAME}/harris_3d.h"
"include/pcl/${SUBSYS_NAME}/harris_6d.h"
"include/pcl/${SUBSYS_NAME}/susan.h"
"include/pcl/${SUBSYS_NAME}/iss_3d.h"
+ "include/pcl/${SUBSYS_NAME}/brisk_2d.h"
)
set(impl_incs
"include/pcl/${SUBSYS_NAME}/impl/keypoint.hpp"
"include/pcl/${SUBSYS_NAME}/impl/sift_keypoint.hpp"
- "include/pcl/${SUBSYS_NAME}/impl/uniform_sampling.hpp"
"include/pcl/${SUBSYS_NAME}/impl/smoothed_surfaces_keypoint.hpp"
"include/pcl/${SUBSYS_NAME}/impl/agast_2d.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/harris_2d.hpp"
"include/pcl/${SUBSYS_NAME}/impl/harris_3d.hpp"
"include/pcl/${SUBSYS_NAME}/impl/harris_6d.hpp"
"include/pcl/${SUBSYS_NAME}/impl/susan.hpp"
"include/pcl/${SUBSYS_NAME}/impl/iss_3d.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/brisk_2d.hpp"
)
set(LIB_NAME "pcl_${SUBSYS_NAME}")
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (C) 2011, The Autonomous Systems Lab (ASL), ETH Zurich,
+ * Stefan Leutenegger, Simon Lynen and Margarita Chli.
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_KEYPOINTS_BRISK_KEYPOINT_2D_H_
+#define PCL_KEYPOINTS_BRISK_KEYPOINT_2D_H_
+
+#include <pcl/keypoints/agast_2d.h>
+
+namespace pcl
+{
+ /** \brief Detects BRISK interest points based on the original code and paper
+ * reference by
+ *
+ * \par
+ * Stefan Leutenegger,Margarita Chli and Roland Siegwart,
+ * BRISK: Binary Robust Invariant Scalable Keypoints,
+ * in Proceedings of the IEEE International Conference on Computer Vision (ICCV2011).
+ *
+ * Code example:
+ *
+ * \code
+ * pcl::PointCloud<pcl::PointXYZRGBA> cloud;
+ * pcl::BriskKeypoint2D<pcl::PointXYZRGBA> brisk;
+ * brisk.setThreshold (60);
+ * brisk.setOctaves (4);
+ * brisk.setInputCloud (cloud);
+ *
+ * PointCloud<pcl::PointWithScale> keypoints;
+ * brisk.compute (keypoints);
+ * \endcode
+ *
+ * \author Radu B. Rusu, Stefan Holzer
+ * \ingroup keypoints
+ */
+ template <typename PointInT, typename PointOutT = pcl::PointWithScale, typename IntensityT = pcl::common::IntensityFieldAccessor<PointInT> >
+ class BriskKeypoint2D: public Keypoint<PointInT, PointOutT>
+ {
+ public:
+ typedef boost::shared_ptr<BriskKeypoint2D<PointInT, PointOutT, IntensityT> > Ptr;
+ typedef boost::shared_ptr<const BriskKeypoint2D<PointInT, PointOutT, IntensityT> > ConstPtr;
+
+ typedef typename Keypoint<PointInT, PointOutT>::PointCloudIn PointCloudIn;
+ typedef typename Keypoint<PointInT, PointOutT>::PointCloudOut PointCloudOut;
+ typedef typename Keypoint<PointInT, PointOutT>::KdTree KdTree;
+ typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
+
+ using Keypoint<PointInT, PointOutT>::name_;
+ using Keypoint<PointInT, PointOutT>::input_;
+ using Keypoint<PointInT, PointOutT>::indices_;
+ using Keypoint<PointInT, PointOutT>::k_;
+
+ /** \brief Constructor */
+ BriskKeypoint2D (int octaves = 4, int threshold = 60)
+ : threshold_ (threshold)
+ , octaves_ (octaves)
+ , remove_invalid_3D_keypoints_ (false)
+ {
+ k_ = 1;
+ name_ = "BriskKeypoint2D";
+ }
+
+ /** \brief Destructor. */
+ virtual ~BriskKeypoint2D ()
+ {
+ }
+
+ /** \brief Sets the threshold for corner detection.
+ * \param[in] threshold the threshold used for corner detection.
+ */
+ inline void
+ setThreshold (const int threshold)
+ {
+ threshold_ = threshold;
+ }
+
+ /** \brief Get the threshold for corner detection, as set by the user. */
+ inline size_t
+ getThreshold ()
+ {
+ return (threshold_);
+ }
+
+ /** \brief Set the number of octaves to use
+ * \param[in] octaves the number of octaves to use
+ */
+ inline void
+ setOctaves (const int octaves)
+ {
+ octaves_ = octaves;
+ }
+
+ /** \brief Returns the number of octaves used. */
+ inline int
+ getOctaves ()
+ {
+ return (octaves_);
+ }
+
+ /** \brief Specify whether we should do a 2nd pass through the list of keypoints
+ * found, and remove the ones that do not have a valid 3D (x-y-z) position
+ * (i.e., are NaN or Inf).
+ * \param[in] remove set to true whether we want the invalid 3D keypoints removed
+ */
+ inline void
+ setRemoveInvalid3DKeypoints (bool remove)
+ {
+ remove_invalid_3D_keypoints_ = remove;
+ }
+
+ /** \brief Specify whether the keypoints that do not have a valid 3D position are
+ * kept (false) or removed (true).
+ */
+ inline bool
+ getRemoveInvalid3DKeypoints ()
+ {
+ return (remove_invalid_3D_keypoints_);
+ }
+
+ /////////////////////////////////////////////////////////////////////////
+ inline void
+ bilinearInterpolation (const PointCloudInConstPtr &cloud,
+ float x, float y,
+ PointOutT &pt)
+ {
+ int u = int (x);
+ int v = int (y);
+
+ pt.x = pt.y = pt.z = 0;
+
+ const PointInT &p1 = (*cloud)(u, v);
+ const PointInT &p2 = (*cloud)(u+1, v);
+ const PointInT &p3 = (*cloud)(u, v+1);
+ const PointInT &p4 = (*cloud)(u+1, v+1);
+
+ float fx = x - float (u), fy = y - float (v);
+ float fx1 = 1.0f - fx, fy1 = 1.0f - fy;
+
+ float w1 = fx1 * fy1, w2 = fx * fy1, w3 = fx1 * fy, w4 = fx * fy;
+ float weight = 0;
+
+ if (pcl::isFinite (p1))
+ {
+ pt.x += p1.x * w1;
+ pt.y += p1.y * w1;
+ pt.z += p1.z * w1;
+ weight += w1;
+ }
+ if (pcl::isFinite (p2))
+ {
+ pt.x += p2.x * w2;
+ pt.y += p2.y * w2;
+ pt.z += p2.z * w2;
+ weight += w2;
+ }
+ if (pcl::isFinite (p3))
+ {
+ pt.x += p3.x * w3;
+ pt.y += p3.y * w3;
+ pt.z += p3.z * w3;
+ weight += w3;
+ }
+ if (pcl::isFinite (p4))
+ {
+ pt.x += p4.x * w4;
+ pt.y += p4.y * w4;
+ pt.z += p4.z * w4;
+ weight += w4;
+ }
+
+ if (weight == 0)
+ pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
+ else
+ {
+ weight = 1.0f / weight;
+ pt.x *= weight; pt.y *= weight; pt.z *= weight;
+ }
+ }
+
+ protected:
+ /** \brief Initializes everything and checks whether input data is fine. */
+ bool
+ initCompute ();
+
+ /** \brief Detects the keypoints. */
+ void
+ detectKeypoints (PointCloudOut &output);
+
+ private:
+ /** \brief Intensity field accessor. */
+ IntensityT intensity_;
+
+ /** \brief Threshold for corner detection. */
+ int threshold_;
+
+ int octaves_;
+
+ /** \brief Specify whether the keypoints that do not have a valid 3D position are
+ * kept (false) or removed (true).
+ */
+ bool remove_invalid_3D_keypoints_;
+ };
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ namespace keypoints
+ {
+ namespace brisk
+ {
+ /** \brief A layer in the BRISK detector pyramid. */
+ class PCL_EXPORTS Layer
+ {
+ public:
+ // constructor arguments
+ struct CommonParams
+ {
+ static const int HALFSAMPLE = 0;
+ static const int TWOTHIRDSAMPLE = 1;
+ };
+
+ /** \brief Constructor.
+ * \param[in] img input image
+ * \param[in] width image width
+ * \param[in] height image height
+ * \param[in] scale scale
+ * \param[in] offset offset
+ */
+ Layer (const std::vector<unsigned char>& img,
+ int width, int height,
+ float scale = 1.0f, float offset = 0.0f);
+
+ /** \brief Copy constructor for deriving a layer.
+ * \param[in] layer layer to derive from
+ * \param[in] mode deriving mode
+ */
+ Layer (const Layer& layer, int mode);
+
+ /** \brief AGAST keypoints without non-max suppression.
+ * \param[in] threshold the keypoints threshold
+ * \param[out] keypoints the AGAST keypoints
+ */
+ void
+ getAgastPoints (uint8_t threshold, std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > &keypoints);
+
+ // get scores - attention, this is in layer coordinates, not scale=1 coordinates!
+ /** \brief Get the AGAST keypoint score for a given pixel using a threshold
+ * \param[in] x the U coordinate of the pixel
+ * \param[in] y the V coordinate of the pixel
+ * \param[in] threshold the threshold to use for cutting the response
+ */
+ uint8_t
+ getAgastScore (int x, int y, uint8_t threshold);
+ /** \brief Get the AGAST keypoint score for a given pixel using a threshold
+ * \param[in] x the U coordinate of the pixel
+ * \param[in] y the V coordinate of the pixel
+ * \param[in] threshold the threshold to use for cutting the response
+ */
+ uint8_t
+ getAgastScore_5_8 (int x, int y, uint8_t threshold);
+ /** \brief Get the AGAST keypoint score for a given pixel using a threshold
+ * \param[in] xf the X coordinate of the pixel
+ * \param[in] yf the Y coordinate of the pixel
+ * \param[in] threshold the threshold to use for cutting the response
+ * \param[in] scale the scale
+ */
+ uint8_t
+ getAgastScore (float xf, float yf, uint8_t threshold, float scale = 1.0f);
+
+ /** \brief Access gray values (smoothed/interpolated)
+ * \param[in] mat the image
+ * \param[in] width the image width
+ * \param[in] height the image height
+ * \param[in] xf the x coordinate
+ * \param[in] yf the y coordinate
+ * \param[in] scale the scale
+ */
+ uint8_t
+ getValue (const std::vector<unsigned char>& mat,
+ int width, int height, float xf, float yf, float scale);
+
+ /** \brief Get the image used. */
+ const std::vector<unsigned char>&
+ getImage () const
+ {
+ return (img_);
+ }
+
+ /** \brief Get the width of the image used. */
+ int
+ getImageWidth () const
+ {
+ return (img_width_);
+ }
+
+ /** \brief Get the height of the image used. */
+ int
+ getImageHeight () const
+ {
+ return (img_height_);
+ }
+
+ /** \brief Get the scale used. */
+ float
+ getScale () const
+ {
+ return (scale_);
+ }
+
+ /** \brief Get the offset used. */
+ inline float
+ getOffset () const
+ {
+ return (offset_);
+ }
+
+ /** \brief Get the scores obtained. */
+ inline const std::vector<unsigned char>&
+ getScores () const
+ {
+ return (scores_);
+ }
+
+ private:
+ // half sampling
+ inline void
+ halfsample (const std::vector<unsigned char>& srcimg,
+ int srcwidth, int srcheight,
+ std::vector<unsigned char>& dstimg,
+ int dstwidth, int dstheight);
+
+ // two third sampling
+ inline void
+ twothirdsample (const std::vector<unsigned char>& srcimg,
+ int srcwidth, int srcheight,
+ std::vector<unsigned char>& dstimg,
+ int dstwidth, int dstheight);
+
+ /** the image */
+ std::vector<unsigned char> img_;
+ int img_width_;
+ int img_height_;
+
+ /** its Fast scores */
+ std::vector<unsigned char> scores_;
+
+ /** coordinate transformation */
+ float scale_;
+ float offset_;
+
+ /** agast */
+ boost::shared_ptr<pcl::keypoints::agast::OastDetector9_16> oast_detector_;
+ boost::shared_ptr<pcl::keypoints::agast::AgastDetector5_8> agast_detector_5_8_;
+ };
+
+ /** BRISK Scale Space helper. */
+ class PCL_EXPORTS ScaleSpace
+ {
+ public:
+ /** \brief Constructor. Specify the number of octaves.
+ * \param[in] octaves the number of octaves (default: 3)
+ */
+ ScaleSpace (int octaves = 3);
+ ~ScaleSpace ();
+
+ /** \brief Construct the image pyramids.
+ * \param[in] image the image to construct pyramids for
+ * \param[in] width the image width
+ * \param[in] height the image height
+ */
+ void
+ constructPyramid (const std::vector<unsigned char>& image,
+ int width, int height);
+
+ /** \brief Get the keypoints for the associated image and threshold.
+ * \param[in] threshold the threshold for the keypoints
+ * \param[out] keypoints the resultant list of keypoints
+ */
+ void
+ getKeypoints (const int threshold,
+ std::vector<pcl::PointWithScale, Eigen::aligned_allocator<pcl::PointWithScale> > &keypoints);
+
+ protected:
+ /** Nonmax suppression. */
+ inline bool
+ isMax2D (const uint8_t layer, const int x_layer, const int y_layer);
+
+ /** 1D (scale axis) refinement: around octave */
+ inline float
+ refine1D (const float s_05, const float s0, const float s05, float& max);
+
+ /** 1D (scale axis) refinement: around intra */
+ inline float
+ refine1D_1 (const float s_05, const float s0, const float s05, float& max);
+
+ /** 1D (scale axis) refinement: around octave 0 only */
+ inline float
+ refine1D_2 (const float s_05, const float s0, const float s05, float& max);
+
+ /** 2D maximum refinement */
+ inline float
+ subpixel2D (const int s_0_0, const int s_0_1, const int s_0_2,
+ const int s_1_0, const int s_1_1, const int s_1_2,
+ const int s_2_0, const int s_2_1, const int s_2_2,
+ float& delta_x, float& delta_y);
+
+ /** 3D maximum refinement centered around (x_layer,y_layer) */
+ inline float
+ refine3D (const uint8_t layer,
+ const int x_layer, const int y_layer,
+ float& x, float& y, float& scale, bool& ismax);
+
+ /** interpolated score access with recalculation when needed */
+ inline int
+ getScoreAbove (const uint8_t layer, const int x_layer, const int y_layer);
+
+ inline int
+ getScoreBelow (const uint8_t layer, const int x_layer, const int y_layer);
+
+ /** return the maximum of score patches above or below */
+ inline float
+ getScoreMaxAbove (const uint8_t layer,
+ const int x_layer, const int y_layer,
+ const int threshold, bool& ismax,
+ float& dx, float& dy);
+
+ inline float
+ getScoreMaxBelow (const uint8_t layer,
+ const int x_layer, const int y_layer,
+ const int threshold, bool& ismax,
+ float& dx, float& dy);
+
+ // the image pyramids
+ uint8_t layers_;
+ std::vector<pcl::keypoints::brisk::Layer> pyramid_;
+
+ // Agast
+ uint8_t threshold_;
+ uint8_t safe_threshold_;
+
+ // some constant parameters
+ float safety_factor_;
+ float basic_size_;
+ };
+ } // namespace brisk
+ } // namespace keypoints
+
+}
+
+#include <pcl/keypoints/impl/brisk_2d.hpp>
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (C) 2011, The Autonomous Systems Lab (ASL), ETH Zurich,
+ * Stefan Leutenegger, Simon Lynen and Margarita Chli.
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_KEYPOINTS_BRISK_KEYPOINT_2D_IMPL_H_
+#define PCL_KEYPOINTS_BRISK_KEYPOINT_2D_IMPL_H_
+
+#include <pcl/common/io.h>
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT, typename IntensityT> bool
+pcl::BriskKeypoint2D<PointInT, PointOutT, IntensityT>::initCompute ()
+{
+ if (!pcl::Keypoint<PointInT, PointOutT>::initCompute ())
+ {
+ PCL_ERROR ("[pcl::%s::initCompute] init failed.!\n", name_.c_str ());
+ return (false);
+ }
+
+ if (!input_->isOrganized ())
+ {
+ PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
+ return (false);
+ }
+
+ return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointInT, typename PointOutT, typename IntensityT> void
+pcl::BriskKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCloudOut &output)
+{
+ // image size
+ const int width = int (input_->width);
+ const int height = int (input_->height);
+
+ // destination for intensity data; will be forwarded to BRISK
+ std::vector<unsigned char> image_data (width*height);
+
+ for (size_t i = 0; i < image_data.size (); ++i)
+ image_data[i] = static_cast<unsigned char> (intensity_ ((*input_)[i]));
+
+ pcl::keypoints::brisk::ScaleSpace brisk_scale_space (octaves_);
+ brisk_scale_space.constructPyramid (image_data, width, height);
+ pcl::PointCloud<pcl::PointWithScale> output_temp;
+ brisk_scale_space.getKeypoints (threshold_, output_temp.points);
+ pcl::copyPointCloud<pcl::PointWithScale, PointOutT> (output_temp, output);
+
+ // we do not change the denseness
+ output.width = int (output.points.size ());
+ output.height = 1;
+ output.is_dense = false; // set to false to be sure
+
+ // 2nd pass to remove the invalid set of 3D keypoints
+ if (remove_invalid_3D_keypoints_)
+ {
+ PointCloudOut output_clean;
+ for (size_t i = 0; i < output.size (); ++i)
+ {
+ PointOutT pt;
+ // Interpolate its position in 3D, as the "u" and "v" are subpixel accurate
+ bilinearInterpolation (input_, output[i].x, output[i].y, pt);
+
+ // Check if the point is finite
+ if (pcl::isFinite (pt))
+ output_clean.push_back (output[i]);
+ }
+ output = output_clean;
+ output.is_dense = true; // set to true as there's no keypoint at an invalid XYZ
+ }
+}
+
+#endif
const int occupency_map_size (occupency_map.size ());
#ifdef _OPENMP
-#pragma omp parallel for shared (output, occupency_map) private (width, height) num_threads(threads_)
+#pragma omp parallel for shared (output, occupency_map) firstprivate (width, height) num_threads(threads_)
#endif
for (int i = 0; i < occupency_map_size; ++i)
{
}
}
+#ifdef _OPENMP
Eigen::Vector3d *omp_mem = new Eigen::Vector3d[threads_];
for (size_t i = 0; i < threads_; i++)
omp_mem[i].setZero (3);
+#else
+ Eigen::Vector3d *omp_mem = new Eigen::Vector3d[1];
+
+ omp_mem[0].setZero (3);
+#endif
double *prg_local_mem = new double[input_->size () * 3];
double **prg_mem = new double * [input_->size ()];
scale *= 2;
}
+ // Set final properties
output.height = 1;
output.width = static_cast<uint32_t> (output.points.size ());
+ output.header = input_->header;
+ output.sensor_origin_ = input_->sensor_origin_;
+ output.sensor_orientation_ = input_->sensor_orientation_;
}
#ifdef _OPENMP
#pragma omp parallel for shared (output) num_threads (threads_)
#endif
- for (int i = 0; i < indices.size (); ++i)
+ for (size_t i = 0; i < indices.size (); ++i)
{
int idx = indices[i];
if ((response_->points[idx] < second_threshold_) || occupency_map[idx])
#ifdef _OPENMP
#pragma omp parallel for shared (output) num_threads (threads_)
#endif
- for (int i = 0; i < indices.size (); ++i)
+ for (size_t i = 0; i < indices.size (); ++i)
{
int idx = indices[i];
if ((response_->points[idx] < second_threshold_) || occupency_map[idx])
+++ /dev/null
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2009, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * $Id$
- *
- */
-
-#ifndef PCL_KEYPOINTS_UNIFORM_SAMPLING_IMPL_H_
-#define PCL_KEYPOINTS_UNIFORM_SAMPLING_IMPL_H_
-
-#include <pcl/common/common.h>
-#include <pcl/keypoints/uniform_sampling.h>
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointInT> void
-pcl::UniformSampling<PointInT>::detectKeypoints (PointCloudOut &output)
-{
- // Has the input dataset been set already?
- if (!input_)
- {
- PCL_WARN ("[pcl::%s::detectKeypoints] No input dataset given!\n", getClassName ().c_str ());
- output.width = output.height = 0;
- output.points.clear ();
- return;
- }
-
- output.height = 1; // downsampling breaks the organized structure
- output.is_dense = true; // we filter out invalid points
-
- Eigen::Vector4f min_p, max_p;
- // Get the minimum and maximum dimensions
- pcl::getMinMax3D<PointInT>(*input_, min_p, max_p);
-
- // Compute the minimum and maximum bounding box values
- min_b_[0] = static_cast<int> (floor (min_p[0] * inverse_leaf_size_[0]));
- max_b_[0] = static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0]));
- min_b_[1] = static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1]));
- max_b_[1] = static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1]));
- min_b_[2] = static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2]));
- max_b_[2] = static_cast<int> (floor (max_p[2] * inverse_leaf_size_[2]));
-
- // Compute the number of divisions needed along all axis
- div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
- div_b_[3] = 0;
-
- // Clear the leaves
- leaves_.clear ();
-
- // Set up the division multiplier
- divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
-
- // First pass: build a set of leaves with the point index closest to the leaf center
- for (size_t cp = 0; cp < indices_->size (); ++cp)
- {
- if (!input_->is_dense)
- // Check if the point is invalid
- if (!pcl_isfinite (input_->points[(*indices_)[cp]].x) ||
- !pcl_isfinite (input_->points[(*indices_)[cp]].y) ||
- !pcl_isfinite (input_->points[(*indices_)[cp]].z))
- continue;
-
- Eigen::Vector4i ijk = Eigen::Vector4i::Zero ();
- ijk[0] = static_cast<int> (floor (input_->points[(*indices_)[cp]].x * inverse_leaf_size_[0]));
- ijk[1] = static_cast<int> (floor (input_->points[(*indices_)[cp]].y * inverse_leaf_size_[1]));
- ijk[2] = static_cast<int> (floor (input_->points[(*indices_)[cp]].z * inverse_leaf_size_[2]));
-
- // Compute the leaf index
- int idx = (ijk - min_b_).dot (divb_mul_);
- Leaf& leaf = leaves_[idx];
- // First time we initialize the index
- if (leaf.idx == -1)
- {
- leaf.idx = (*indices_)[cp];
- continue;
- }
-
- // Check to see if this point is closer to the leaf center than the previous one we saved
- float diff_cur = (input_->points[(*indices_)[cp]].getVector4fMap () - ijk.cast<float> ()).squaredNorm ();
- float diff_prev = (input_->points[leaf.idx].getVector4fMap () - ijk.cast<float> ()).squaredNorm ();
-
- // If current point is closer, copy its index instead
- if (diff_cur < diff_prev)
- leaf.idx = (*indices_)[cp];
- }
-
- // Second pass: go over all leaves and copy data
- output.points.resize (leaves_.size ());
- int cp = 0;
-
- for (typename boost::unordered_map<size_t, Leaf>::const_iterator it = leaves_.begin (); it != leaves_.end (); ++it)
- output.points[cp++] = it->second.idx;
- output.width = static_cast<uint32_t> (output.points.size ());
-}
-
-#define PCL_INSTANTIATE_UniformSampling(T) template class PCL_EXPORTS pcl::UniformSampling<T>;
-
-#endif // PCL_KEYPOINTS_UNIFORM_SAMPLING_IMPL_H_
-
/// \brief \return points normals as calculated or given
inline void
- getNormals (const NormalsConstPtr &normals) const { return (normals_); }
+ getNormals () const { return (normals_); }
/** \brief Initialize the scheduler and set the number of threads to use.
* \param nr_threads the number of hardware threads to use, 0 for automatic.
#ifndef PCL_KEYPOINTS_UNIFORM_SAMPLING_H_
#define PCL_KEYPOINTS_UNIFORM_SAMPLING_H_
-#include <pcl/keypoints/keypoint.h>
-#include <boost/unordered_map.hpp>
-
-namespace pcl
-{
- /** \brief @b UniformSampling assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
- *
- * The @b UniformSampling class creates a *3D voxel grid* (think about a voxel
- * grid as a set of tiny 3D boxes in space) over the input point cloud data.
- * Then, in each *voxel* (i.e., 3D box), all the points present will be
- * approximated (i.e., *downsampled*) with their centroid. This approach is
- * a bit slower than approximating them with the center of the voxel, but it
- * represents the underlying surface more accurately.
- *
- * \author Radu Bogdan Rusu
- * \ingroup keypoints
- */
- template <typename PointInT>
- class UniformSampling: public Keypoint<PointInT, int>
- {
- typedef typename Keypoint<PointInT, int>::PointCloudIn PointCloudIn;
- typedef typename Keypoint<PointInT, int>::PointCloudOut PointCloudOut;
-
- using Keypoint<PointInT, int>::name_;
- using Keypoint<PointInT, int>::input_;
- using Keypoint<PointInT, int>::indices_;
- using Keypoint<PointInT, int>::search_radius_;
- using Keypoint<PointInT, int>::getClassName;
-
- public:
- typedef boost::shared_ptr<UniformSampling<PointInT> > Ptr;
- typedef boost::shared_ptr<const UniformSampling<PointInT> > ConstPtr;
-
- /** \brief Empty constructor. */
- UniformSampling () :
- leaves_ (),
- leaf_size_ (Eigen::Vector4f::Zero ()),
- inverse_leaf_size_ (Eigen::Vector4f::Zero ()),
- min_b_ (Eigen::Vector4i::Zero ()),
- max_b_ (Eigen::Vector4i::Zero ()),
- div_b_ (Eigen::Vector4i::Zero ()),
- divb_mul_ (Eigen::Vector4i::Zero ())
- {
- name_ = "UniformSampling";
- }
-
- /** \brief Destructor. */
- virtual ~UniformSampling ()
- {
- leaves_.clear();
- }
-
- /** \brief Set the 3D grid leaf size.
- * \param radius the 3D grid leaf size
- */
- virtual inline void
- setRadiusSearch (double radius)
- {
- leaf_size_[0] = leaf_size_[1] = leaf_size_[2] = static_cast<float> (radius);
- // Avoid division errors
- if (leaf_size_[3] == 0)
- leaf_size_[3] = 1;
- // Use multiplications instead of divisions
- inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
- search_radius_ = radius;
- }
-
- protected:
- /** \brief Simple structure to hold an nD centroid and the number of points in a leaf. */
- struct Leaf
- {
- Leaf () : idx (-1) { }
- int idx;
- };
-
- /** \brief The 3D grid leaves. */
- boost::unordered_map<size_t, Leaf> leaves_;
-
- /** \brief The size of a leaf. */
- Eigen::Vector4f leaf_size_;
-
- /** \brief Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons. */
- Eigen::Array4f inverse_leaf_size_;
-
- /** \brief The minimum and maximum bin coordinates, the number of divisions, and the division multiplier. */
- Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_;
-
- /** \brief Downsample a Point Cloud using a voxelized grid approach
- * \param output the resultant point cloud message
- */
- void
- detectKeypoints (PointCloudOut &output);
- };
-}
-
-#ifdef PCL_NO_PRECOMPILE
-#include <pcl/keypoints/impl/uniform_sampling.hpp>
+#ifdef __DEPRECATED
+#warning UniformSampling is not a Keypoint anymore, use <pcl/filters/uniform_sampling.h> instead.
#endif
+#include <pcl/filters/uniform_sampling.h>
+
#endif //#ifndef PCL_KEYPOINTS_UNIFORM_SAMPLING_H_
}
}
- curr_corner++;
+ ++curr_corner;
}
// collecting maximum corners
int total = 0;
int n_expected_corners = int (corners.capacity ());
pcl::PointUV h;
- register int x, y;
- register int width_b = img_width - 3; //2, +1 due to faster test x>width_b
- register int height_b = img_height - 2;
- register int_fast16_t offset0, offset1, offset2, offset3, offset4, offset5, offset6, offset7, offset8, offset9, offset10, offset11;
- register int width;
+ int x, y;
+ int width_b = img_width - 3; //2, +1 due to faster test x>width_b
+ int height_b = img_height - 2;
+ int_fast16_t offset0, offset1, offset2, offset3, offset4, offset5, offset6, offset7, offset8, offset9, offset10, offset11;
+ int width;
corners.resize (0);
break;
else
{
- register const T1* const p = im + y * width + x;
- register const T2 cb = *p + T2 (threshold);
- register const T2 c_b = *p - T2 (threshold);
+ const T1* const p = im + y * width + x;
+ const T2 cb = *p + T2 (threshold);
+ const T2 c_b = *p - T2 (threshold);
if (p[offset0] > cb)
if (p[offset2] > cb)
if (p[offset5] > cb)
break;
else
{
- register const T1* const p = im + y * width + x;
- register const T2 cb = *p + T2 (threshold);
- register const T2 c_b = *p - T2 (threshold);
+ const T1* const p = im + y * width + x;
+ const T2 cb = *p + T2 (threshold);
+ const T2 c_b = *p - T2 (threshold);
if (p[offset0] > cb)
if (p[offset2] > cb)
if (p[offset5] > cb)
T2 bmax = T2 (im_bmax); // 255;
int b_test = int ((bmax + bmin) / 2);
- register int_fast16_t offset0 = s_offset0;
- register int_fast16_t offset1 = s_offset1;
- register int_fast16_t offset2 = s_offset2;
- register int_fast16_t offset3 = s_offset3;
- register int_fast16_t offset4 = s_offset4;
- register int_fast16_t offset5 = s_offset5;
- register int_fast16_t offset6 = s_offset6;
- register int_fast16_t offset7 = s_offset7;
- register int_fast16_t offset8 = s_offset8;
- register int_fast16_t offset9 = s_offset9;
- register int_fast16_t offset10 = s_offset10;
- register int_fast16_t offset11 = s_offset11;
+ int_fast16_t offset0 = s_offset0;
+ int_fast16_t offset1 = s_offset1;
+ int_fast16_t offset2 = s_offset2;
+ int_fast16_t offset3 = s_offset3;
+ int_fast16_t offset4 = s_offset4;
+ int_fast16_t offset5 = s_offset5;
+ int_fast16_t offset6 = s_offset6;
+ int_fast16_t offset7 = s_offset7;
+ int_fast16_t offset8 = s_offset8;
+ int_fast16_t offset9 = s_offset9;
+ int_fast16_t offset10 = s_offset10;
+ int_fast16_t offset11 = s_offset11;
while (1)
{
- register const T2 cb = *p + T2 (b_test);
- register const T2 c_b = *p - T2 (b_test);
+ const T2 cb = *p + T2 (b_test);
+ const T2 c_b = *p - T2 (b_test);
if (p[offset0] > cb)
if (p[offset5] > cb)
if (p[offset2] < c_b)
int total = 0;
int n_expected_corners = int (corners.capacity ());
pcl::PointUV h;
- register int x, y;
- register int xsize_b = int (img_width) - 2;
- register int ysize_b = int (img_height) - 1;
- register int_fast16_t offset0, offset1, offset2, offset3, offset4, offset5, offset6, offset7;
- register int width;
+ int x, y;
+ int xsize_b = int (img_width) - 2;
+ int ysize_b = int (img_height) - 1;
+ int_fast16_t offset0, offset1, offset2, offset3, offset4, offset5, offset6, offset7;
+ int width;
corners.resize (0);
break;
else
{
- register const T1* const p = im + y * width + x;
- register const T2 cb = *p + T2 (threshold);
- register const T2 c_b = *p - T2 (threshold);
+ const T1* const p = im + y * width + x;
+ const T2 cb = *p + T2 (threshold);
+ const T2 c_b = *p - T2 (threshold);
if (p[offset0] > cb)
if (p[offset2] > cb)
if (p[offset3] > cb)
break;
else
{
- register const T1* const p = im + y * width + x;
- register const T2 cb = *p + T2 (threshold);
- register const T2 c_b = *p - T2 (threshold);
+ const T1* const p = im + y * width + x;
+ const T2 cb = *p + T2 (threshold);
+ const T2 c_b = *p - T2 (threshold);
if (p[offset0] > cb)
if (p[offset2] > cb)
if (p[offset3] > cb)
T2 bmax = T2 (im_bmax);
int b_test = int ((bmax + bmin) / 2);
- register int_fast16_t offset0 = s_offset0;
- register int_fast16_t offset1 = s_offset1;
- register int_fast16_t offset2 = s_offset2;
- register int_fast16_t offset3 = s_offset3;
- register int_fast16_t offset4 = s_offset4;
- register int_fast16_t offset5 = s_offset5;
- register int_fast16_t offset6 = s_offset6;
- register int_fast16_t offset7 = s_offset7;
+ int_fast16_t offset0 = s_offset0;
+ int_fast16_t offset1 = s_offset1;
+ int_fast16_t offset2 = s_offset2;
+ int_fast16_t offset3 = s_offset3;
+ int_fast16_t offset4 = s_offset4;
+ int_fast16_t offset5 = s_offset5;
+ int_fast16_t offset6 = s_offset6;
+ int_fast16_t offset7 = s_offset7;
while (1)
{
- register const T2 cb = *p + T2 (b_test);
- register const T2 c_b = *p - T2 (b_test);
+ const T2 cb = *p + T2 (b_test);
+ const T2 c_b = *p - T2 (b_test);
if (p[offset0] > cb)
if (p[offset2] > cb)
if (p[offset3] > cb)
int total = 0;
int n_expected_corners = int (corners.capacity ());
pcl::PointUV h;
- register int x, y;
- register int xsize_b = int (img_width) - 4;
- register int ysize_b = int (img_height) - 3;
- register int_fast16_t offset0, offset1, offset2, offset3, offset4, offset5, offset6, offset7, offset8, offset9, offset10, offset11, offset12, offset13, offset14, offset15;
- register int width;
+ int x, y;
+ int xsize_b = int (img_width) - 4;
+ int ysize_b = int (img_height) - 3;
+ int_fast16_t offset0, offset1, offset2, offset3, offset4, offset5, offset6, offset7, offset8, offset9, offset10, offset11, offset12, offset13, offset14, offset15;
+ int width;
corners.resize (0);
break;
else
{
- register const T1* const p = im + y * width + x;
- register const T2 cb = *p + T2 (threshold);
- register const T2 c_b = *p - T2 (threshold);
+ const T1* const p = im + y * width + x;
+ const T2 cb = *p + T2 (threshold);
+ const T2 c_b = *p - T2 (threshold);
if (p[offset0] > cb)
if (p[offset2] > cb)
if (p[offset4] > cb)
T2 bmax = T2 (im_bmax);
int b_test = int ((bmax + bmin) / 2);
- register int_fast16_t offset0 = s_offset0;
- register int_fast16_t offset1 = s_offset1;
- register int_fast16_t offset2 = s_offset2;
- register int_fast16_t offset3 = s_offset3;
- register int_fast16_t offset4 = s_offset4;
- register int_fast16_t offset5 = s_offset5;
- register int_fast16_t offset6 = s_offset6;
- register int_fast16_t offset7 = s_offset7;
- register int_fast16_t offset8 = s_offset8;
- register int_fast16_t offset9 = s_offset9;
- register int_fast16_t offset10 = s_offset10;
- register int_fast16_t offset11 = s_offset11;
- register int_fast16_t offset12 = s_offset12;
- register int_fast16_t offset13 = s_offset13;
- register int_fast16_t offset14 = s_offset14;
- register int_fast16_t offset15 = s_offset15;
+ int_fast16_t offset0 = s_offset0;
+ int_fast16_t offset1 = s_offset1;
+ int_fast16_t offset2 = s_offset2;
+ int_fast16_t offset3 = s_offset3;
+ int_fast16_t offset4 = s_offset4;
+ int_fast16_t offset5 = s_offset5;
+ int_fast16_t offset6 = s_offset6;
+ int_fast16_t offset7 = s_offset7;
+ int_fast16_t offset8 = s_offset8;
+ int_fast16_t offset9 = s_offset9;
+ int_fast16_t offset10 = s_offset10;
+ int_fast16_t offset11 = s_offset11;
+ int_fast16_t offset12 = s_offset12;
+ int_fast16_t offset13 = s_offset13;
+ int_fast16_t offset14 = s_offset14;
+ int_fast16_t offset15 = s_offset15;
while (1)
{
- register const T2 cb = *p + T2 (b_test);
- register const T2 c_b = *p - T2 (b_test);
+ const T2 cb = *p + T2 (b_test);
+ const T2 c_b = *p - T2 (b_test);
if (p[offset0] > cb)
if (p[offset2] > cb)
if (p[offset4] > cb)
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (C) 2011, The Autonomous Systems Lab (ASL), ETH Zurich,
+ * Stefan Leutenegger, Simon Lynen and Margarita Chli.
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and threshold_inary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in threshold_inary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may threshold_e used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/pcl_config.h>
+#include <pcl/pcl_macros.h>
+#include <pcl/keypoints/brisk_2d.h>
+#include <pcl/point_types.h>
+#include <pcl/impl/instantiate.hpp>
+#if defined(__SSSE3__) && !defined(__i386__)
+#include <tmmintrin.h>
+#include <emmintrin.h>
+#endif
+
+/////////////////////////////////////////////////////////////////////////////////////////
+// construct telling the octaves number:
+pcl::keypoints::brisk::ScaleSpace::ScaleSpace (int octaves)
+ : safety_factor_ (1.0)
+ , basic_size_ (12.0)
+{
+ if (octaves == 0)
+ layers_ = 1;
+ else
+ layers_ = uint8_t (2 * octaves);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+pcl::keypoints::brisk::ScaleSpace::~ScaleSpace ()
+{
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+// construct the image pyramids
+void
+pcl::keypoints::brisk::ScaleSpace::constructPyramid (
+ const std::vector<unsigned char>& image, int width, int height)
+{
+ // set correct size:
+ pyramid_.clear ();
+
+ // fill the pyramid
+ pyramid_.push_back (pcl::keypoints::brisk::Layer (std::vector<unsigned char> (image), width, height));
+ if (layers_ > 1)
+ pyramid_.push_back (pcl::keypoints::brisk::Layer (pyramid_.back (), pcl::keypoints::brisk::Layer::CommonParams::TWOTHIRDSAMPLE));
+ const int octaves2 = layers_;
+
+ for (int i = 2; i < octaves2; i += 2)
+ {
+ pyramid_.push_back (pcl::keypoints::brisk::Layer (pyramid_[i-2], pcl::keypoints::brisk::Layer::CommonParams::HALFSAMPLE));
+ pyramid_.push_back (pcl::keypoints::brisk::Layer (pyramid_[i-1], pcl::keypoints::brisk::Layer::CommonParams::HALFSAMPLE));
+ }
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::keypoints::brisk::ScaleSpace::getKeypoints (
+ const int threshold,
+ std::vector<pcl::PointWithScale, Eigen::aligned_allocator<pcl::PointWithScale> >& keypoints)
+{
+ // make sure keypoints is empty
+ //keypoints.resize (0);
+ keypoints.clear ();
+ keypoints.reserve (2000);
+
+ // assign thresholds
+ threshold_ = uint8_t (threshold);
+ safe_threshold_ = uint8_t (threshold_ * safety_factor_);
+ std::vector<std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > > agast_points;
+ agast_points.resize (layers_);
+
+ // go through the octaves and intra layers and calculate fast corner scores:
+ for (uint8_t i = 0; i < layers_; i++)
+ {
+ // call OAST16_9 without nms
+ pcl::keypoints::brisk::Layer& l = pyramid_[i];
+ l.getAgastPoints (safe_threshold_, agast_points[i]);
+ }
+
+ if (layers_ == 1)
+ {
+ // just do a simple 2d subpixel refinement...
+ const int num = int (agast_points[0].size ());
+ for (int n = 0; n < num; n++)
+ {
+ const pcl::PointUV& point = agast_points.at (0)[n];
+ // first check if it is a maximum:
+ if (!isMax2D (0, int (point.u), int (point.v)))
+ continue;
+
+ // let's do the subpixel and float scale refinement:
+ pcl::keypoints::brisk::Layer& l = pyramid_[0];
+ int s_0_0 = l.getAgastScore (point.u-1, point.v-1, 1);
+ int s_1_0 = l.getAgastScore (point.u, point.v-1, 1);
+ int s_2_0 = l.getAgastScore (point.u+1, point.v-1, 1);
+ int s_2_1 = l.getAgastScore (point.u+1, point.v, 1);
+ int s_1_1 = l.getAgastScore (point.u, point.v, 1);
+ int s_0_1 = l.getAgastScore (point.u-1, point.v, 1);
+ int s_0_2 = l.getAgastScore (point.u-1, point.v+1, 1);
+ int s_1_2 = l.getAgastScore (point.u, point.v+1, 1);
+ int s_2_2 = l.getAgastScore (point.u+1, point.v+1, 1);
+ float delta_x, delta_y;
+ float max = subpixel2D (s_0_0, s_0_1, s_0_2,
+ s_1_0, s_1_1, s_1_2,
+ s_2_0, s_2_1, s_2_2,
+ delta_x, delta_y);
+
+ // store:
+ keypoints.push_back (pcl::PointWithScale (point.u + delta_x, point.v + delta_y, 0.0f, basic_size_, -1, max, 0));
+ }
+ return;
+ }
+
+ float x, y, scale, score;
+ for (uint8_t i = 0; i < layers_; i++)
+ {
+ pcl::keypoints::brisk::Layer& l = pyramid_[i];
+ const int num = int (agast_points[i].size ());
+
+ if (i == layers_ - 1)
+ {
+ for (int n = 0; n < num; n++)
+ {
+ const pcl::PointUV& point = agast_points.at (i)[n];
+
+ // consider only 2D maxima...
+ if (!isMax2D (i, int (point.u), int (point.v)))
+ continue;
+
+ bool ismax;
+ float dx, dy;
+ getScoreMaxBelow (i, int (point.u), int (point.v),
+ l.getAgastScore (point.u, point.v, safe_threshold_), ismax,
+ dx, dy);
+ if (!ismax)
+ continue;
+
+ // get the patch on this layer:
+ int s_0_0 = l.getAgastScore (point.u-1, point.v-1, 1);
+ int s_1_0 = l.getAgastScore (point.u, point.v-1, 1);
+ int s_2_0 = l.getAgastScore (point.u+1, point.v-1, 1);
+ int s_2_1 = l.getAgastScore (point.u+1, point.v, 1);
+ int s_1_1 = l.getAgastScore (point.u, point.v, 1);
+ int s_0_1 = l.getAgastScore (point.u-1, point.v, 1);
+ int s_0_2 = l.getAgastScore (point.u-1, point.v+1, 1);
+ int s_1_2 = l.getAgastScore (point.u, point.v+1, 1);
+ int s_2_2 = l.getAgastScore (point.u+1, point.v+1, 1);
+ float delta_x, delta_y;
+ float max = subpixel2D (s_0_0, s_0_1, s_0_2,
+ s_1_0, s_1_1, s_1_2,
+ s_2_0, s_2_1, s_2_2,
+ delta_x, delta_y);
+
+ // store:
+ keypoints.push_back (pcl::PointWithScale ((point.u + delta_x) * l.getScale () + l.getOffset (), // x
+ (point.v + delta_y) * l.getScale () + l.getOffset (), // y
+ 0.0f, // z
+ basic_size_ * l.getScale (), // size
+ -1, // angle
+ max, // response
+ i)); // octave
+ }
+ }
+ else
+ {
+ // not the last layer:
+ for (int n = 0; n < num; n++)
+ {
+ const pcl::PointUV& point = agast_points.at (i)[n];
+
+ // first check if it is a maximum:
+ if (!isMax2D (i, int (point.u), int (point.v)))
+ {
+ continue;
+ }
+
+ // let's do the subpixel and float scale refinement:
+ bool ismax;
+ score = refine3D (i, int (point.u), int (point.v), x, y, scale, ismax);
+
+ if (!ismax)
+ continue;
+
+ // finally store the detected keypoint:
+ if (score > float (threshold_))
+ {
+ keypoints.push_back (pcl::PointWithScale (x, y, 0.0f, basic_size_ * scale, -1, score, i));
+ }
+ }
+ }
+ }
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+// interpolated score access with recalculation when needed:
+int
+pcl::keypoints::brisk::ScaleSpace::getScoreAbove (
+ const uint8_t layer, const int x_layer, const int y_layer)
+{
+ assert (layer < layers_ - 1);
+ pcl::keypoints::brisk::Layer& l = pyramid_[layer+1];
+ if (layer % 2 == 0)
+ { // octave
+ const int sixths_x = 4 * x_layer - 1;
+ const int x_above = sixths_x / 6;
+ const int sixths_y = 4 * y_layer - 1;
+ const int y_above = sixths_y / 6;
+ const int r_x = (sixths_x % 6);
+ const int r_x_1 =6 - r_x;
+ const int r_y = (sixths_y % 6);
+ const int r_y_1 = 6 - r_y;
+ uint8_t score = static_cast<uint8_t> (
+ 0xFF & ((r_x_1 * r_y_1 * l.getAgastScore (x_above, y_above, 1) +
+ r_x * r_y_1 * l.getAgastScore (x_above + 1, y_above, 1) +
+ r_x_1 * r_y * l.getAgastScore (x_above, y_above + 1, 1) +
+ r_x * r_y * l.getAgastScore (x_above + 1, y_above + 1, 1) + 18) / 36));
+
+ return (score);
+ }
+ else
+ { // intra
+ const int eighths_x = 6 * x_layer - 1;
+ const int x_above = eighths_x / 8;
+ const int eighths_y = 6 * y_layer - 1;
+ const int y_above = eighths_y / 8;
+ const int r_x = (eighths_x % 8);
+ const int r_x_1 = 8 - r_x;
+ const int r_y = (eighths_y % 8);
+ const int r_y_1 = 8 - r_y;
+ uint8_t score = static_cast<uint8_t> (
+ 0xFF & ((r_x_1 * r_y_1 * l.getAgastScore (x_above, y_above, 1) +
+ r_x * r_y_1 * l.getAgastScore (x_above + 1, y_above, 1) +
+ r_x_1 * r_y * l.getAgastScore (x_above , y_above + 1, 1) +
+ r_x * r_y * l.getAgastScore (x_above + 1, y_above + 1, 1) + 32) / 64));
+ return (score);
+ }
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+int
+pcl::keypoints::brisk::ScaleSpace::getScoreBelow (
+ const uint8_t layer, const int x_layer, const int y_layer)
+{
+ assert (layer);
+ pcl::keypoints::brisk::Layer& l = pyramid_[layer-1];
+ int sixth_x;
+ int quarter_x;
+ float xf;
+ int sixth_y;
+ int quarter_y;
+ float yf;
+
+ // scaling:
+ float offs;
+ float area;
+ int scaling;
+ int scaling2;
+
+ if (layer % 2 == 0)
+ { // octave
+ sixth_x = 8 * x_layer + 1;
+ xf = float (sixth_x) / 6.0f;
+ sixth_y = 8 * y_layer + 1;
+ yf = float (sixth_y) / 6.0f;
+
+ // scaling:
+ offs = 2.0f / 3.0f;
+ area = 4.0f * offs * offs;
+ scaling = static_cast<int> (4194304.0f / area);
+ scaling2 = static_cast<int> (float (scaling) * area);
+ }
+ else
+ {
+ quarter_x = 6 * x_layer + 1;
+ xf = float (quarter_x) / 4.0f;
+ quarter_y = 6 * y_layer + 1;
+ yf = float (quarter_y) / 4.0f;
+
+ // scaling:
+ offs = 3.0f / 4.0f;
+ area = 4.0f * offs * offs;
+ scaling = static_cast<int> (4194304.0f / area);
+ scaling2 = static_cast<int> (float (scaling) * area);
+ }
+
+ // calculate borders
+ const float x_1 = xf - offs;
+ const float x1 = xf + offs;
+ const float y_1 = yf - offs;
+ const float y1 = yf + offs;
+
+ const int x_left = int (x_1 + 0.5);
+ const int y_top = int (y_1 + 0.5);
+ const int x_right = int (x1 + 0.5);
+ const int y_bottom = int (y1 + 0.5);
+
+ // overlap area - multiplication factors:
+ const float r_x_1 = float (x_left) - x_1 + 0.5f;
+ const float r_y_1 = float (y_top) - y_1 + 0.5f;
+ const float r_x1 = x1 - float (x_right) + 0.5f;
+ const float r_y1 = y1 - float (y_bottom) + 0.5f;
+ const int dx = x_right - x_left - 1;
+ const int dy = y_bottom - y_top - 1;
+ const int A = static_cast<int> ((r_x_1 * r_y_1) * float (scaling));
+ const int B = static_cast<int> ((r_x1 * r_y_1) * float (scaling));
+ const int C = static_cast<int> ((r_x1 * r_y1) * float (scaling));
+ const int D = static_cast<int> ((r_x_1 * r_y1) * float (scaling));
+ const int r_x_1_i = static_cast<int> (r_x_1 * float (scaling));
+ const int r_y_1_i = static_cast<int> (r_y_1 * float (scaling));
+ const int r_x1_i = static_cast<int> (r_x1 * float (scaling));
+ const int r_y1_i = static_cast<int> (r_y1 * float (scaling));
+
+ // first row:
+ int ret_val = A * int (l.getAgastScore (x_left, y_top, 1));
+ for (int X = 1; X <= dx; X++)
+ ret_val += r_y_1_i * int (l.getAgastScore (x_left + X, y_top, 1));
+
+ ret_val += B * int (l.getAgastScore (x_left + dx + 1, y_top, 1));
+ // middle ones:
+ for (int Y = 1; Y <= dy; Y++)
+ {
+ ret_val += r_x_1_i * int (l.getAgastScore (x_left, y_top + Y, 1));
+
+ for (int X = 1; X <= dx; X++)
+ ret_val += int (l.getAgastScore (x_left + X, y_top + Y, 1)) * scaling;
+
+ ret_val += r_x1_i * int (l.getAgastScore (x_left + dx + 1, y_top + Y, 1));
+ }
+ // last row:
+ ret_val += D * int (l.getAgastScore (x_left, y_top + dy + 1, 1));
+ for (int X = 1; X <= dx; X++)
+ ret_val += r_y1_i * int (l.getAgastScore (x_left + X, y_top + dy + 1, 1));
+
+ ret_val += C * int (l.getAgastScore (x_left + dx + 1, y_top + dy + 1, 1));
+
+ return ((ret_val + scaling2 / 2) / scaling2);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::keypoints::brisk::ScaleSpace::isMax2D (
+ const uint8_t layer, const int x_layer, const int y_layer)
+{
+ const std::vector<unsigned char>& scores = pyramid_[layer].getScores ();
+ const int scorescols = pyramid_[layer].getImageWidth ();
+ const unsigned char* data = &scores[0] + y_layer * scorescols + x_layer;
+
+ // decision tree:
+ const unsigned char center = (*data);
+ data--;
+ const unsigned char s_10 = *data;
+ if (center < s_10) return (false);
+ data += 2;
+ const unsigned char s10 = *data;
+ if (center < s10) return (false);
+ data -= (scorescols + 1);
+ const unsigned char s0_1 = *data;
+ if (center < s0_1) return (false);
+ data += 2 * scorescols;
+ const unsigned char s01 = *data;
+ if (center < s01) return (false);
+ data--;
+ const unsigned char s_11 =* data;
+ if (center < s_11) return (false);
+ data += 2;
+ const unsigned char s11 =* data;
+ if (center < s11) return (false);
+ data -= 2 * scorescols;
+ const unsigned char s1_1 =* data;
+ if (center < s1_1) return (false);
+ data -= 2;
+ const unsigned char s_1_1 =* data;
+ if (center < s_1_1) return (false);
+
+ // reject neighbor maxima
+ std::vector<int> delta;
+ // put together a list of 2d-offsets to where the maximum is also reached
+ if (center == s_1_1)
+ {
+ delta.push_back (-1);
+ delta.push_back (-1);
+ }
+ if (center == s0_1)
+ {
+ delta.push_back (0);
+ delta.push_back (-1);
+ }
+ if (center == s1_1)
+ {
+ delta.push_back (1);
+ delta.push_back (-1);
+ }
+ if (center == s_10)
+ {
+ delta.push_back (-1);
+ delta.push_back (0);
+ }
+ if (center == s10)
+ {
+ delta.push_back (1);
+ delta.push_back (0);
+ }
+ if (center == s_11)
+ {
+ delta.push_back (-1);
+ delta.push_back (1);
+ }
+ if (center == s01)
+ {
+ delta.push_back (0);
+ delta.push_back (1);
+ }
+ if (center == s11)
+ {
+ delta.push_back (1);
+ delta.push_back (1);
+ }
+
+ unsigned int deltasize = static_cast<unsigned int> (delta.size ());
+ if (deltasize != 0)
+ {
+ // in this case, we have to analyze the situation more carefully:
+ // the values are gaussian blurred and then we really decide
+ data = &scores[0] + y_layer * scorescols + x_layer;
+ int smoothedcenter = 4 * center + 2 * (s_10 + s10 + s0_1 + s01) + s_1_1 + s1_1 + s_11 + s11;
+
+ for (unsigned int i = 0; i < deltasize; i+= 2)
+ {
+ data = &scores[0] + (y_layer - 1 + delta[i+1]) * scorescols + x_layer + delta[i] - 1;
+
+ int othercenter = *data;
+ data++;
+ othercenter += 2 * (*data);
+ data++;
+ othercenter += *data;
+ data += scorescols;
+ othercenter += 2 * (*data);
+ data--;
+ othercenter += 4 * (*data);
+ data--;
+ othercenter += 2 * (*data);
+ data += scorescols;
+ othercenter += *data;
+ data++;
+ othercenter += 2 * (*data);
+ data++;
+ othercenter += *data;
+ if (othercenter > smoothedcenter) return (false);
+ }
+ }
+ return (true);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+// 3D maximum refinement centered around (x_layer,y_layer)
+float
+pcl::keypoints::brisk::ScaleSpace::refine3D (
+ const uint8_t layer, const int x_layer, const int y_layer,
+ float& x, float& y, float& scale, bool& ismax)
+{
+ ismax = true;
+ pcl::keypoints::brisk::Layer& this_layer = pyramid_[layer];
+ const int center = this_layer.getAgastScore (x_layer, y_layer, 1);
+
+ // check and get above maximum:
+ float delta_x_above = 0, delta_y_above = 0;
+ float max_above = getScoreMaxAbove (layer,x_layer, y_layer,
+ center, ismax,
+ delta_x_above, delta_y_above);
+
+ if (!ismax) return (0.0);
+
+ float max; // to be returned
+
+ if (layer % 2 == 0)
+ { // on octave
+ // treat the patch below:
+ float delta_x_below, delta_y_below;
+ float max_below_float;
+ unsigned char max_below_uchar = 0;
+ if (layer == 0)
+ {
+ // guess the lower intra octave...
+ pcl::keypoints::brisk::Layer& l = pyramid_[0];
+ int s_0_0 = l.getAgastScore_5_8 (x_layer - 1, y_layer - 1, 1);
+ max_below_uchar = static_cast<unsigned char> (s_0_0);
+ int s_1_0 = l.getAgastScore_5_8 (x_layer, y_layer - 1, 1);
+
+ if (s_1_0 > max_below_uchar) max_below_uchar = static_cast<unsigned char> (s_1_0);
+ int s_2_0 = l.getAgastScore_5_8 (x_layer + 1, y_layer - 1, 1);
+ if (s_2_0 > max_below_uchar) max_below_uchar = static_cast<unsigned char> (s_2_0);
+ int s_2_1 = l.getAgastScore_5_8 (x_layer + 1, y_layer, 1);
+ if (s_2_1 > max_below_uchar) max_below_uchar = static_cast<unsigned char> (s_2_1);
+ int s_1_1 = l.getAgastScore_5_8 (x_layer, y_layer, 1);
+ if (s_1_1 > max_below_uchar) max_below_uchar = static_cast<unsigned char> (s_1_1);
+ int s_0_1 = l.getAgastScore_5_8 (x_layer - 1, y_layer, 1);
+ if (s_0_1 > max_below_uchar) max_below_uchar = static_cast<unsigned char> (s_0_1);
+ int s_0_2 = l.getAgastScore_5_8 (x_layer - 1, y_layer + 1, 1);
+ if (s_0_2 > max_below_uchar) max_below_uchar = static_cast<unsigned char> (s_0_2);
+ int s_1_2 = l.getAgastScore_5_8 (x_layer, y_layer + 1, 1);
+ if (s_1_2 > max_below_uchar) max_below_uchar = static_cast<unsigned char> (s_1_2);
+ int s_2_2 = l.getAgastScore_5_8 (x_layer + 1, y_layer +1 , 1);
+ if (s_2_2 > max_below_uchar) max_below_uchar = static_cast<unsigned char> (s_2_2);
+
+ max_below_float = subpixel2D (s_0_0, s_0_1, s_0_2,
+ s_1_0, s_1_1, s_1_2,
+ s_2_0, s_2_1, s_2_2,
+ delta_x_below, delta_y_below);
+ max_below_float = max_below_uchar;
+ }
+ else
+ {
+ max_below_float = getScoreMaxBelow (layer,x_layer, y_layer,
+ center, ismax,
+ delta_x_below, delta_y_below);
+ if (!ismax) return (0);
+ }
+
+ // get the patch on this layer:
+ int s_0_0 = this_layer.getAgastScore (x_layer - 1, y_layer - 1, 1);
+ int s_1_0 = this_layer.getAgastScore (x_layer, y_layer - 1, 1);
+ int s_2_0 = this_layer.getAgastScore (x_layer + 1, y_layer - 1, 1);
+ int s_2_1 = this_layer.getAgastScore (x_layer + 1, y_layer, 1);
+ int s_1_1 = this_layer.getAgastScore (x_layer, y_layer, 1);
+ int s_0_1 = this_layer.getAgastScore (x_layer - 1, y_layer, 1);
+ int s_0_2 = this_layer.getAgastScore (x_layer - 1, y_layer + 1, 1);
+ int s_1_2 = this_layer.getAgastScore (x_layer, y_layer + 1, 1);
+ int s_2_2 = this_layer.getAgastScore (x_layer + 1, y_layer + 1, 1);
+ float delta_x_layer, delta_y_layer;
+ float max_layer = subpixel2D (s_0_0, s_0_1, s_0_2,
+ s_1_0, s_1_1, s_1_2,
+ s_2_0, s_2_1, s_2_2,
+ delta_x_layer, delta_y_layer);
+
+ // calculate the relative scale (1D maximum):
+ if (layer == 0)
+ scale = refine1D_2 (max_below_float, std::max (float (center), max_layer), max_above,max);
+ else
+ scale = refine1D (max_below_float, std::max (float (center), max_layer), max_above,max);
+
+ if (scale > 1.0)
+ {
+ // interpolate the position:
+ const float r0 = (1.5f - scale) / .5f;
+ const float r1 = 1.0f - r0;
+ x = (r0 * delta_x_layer + r1 * delta_x_above + float (x_layer))
+ * this_layer.getScale () + this_layer.getOffset ();
+ y = (r0 * delta_y_layer + r1 * delta_y_above + float (y_layer))
+ * this_layer.getScale () + this_layer.getOffset ();
+ }
+ else
+ {
+ if (layer == 0)
+ {
+ // interpolate the position:
+ const float r0 = (scale - 0.5f) / 0.5f;
+ const float r_1 = 1.0f - r0;
+ x = r0 * delta_x_layer + r_1 * delta_x_below + float (x_layer);
+ y = r0 * delta_y_layer + r_1 * delta_y_below + float (y_layer);
+ }
+ else
+ {
+ // interpolate the position:
+ const float r0 = (scale - 0.75f) / 0.25f;
+ const float r_1 = 1.0f -r0;
+ x = (r0 * delta_x_layer + r_1 * delta_x_below + float (x_layer))
+ * this_layer.getScale () +this_layer.getOffset ();
+ y = (r0 * delta_y_layer + r_1 * delta_y_below + float (y_layer))
+ * this_layer.getScale () + this_layer.getOffset ();
+ }
+ }
+ }
+ else
+ {
+ // on intra
+ // check the patch below:
+ float delta_x_below, delta_y_below;
+ float max_below = getScoreMaxBelow (layer, x_layer, y_layer,
+ center, ismax,
+ delta_x_below, delta_y_below);
+ if (!ismax) return (0.0);
+
+ // get the patch on this layer:
+ int s_0_0 = this_layer.getAgastScore (x_layer - 1, y_layer - 1, 1);
+ int s_1_0 = this_layer.getAgastScore (x_layer, y_layer - 1, 1);
+ int s_2_0 = this_layer.getAgastScore (x_layer + 1, y_layer - 1, 1);
+ int s_2_1 = this_layer.getAgastScore (x_layer + 1, y_layer, 1);
+ int s_1_1 = this_layer.getAgastScore (x_layer, y_layer, 1);
+ int s_0_1 = this_layer.getAgastScore (x_layer - 1, y_layer, 1);
+ int s_0_2 = this_layer.getAgastScore (x_layer - 1, y_layer + 1, 1);
+ int s_1_2 = this_layer.getAgastScore (x_layer, y_layer + 1, 1);
+ int s_2_2 = this_layer.getAgastScore (x_layer + 1, y_layer + 1, 1);
+ float delta_x_layer, delta_y_layer;
+ float max_layer = subpixel2D (s_0_0, s_0_1, s_0_2,
+ s_1_0, s_1_1, s_1_2,
+ s_2_0, s_2_1, s_2_2,
+ delta_x_layer, delta_y_layer);
+
+ // calculate the relative scale (1D maximum):
+ scale = refine1D_1 (max_below, std::max (float (center),max_layer), max_above,max);
+ if (scale > 1.0)
+ {
+ // interpolate the position:
+ const float r0 = 4.0f - scale * 3.0f;
+ const float r1 = 1.0f - r0;
+ x = (r0 * delta_x_layer + r1 * delta_x_above + float (x_layer))
+ * this_layer.getScale () + this_layer.getOffset ();
+ y = (r0 * delta_y_layer + r1 * delta_y_above + float (y_layer))
+ * this_layer.getScale () + this_layer.getOffset ();
+ }
+ else
+ {
+ // interpolate the position:
+ const float r0 = scale * 3.0f - 2.0f;
+ const float r_1 = 1.0f - r0;
+ x = (r0 * delta_x_layer + r_1 * delta_x_below + float (x_layer))
+ * this_layer.getScale () + this_layer.getOffset ();
+ y = (r0 * delta_y_layer + r_1 * delta_y_below + float (y_layer))
+ * this_layer.getScale () + this_layer.getOffset ();
+ }
+ }
+
+ // calculate the absolute scale:
+ scale *= this_layer.getScale ();
+
+ // that's it, return the refined maximum:
+ return (max);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+// return the maximum of score patches above or below
+float
+pcl::keypoints::brisk::ScaleSpace::getScoreMaxAbove (
+ const uint8_t layer, const int x_layer, const int y_layer,
+ const int threshold, bool& ismax, float& dx, float& dy)
+{
+ ismax = false;
+ // relevant floating point coordinates
+ float x_1;
+ float x1;
+ float y_1;
+ float y1;
+
+ // the layer above
+ assert (layer + 1 < layers_);
+ pcl::keypoints::brisk::Layer& layer_above = pyramid_[layer+1];
+
+ if (layer % 2 == 0)
+ {
+ // octave
+ x_1 = float (4 * (x_layer) - 1 - 2) / 6.0f;
+ x1 = float (4 * (x_layer) - 1 + 2) / 6.0f;
+ y_1 = float (4 * (y_layer) - 1 - 2) / 6.0f;
+ y1 = float (4 * (y_layer) - 1 + 2) / 6.0f;
+ }
+ else
+ {
+ // intra
+ x_1 = float (6 * (x_layer) - 1 - 3) / 8.0f;
+ x1 = float (6 * (x_layer) - 1 + 3) / 8.0f;
+ y_1 = float (6 * (y_layer) - 1 - 3) / 8.0f;
+ y1 = float (6 * (y_layer) - 1 + 3) / 8.0f;
+ }
+
+ // check the first row
+ //int max_x = int (x_1) + 1;
+ //int max_y = int (y_1) + 1;
+ int max_x = int (x_1 + 1.0f);
+ int max_y = int (y_1 + 1.0f);
+ float tmp_max = 0;
+ float max = layer_above.getAgastScore (x_1, y_1, 1,1.0f);
+
+ if (max > threshold) return (0);
+ //for (int x = int (x_1) + 1; x <= int (x1); x++)
+ for (int x = int (x_1 + 1.0f); x <= int (x1); x++)
+ {
+ tmp_max = layer_above.getAgastScore (float (x), y_1, 1,1.0f);
+
+ if (tmp_max > threshold) return (0);
+ if (tmp_max > max)
+ {
+ max = tmp_max;
+ max_x = x;
+ }
+ }
+ tmp_max = layer_above.getAgastScore (x1, y_1, 1,1.0f);
+
+ if (tmp_max > threshold) return (0);
+ if (tmp_max > max)
+ {
+ max = tmp_max;
+ max_x = int (x1);
+ }
+
+ // middle rows
+ for (int y = int (y_1) + 1; y <= int (y1); y++)
+ {
+ tmp_max = layer_above.getAgastScore (x_1, float (y), 1);
+
+ if (tmp_max > threshold) return (0);
+ if (tmp_max > max)
+ {
+ max = tmp_max;
+ max_x = int (x_1 + 1);
+ max_y = y;
+ }
+ for (int x = int (x_1) + 1; x <= int (x1); x++)
+ {
+ tmp_max = layer_above.getAgastScore (x, y, 1);
+
+ if (tmp_max > threshold) return (0);
+ if (tmp_max > max)
+ {
+ max = tmp_max;
+ max_x = x;
+ max_y = y;
+ }
+ }
+ tmp_max = layer_above.getAgastScore(x1,float(y),1);
+
+ if (tmp_max > threshold) return 0;
+ if (tmp_max > max)
+ {
+ max = tmp_max;
+ max_x = int (x1);
+ max_y = y;
+ }
+ }
+
+ // bottom row
+ tmp_max = layer_above.getAgastScore (x_1, y1, 1);
+
+ if (tmp_max > max)
+ {
+ max = tmp_max;
+ max_x = int (x_1 + 1);
+ max_y = int (y1);
+ }
+ for (int x = int (x_1) + 1; x <= int (x1); x++)
+ {
+ tmp_max = layer_above.getAgastScore (float (x), y1, 1);
+
+ if (tmp_max > max)
+ {
+ max = tmp_max;
+ max_x = x;
+ max_y = int (y1);
+ }
+ }
+ tmp_max = layer_above.getAgastScore (x1, y1, 1);
+
+ if (tmp_max > max)
+ {
+ max = tmp_max;
+ max_x = int (x1);
+ max_y = int (y1);
+ }
+
+ //find dx/dy:
+ int s_0_0 = layer_above.getAgastScore (max_x - 1, max_y - 1, 1);
+ int s_1_0 = layer_above.getAgastScore (max_x, max_y - 1, 1);
+ int s_2_0 = layer_above.getAgastScore (max_x + 1, max_y - 1, 1);
+ int s_2_1 = layer_above.getAgastScore (max_x + 1, max_y, 1);
+ int s_1_1 = layer_above.getAgastScore (max_x, max_y, 1);
+ int s_0_1 = layer_above.getAgastScore (max_x - 1, max_y, 1);
+ int s_0_2 = layer_above.getAgastScore (max_x - 1, max_y + 1, 1);
+ int s_1_2 = layer_above.getAgastScore (max_x, max_y + 1, 1);
+ int s_2_2 = layer_above.getAgastScore (max_x + 1, max_y + 1, 1);
+ float dx_1, dy_1;
+ float refined_max = subpixel2D (s_0_0, s_0_1, s_0_2,
+ s_1_0, s_1_1, s_1_2,
+ s_2_0, s_2_1, s_2_2,
+ dx_1, dy_1);
+
+ // calculate dx/dy in above coordinates
+ float real_x = float (max_x) + dx_1;
+ float real_y = float (max_y) + dy_1;
+ bool returnrefined = true;
+ if (layer % 2 == 0)
+ {
+ dx = (real_x * 6.0f + 1.0f) / 4.0f - float (x_layer);
+ dy = (real_y * 6.0f + 1.0f) / 4.0f - float (y_layer);
+ }
+ else
+ {
+ dx = (real_x * 8.0f + 1.0f) / 6.0f - float (x_layer);
+ dy = (real_y * 8.0f + 1.0f) / 6.0f - float (y_layer);
+ }
+
+ // saturate
+ if (dx > 1.0f) { dx = 1.0f; returnrefined = false; }
+ if (dx < -1.0f) { dx = -1.0f; returnrefined = false; }
+ if (dy > 1.0f) { dy = 1.0f; returnrefined = false; }
+ if (dy < -1.0f) { dy = -1.0f; returnrefined = false; }
+
+ // done and ok.
+ ismax = true;
+ if (returnrefined)
+ return (std::max (refined_max,max));
+ return (max);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+float
+pcl::keypoints::brisk::ScaleSpace::getScoreMaxBelow (
+ const uint8_t layer, const int x_layer, const int y_layer,
+ const int threshold, bool& ismax, float& dx, float& dy)
+{
+ ismax = false;
+
+ // relevant floating point coordinates
+ float x_1;
+ float x1;
+ float y_1;
+ float y1;
+
+ if (layer % 2 == 0)
+ {
+ // octave
+ x_1 = float (8 * (x_layer) + 1 - 4) / 6.0f;
+ x1 = float (8 * (x_layer) + 1 + 4) / 6.0f;
+ y_1 = float (8 * (y_layer) + 1 - 4) / 6.0f;
+ y1 = float (8 * (y_layer) + 1 + 4) / 6.0f;
+ }
+ else
+ {
+ x_1 = float (6 * (x_layer) + 1 - 3) / 4.0f;
+ x1 = float (6 * (x_layer) + 1 + 3) / 4.0f;
+ y_1 = float (6 * (y_layer) + 1 - 3) / 4.0f;
+ y1 = float (6 * (y_layer) + 1 + 3) / 4.0f;
+ }
+
+ // the layer below
+ assert (layer > 0);
+ pcl::keypoints::brisk::Layer& layer_below = pyramid_[layer-1];
+
+ // check the first row
+ int max_x = int (x_1) + 1;
+ int max_y = int (y_1) + 1;
+ float tmp_max;
+ float max = layer_below.getAgastScore (x_1, y_1, 1);
+ if (max > threshold) return (0);
+ for (int x = int (x_1) + 1; x <= int (x1); x++)
+ {
+ tmp_max = layer_below.getAgastScore (float (x), y_1, 1);
+ if (tmp_max > threshold) return (0);
+ if (tmp_max > max)
+ {
+ max = tmp_max;
+ max_x = x;
+ }
+ }
+ tmp_max = layer_below.getAgastScore (x1, y_1, 1);
+ if (tmp_max > threshold) return (0);
+ if (tmp_max > max)
+ {
+ max = tmp_max;
+ max_x = int (x1);
+ }
+
+ // middle rows
+ for (int y = int (y_1) + 1; y <= int (y1); y++)
+ {
+ tmp_max = layer_below.getAgastScore (x_1, float (y), 1);
+ if (tmp_max > threshold) return (0);
+ if (tmp_max > max)
+ {
+ max = tmp_max;
+ max_x = int (x_1 + 1);
+ max_y = y;
+ }
+ for (int x = int (x_1) + 1; x <= int (x1); x++)
+ {
+ tmp_max = layer_below.getAgastScore (x, y, 1);
+ if (tmp_max > threshold) return (0);
+ if (tmp_max == max)
+ {
+ const int t1 = 2 * (
+ layer_below.getAgastScore (x - 1, y, 1)
+ + layer_below.getAgastScore (x + 1, y, 1)
+ + layer_below.getAgastScore (x, y + 1, 1)
+ + layer_below.getAgastScore (x, y - 1, 1))
+ + (layer_below.getAgastScore (x + 1, y + 1, 1)
+ + layer_below.getAgastScore (x - 1, y + 1, 1)
+ + layer_below.getAgastScore (x + 1, y - 1, 1)
+ + layer_below.getAgastScore (x - 1, y - 1, 1));
+ const int t2 = 2 * (
+ layer_below.getAgastScore (max_x - 1, max_y, 1)
+ + layer_below.getAgastScore (max_x + 1, max_y, 1)
+ + layer_below.getAgastScore (max_x, max_y + 1, 1)
+ + layer_below.getAgastScore (max_x, max_y - 1, 1))
+ + (layer_below.getAgastScore (max_x + 1, max_y + 1, 1)
+ + layer_below.getAgastScore (max_x - 1, max_y + 1, 1)
+ + layer_below.getAgastScore (max_x + 1, max_y - 1, 1)
+ + layer_below.getAgastScore (max_x - 1, max_y - 1, 1));
+ if (t1 > t2)
+ {
+ max_x = x;
+ max_y = y;
+ }
+ }
+ if (tmp_max > max)
+ {
+ max = tmp_max;
+ max_x = x;
+ max_y = y;
+ }
+ }
+ tmp_max = layer_below.getAgastScore (x1, float (y), 1);
+ if (tmp_max > threshold) return (0);
+ if (tmp_max > max)
+ {
+ max = tmp_max;
+ max_x = int (x1);
+ max_y = y;
+ }
+ }
+
+ // bottom row
+ tmp_max = layer_below.getAgastScore (x_1, y1, 1);
+ if (tmp_max > max)
+ {
+ max = tmp_max;
+ max_x = int (x_1 + 1);
+ max_y = int (y1);
+ }
+ for (int x = int (x_1) + 1; x <= int (x1); x++)
+ {
+ tmp_max = layer_below.getAgastScore (float (x), y1, 1);
+ if (tmp_max>max)
+ {
+ max = tmp_max;
+ max_x = x;
+ max_y = int (y1);
+ }
+ }
+ tmp_max = layer_below.getAgastScore (x1, y1, 1);
+ if (tmp_max > max)
+ {
+ max = tmp_max;
+ max_x = int (x1);
+ max_y = int (y1);
+ }
+
+ //find dx/dy:
+ int s_0_0 = layer_below.getAgastScore (max_x - 1, max_y - 1, 1);
+ int s_1_0 = layer_below.getAgastScore (max_x, max_y - 1, 1);
+ int s_2_0 = layer_below.getAgastScore (max_x + 1, max_y - 1, 1);
+ int s_2_1 = layer_below.getAgastScore (max_x + 1, max_y, 1);
+ int s_1_1 = layer_below.getAgastScore (max_x, max_y, 1);
+ int s_0_1 = layer_below.getAgastScore (max_x - 1, max_y, 1);
+ int s_0_2 = layer_below.getAgastScore (max_x - 1, max_y + 1, 1);
+ int s_1_2 = layer_below.getAgastScore (max_x, max_y + 1, 1);
+ int s_2_2 = layer_below.getAgastScore (max_x + 1, max_y + 1, 1);
+ float dx_1, dy_1;
+ float refined_max = subpixel2D (s_0_0, s_0_1, s_0_2,
+ s_1_0, s_1_1, s_1_2,
+ s_2_0, s_2_1, s_2_2,
+ dx_1, dy_1);
+
+ // calculate dx/dy in above coordinates
+ float real_x = float (max_x) + dx_1;
+ float real_y = float (max_y) + dy_1;
+ bool returnrefined = true;
+ if (layer % 2 == 0)
+ {
+ dx = (real_x * 6.0f + 1.0f) / 8.0f - float (x_layer);
+ dy = (real_y * 6.0f + 1.0f) / 8.0f - float (y_layer);
+ }
+ else
+ {
+ dx = (real_x * 4.0f - 1.0f) / 6.0f - float (x_layer);
+ dy = (real_y * 4.0f - 1.0f) / 6.0f - float (y_layer);
+ }
+
+ // saturate
+ if (dx > 1.0f) { dx = 1.0f; returnrefined = false; }
+ if (dx < -1.0f) { dx = -1.0f; returnrefined = false; }
+ if (dy > 1.0f) { dy = 1.0f; returnrefined = false; }
+ if (dy < -1.0f) { dy = -1.0f; returnrefined = false; }
+
+ // done and ok.
+ ismax = true;
+ if (returnrefined)
+ return (std::max (refined_max, max));
+
+ return (max);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+float
+pcl::keypoints::brisk::ScaleSpace::refine1D (
+ const float s_05, const float s0, const float s05, float& max)
+{
+ int i_05 = int (1024.0 * s_05 + 0.5);
+ int i0 = int (1024.0 * s0 + 0.5);
+ int i05 = int (1024.0 * s05 + 0.5);
+
+ // 16.0000 -24.0000 8.0000
+ // -40.0000 54.0000 -14.0000
+ // 24.0000 -27.0000 6.0000
+
+ int three_a = 16 * i_05 - 24 * i0 + 8 * i05;
+ // second derivative must be negative:
+ if (three_a >= 0)
+ {
+ if (s0 >= s_05 && s0 >= s05)
+ {
+ max = s0;
+ return (1.0f);
+ }
+ if (s_05 >= s0 && s_05 >= s05)
+ {
+ max = s_05;
+ return (0.75f);
+ }
+ if (s05 >= s0 && s05 >= s_05)
+ {
+ max = s05;
+ return (1.5f);
+ }
+ }
+
+ int three_b = -40 * i_05 + 54 * i0 - 14 * i05;
+ // calculate max location:
+ float ret_val = -float (three_b) / float (2 * three_a);
+ // saturate and return
+ if (ret_val < 0.75f)
+ ret_val= 0.75f;
+ else
+ if (ret_val > 1.5f)
+ ret_val= 1.5f; // allow to be slightly off bounds ...?
+ int three_c = +24 * i_05 -27 * i0 +6 * i05;
+ max = float (three_c) + float (three_a) * ret_val * ret_val + float (three_b) * ret_val;
+ max /= 3072.0f;
+ return (ret_val);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+float
+pcl::keypoints::brisk::ScaleSpace::refine1D_1 (
+ const float s_05, const float s0, const float s05, float& max)
+{
+ int i_05 = int (1024.0 *s_05 + 0.5);
+ int i0 = int (1024.0 *s0 + 0.5);
+ int i05 = int (1024.0 *s05 + 0.5);
+
+ // 4.5000 -9.0000 4.5000
+ //-10.5000 18.0000 -7.5000
+ // 6.0000 -8.0000 3.0000
+
+ int two_a = 9 * i_05 - 18 * i0 + 9 * i05;
+ // second derivative must be negative:
+ if (two_a >= 0)
+ {
+ if (s0 >= s_05 && s0 >= s05)
+ {
+ max = s0;
+ return (1.0f);
+ }
+ if(s_05>=s0 && s_05>=s05)
+ {
+ max = s_05;
+ return (0.6666666666666666666666666667f);
+ }
+ if (s05 >= s0 && s05 >= s_05)
+ {
+ max = s05;
+ return (1.3333333333333333333333333333f);
+ }
+ }
+
+ int two_b = -21 * i_05 + 36 * i0 - 15 * i05;
+ // calculate max location:
+ float ret_val = -float (two_b) / float (2 * two_a);
+ // saturate and return
+ if (ret_val < 0.6666666666666666666666666667f)
+ ret_val = 0.666666666666666666666666667f;
+ else
+ if (ret_val > 1.33333333333333333333333333f)
+ ret_val = 1.333333333333333333333333333f;
+ int two_c = +12 * i_05 -16 * i0 +6 * i05;
+ max = float (two_c) + float (two_a) * ret_val * ret_val + float (two_b) * ret_val;
+ max /= 2048.0f;
+ return (ret_val);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+float
+pcl::keypoints::brisk::ScaleSpace::refine1D_2 (
+ const float s_05, const float s0, const float s05, float& max)
+{
+ int i_05 = int (1024.0 * s_05 + 0.5);
+ int i0 = int (1024.0 * s0 + 0.5);
+ int i05 = int (1024.0 * s05 + 0.5);
+
+ // 18.0000 -30.0000 12.0000
+ // -45.0000 65.0000 -20.0000
+ // 27.0000 -30.0000 8.0000
+
+ int a = 2 * i_05- 4 * i0 + 2 * i05;
+ // second derivative must be negative:
+ if (a >= 0)
+ {
+ if (s0 >= s_05 && s0 >= s05)
+ {
+ max = s0;
+ return (1.0f);
+ }
+ if (s_05 >= s0 && s_05 >= s05)
+ {
+ max = s_05;
+ return (0.7f);
+ }
+ if (s05 >= s0 && s05 >= s_05)
+ {
+ max = s05;
+ return (1.5f);
+ }
+ }
+
+ int b = -5 * i_05 + 8 * i0 - 3 * i05;
+ // calculate max location:
+ float ret_val = -float (b) / float (2 * a);
+ // saturate and return
+ if (ret_val < 0.7f)
+ ret_val = 0.7f;
+ else
+ if (ret_val > 1.5f)
+ ret_val = 1.5f; // allow to be slightly off bounds ...?
+ int c = +3 * i_05 -3 * i0 +1 * i05;
+ max = float (c) + float(a) * ret_val * ret_val + float (b) * ret_val;
+ max /= 1024.0f;
+ return (ret_val);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+float
+pcl::keypoints::brisk::ScaleSpace::subpixel2D (
+ const int s_0_0, const int s_0_1, const int s_0_2,
+ const int s_1_0, const int s_1_1, const int s_1_2,
+ const int s_2_0, const int s_2_1, const int s_2_2,
+ float& delta_x, float& delta_y)
+{
+ // the coefficients of the 2d quadratic function least-squares fit:
+ int tmp1 = s_0_0 + s_0_2 - 2*s_1_1 + s_2_0 + s_2_2;
+ int coeff1 = 3 * (tmp1 + s_0_1 - ((s_1_0 + s_1_2) << 1) + s_2_1);
+ int coeff2 = 3 * (tmp1 - ((s_0_1 + s_2_1) << 1) + s_1_0 + s_1_2);
+ int tmp2 = s_0_2 - s_2_0;
+ int tmp3 = (s_0_0 + tmp2 - s_2_2);
+ int tmp4 = tmp3 -2 * tmp2;
+ int coeff3 = -3 * (tmp3 + s_0_1 - s_2_1);
+ int coeff4 = -3 * (tmp4 + s_1_0 - s_1_2);
+ int coeff5 = (s_0_0 - s_0_2 - s_2_0 + s_2_2) << 2;
+ int coeff6 = -(s_0_0 + s_0_2 - ((s_1_0 + s_0_1 + s_1_2 + s_2_1) << 1) - 5 * s_1_1 + s_2_0 + s_2_2) << 1;
+
+
+ // 2nd derivative test:
+ int H_det = 4 * coeff1 * coeff2 - coeff5 * coeff5;
+
+ if (H_det == 0)
+ {
+ delta_x = 0.0f;
+ delta_y = 0.0f;
+ return (float (coeff6) / 18.0f);
+ }
+
+ if (!(H_det > 0 && coeff1 < 0))
+ {
+ // The maximum must be at the one of the 4 patch corners.
+ int tmp_max = coeff3 + coeff4 + coeff5;
+ delta_x = 1.0f; delta_y = 1.0f;
+
+ int tmp = -coeff3 + coeff4 - coeff5;
+ if (tmp > tmp_max)
+ {
+ tmp_max = tmp;
+ delta_x = -1.0f; delta_y = 1.0f;
+ }
+ tmp = coeff3 - coeff4 - coeff5;
+ if (tmp > tmp_max)
+ {
+ tmp_max = tmp;
+ delta_x = 1.0f; delta_y = -1.0f;
+ }
+ tmp = -coeff3 - coeff4 + coeff5;
+ if (tmp > tmp_max)
+ {
+ tmp_max = tmp;
+ delta_x = -1.0f; delta_y = -1.0f;
+ }
+ return (float (tmp_max + coeff1 + coeff2 + coeff6) / 18.0f);
+ }
+
+ // this is hopefully the normal outcome of the Hessian test
+ delta_x = float (2 * coeff2 * coeff3 - coeff4 * coeff5) / float (-H_det);
+ delta_y = float (2 * coeff1 * coeff4 - coeff3 * coeff5) / float (-H_det);
+ // TODO: this is not correct, but easy, so perform a real boundary maximum search:
+ bool tx = false; bool tx_ = false; bool ty = false; bool ty_ = false;
+ if (delta_x > 1.0f) tx = true;
+ else if (delta_x < -1.0f) tx_=true;
+ if (delta_y > 1.0f) ty = true;
+ if (delta_y < -1.0f) ty_ = true;
+
+ if (tx || tx_ || ty || ty_)
+ {
+ // get two candidates:
+ float delta_x1 = 0.0f, delta_x2 = 0.0f, delta_y1 = 0.0f, delta_y2 = 0.0f;
+ if (tx)
+ {
+ delta_x1 = 1.0f;
+ delta_y1 = -float (coeff4 + coeff5) / float (2.0 * coeff2);
+ if (delta_y1 > 1.0f) delta_y1 = 1.0f; else if (delta_y1 < -1.0f) delta_y1 = -1.0f;
+ }
+ else if (tx_)
+ {
+ delta_x1 = -1.0f;
+ delta_y1 = -float (coeff4 - coeff5) / float (2.0 * coeff2);
+ if (delta_y1 > 1.0f) delta_y1 = 1.0f; else if (delta_y1 < -1.0f) delta_y1 = -1.0f;
+ }
+ if (ty)
+ {
+ delta_y2 = 1.0f;
+ delta_x2 = -float (coeff3 + coeff5) / float (2.0 * coeff1);
+ if (delta_x2 > 1.0f) delta_x2 = 1.0f; else if (delta_x2 < -1.0f) delta_x2 = -1.0f;
+ }
+ else if (ty_)
+ {
+ delta_y2 = -1.0f;
+ delta_x2 = -float (coeff3 - coeff5) / float (2.0 * coeff1);
+ if (delta_x2 > 1.0f) delta_x2 = 1.0f; else if (delta_x2 < -1.0f) delta_x2 = -1.0f;
+ }
+ // insert both options for evaluation which to pick
+ float max1 = (float (coeff1) * delta_x1 * delta_x1 + float (coeff2) * delta_y1 * delta_y1
+ +float (coeff3) * delta_x1 + float (coeff4) * delta_y1
+ +float (coeff5) * delta_x1 * delta_y1
+ +float (coeff6)) / 18.0f;
+ float max2 = (float (coeff1) * delta_x2 * delta_x2 + float (coeff2) * delta_y2 * delta_y2
+ +float (coeff3) * delta_x2 + float (coeff4) * delta_y2
+ +float (coeff5) * delta_x2 * delta_y2
+ +float (coeff6)) / 18.0f;
+ if (max1 > max2)
+ {
+ delta_x = delta_x1;
+ delta_y = delta_x1;
+ return (max1);
+ }
+ else
+ {
+ delta_x = delta_x2;
+ delta_y = delta_x2;
+ return (max2);
+ }
+ }
+
+ // this is the case of the maximum inside the boundaries:
+ return ((float (coeff1) * delta_x * delta_x + float (coeff2) * delta_y * delta_y
+ +float (coeff3) * delta_x + float (coeff4) * delta_y
+ +float (coeff5) * delta_x * delta_y
+ +float (coeff6)) / 18.0f);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+// construct a layer
+pcl::keypoints::brisk::Layer::Layer (
+ const std::vector<unsigned char>& img,
+ int width, int height,
+ float scale, float offset)
+{
+ img_width_ = width;
+ img_height_ = height;
+ img_ = img;
+ scores_ = std::vector<unsigned char> (img_width_ * img_height_, 0);
+
+ // attention: this means that the passed image reference must point to persistent memory
+ scale_ = scale;
+ offset_ = offset;
+
+ // create an agast detector
+ oast_detector_.reset (new pcl::keypoints::agast::OastDetector9_16 (img_width_, img_height_, 0));
+ agast_detector_5_8_.reset (new pcl::keypoints::agast::AgastDetector5_8 (img_width_, img_height_, 0));
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+// derive a layer
+pcl::keypoints::brisk::Layer::Layer (const pcl::keypoints::brisk::Layer& layer, int mode)
+{
+ if (mode == CommonParams::HALFSAMPLE)
+ {
+ img_width_ = layer.img_width_ / 2;
+ img_height_ = layer.img_height_ / 2;
+ img_.resize (img_width_ * img_height_);
+
+ halfsample (layer.img_, layer.img_width_, layer.img_height_, img_, img_width_, img_height_);
+ scale_ = layer.scale_ * 2.0f;
+ offset_ = 0.5f * scale_ - 0.5f;
+ }
+ else
+ {
+ img_width_ = 2 * layer.img_width_ / 3;
+ img_height_ = 2 * layer.img_height_ / 3;
+ img_.resize (img_width_ * img_height_);
+
+ twothirdsample (layer.img_, layer.img_width_, layer.img_height_, img_, img_width_, img_height_);
+ scale_ = layer.scale_ * 1.5f;
+ offset_ = 0.5f * scale_ - 0.5f;
+ }
+
+ scores_ = std::vector<unsigned char> (img_width_ * img_height_, 0);
+
+ // create an agast detector
+ oast_detector_.reset (new pcl::keypoints::agast::OastDetector9_16 (img_width_, img_height_, 0));
+ agast_detector_5_8_.reset (new pcl::keypoints::agast::AgastDetector5_8 (img_width_, img_height_, 0));
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+// Fast/Agast
+// wraps the agast class
+void
+pcl::keypoints::brisk::Layer::getAgastPoints (
+ uint8_t threshold, std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > &keypoints)
+{
+ oast_detector_->setThreshold (threshold);
+ oast_detector_->detect (&img_[0], keypoints);
+
+ // also write scores
+ const int num = int (keypoints.size ());
+ const int imcols = img_width_;
+
+ for (int i = 0; i < num; i++)
+ {
+ const int offs = int (keypoints[i].u + keypoints[i].v * float (imcols));
+ *(&scores_[0] + offs) = static_cast<unsigned char> (oast_detector_->computeCornerScore (&img_[0] + offs));
+ }
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+pcl::uint8_t
+pcl::keypoints::brisk::Layer::getAgastScore (int x, int y, uint8_t threshold)
+{
+ if (x < 3 || y < 3)
+ {
+ return (0);
+ }
+ if (x >= img_width_ - 3 || y >= img_height_ - 3)
+ {
+ return (0);
+ }
+ uint8_t& score = *(&scores_[0] + x + y * img_width_);
+ if (score > 2)
+ {
+ return (score);
+ }
+ oast_detector_->setThreshold (threshold - 1);
+ score = uint8_t (oast_detector_->computeCornerScore (&img_[0] + x + y * img_width_));
+ if (score < threshold) score = 0;
+ return (score);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+pcl::uint8_t
+pcl::keypoints::brisk::Layer::getAgastScore_5_8 (int x, int y, uint8_t threshold)
+{
+ if (x < 2 || y < 2)
+ {
+ return 0;
+ }
+
+ if (x >= img_width_ - 2 || y >= img_height_ - 2)
+ {
+ return 0;
+ }
+
+ agast_detector_5_8_->setThreshold (threshold - 1);
+ uint8_t score = uint8_t (agast_detector_5_8_->computeCornerScore (&img_[0] + x + y * img_width_));
+ if (score < threshold) score = 0;
+ return (score);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+pcl::uint8_t
+pcl::keypoints::brisk::Layer::getAgastScore (float xf, float yf, uint8_t threshold, float scale)
+{
+ if (scale <= 1.0f)
+ {
+ // just do an interpolation inside the layer
+ const int x = int (xf);
+ const float rx1 = xf - float (x);
+ const float rx = 1.0f - rx1;
+ const int y = int (yf);
+ const float ry1 = yf -float (y);
+ const float ry = 1.0f -ry1;
+
+ const float value = rx * ry * getAgastScore (x, y, threshold)+
+ rx1 * ry * getAgastScore (x + 1, y, threshold)+
+ rx * ry1 * getAgastScore (x, y + 1, threshold)+
+ rx1 * ry1 * getAgastScore (x + 1, y + 1, threshold);
+
+
+ return (static_cast<uint8_t> (value));
+ }
+ else
+ {
+ // this means we overlap area smoothing
+ const float halfscale = scale / 2.0f;
+ // get the scores first:
+ for (int x = int (xf - halfscale); x <= int (xf + halfscale + 1.0f); x++)
+ for (int y = int (yf - halfscale); y <= int (yf + halfscale + 1.0f); y++)
+ getAgastScore (x, y, threshold);
+ // get the smoothed value
+ return (getValue (scores_, img_width_, img_height_, xf, yf, scale));
+ }
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+// access gray values (smoothed/interpolated)
+pcl::uint8_t
+pcl::keypoints::brisk::Layer::getValue (
+ const std::vector<unsigned char>& mat,
+ int width, int height,
+ float xf, float yf, float scale)
+{
+ (void)height;
+ assert (!mat.empty ());
+ // get the position
+ const int x = int (floor (xf));
+ const int y = int (floor (yf));
+ const std::vector<unsigned char>& image = mat;
+ const int& imagecols = width;
+
+ // get the sigma_half:
+ const float sigma_half = scale / 2.0f;
+ const float area = 4.0f * sigma_half * sigma_half;
+
+ // calculate output:
+ int ret_val;
+ if (sigma_half < 0.5)
+ {
+ // interpolation multipliers:
+ const int r_x = static_cast<int> ((xf - float (x)) * 1024);
+ const int r_y = static_cast<int> ((yf - float (y)) * 1024);
+ const int r_x_1 = (1024 - r_x);
+ const int r_y_1 = (1024 - r_y);
+ const unsigned char* ptr = &image[0] + x + y * imagecols;
+
+ // just interpolate:
+ ret_val = (r_x_1 * r_y_1 * int (*ptr));
+ ptr++;
+ ret_val += (r_x * r_y_1 * int (*ptr));
+ ptr += imagecols;
+ ret_val += (r_x * r_y * int (*ptr));
+ ptr--;
+ ret_val += (r_x_1 * r_y * int (*ptr));
+ return (static_cast<uint8_t> (0xFF & ((ret_val + 512) / 1024 / 1024)));
+ }
+
+ // this is the standard case (simple, not speed optimized yet):
+
+ // scaling:
+ const int scaling = static_cast<int> (4194304.0f / area);
+ const int scaling2 = static_cast<int> (float (scaling) * area / 1024.0f);
+
+ // calculate borders
+ const float x_1 = xf - sigma_half;
+ const float x1 = xf + sigma_half;
+ const float y_1 = yf - sigma_half;
+ const float y1 = yf + sigma_half;
+
+ const int x_left = int (x_1 + 0.5f);
+ const int y_top = int (y_1 + 0.5f);
+ const int x_right = int (x1 + 0.5f);
+ const int y_bottom = int (y1 + 0.5f);
+
+ // overlap area - multiplication factors:
+ const float r_x_1 = float (x_left) - x_1 + 0.5f;
+ const float r_y_1 = float (y_top) - y_1 + 0.5f;
+ const float r_x1 = x1 - float (x_right) + 0.5f;
+ const float r_y1 = y1 - float (y_bottom) + 0.5f;
+ const int dx = x_right - x_left - 1;
+ const int dy = y_bottom - y_top - 1;
+ const int A = static_cast<int> ((r_x_1 * r_y_1) * float (scaling));
+ const int B = static_cast<int> ((r_x1 * r_y_1) * float (scaling));
+ const int C = static_cast<int> ((r_x1 * r_y1) * float (scaling));
+ const int D = static_cast<int> ((r_x_1 * r_y1) * float (scaling));
+ const int r_x_1_i = static_cast<int> (r_x_1 * float (scaling));
+ const int r_y_1_i = static_cast<int> (r_y_1 * float (scaling));
+ const int r_x1_i = static_cast<int> (r_x1 * float (scaling));
+ const int r_y1_i = static_cast<int> (r_y1 * float (scaling));
+
+ // now the calculation:
+ const unsigned char* ptr = &image[0] + x_left + imagecols * y_top;
+ // first row:
+ ret_val = A * int (*ptr);
+ ptr++;
+ const unsigned char* end1 = ptr + dx;
+ for (; ptr < end1; ptr++)
+ ret_val += r_y_1_i * int (*ptr);
+
+ ret_val += B * int (*ptr);
+
+ // middle ones:
+ ptr += imagecols - dx - 1;
+ const unsigned char* end_j = ptr + dy * imagecols;
+
+ for (; ptr < end_j; ptr += imagecols - dx - 1)
+ {
+ ret_val += r_x_1_i * int (*ptr);
+ ptr++;
+ const unsigned char* end2 = ptr + dx;
+ for (; ptr < end2; ptr++)
+ ret_val += int (*ptr) * scaling;
+
+ ret_val += r_x1_i * int (*ptr);
+ }
+
+ // last row:
+ ret_val += D * int (*ptr);
+ ptr++;
+ const unsigned char* end3 = ptr + dx;
+ for (; ptr < end3; ptr++)
+ ret_val += r_y1_i * int (*ptr);
+
+ ret_val += C * int (*ptr);
+
+ return (static_cast<uint8_t> (0xFF & ((ret_val + scaling2 / 2) / scaling2 / 1024)));
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+// half sampling
+inline void
+pcl::keypoints::brisk::Layer::halfsample (
+ const std::vector<unsigned char>& srcimg,
+ int srcwidth, int srcheight,
+ std::vector<unsigned char>& dstimg,
+ int dstwidth, int dstheight)
+{
+ (void)dstheight;
+#if defined(__SSSE3__) && !defined(__i386__)
+ const unsigned short leftoverCols = static_cast<unsigned short> ((srcwidth % 16) / 2); // take care with border...
+ const bool noleftover = (srcwidth % 16) == 0; // note: leftoverCols can be zero but this still false...
+
+ // make sure the destination image is of the right size:
+ assert (floor (double (srcwidth) / 2.0) == dstwidth);
+ assert (floor (double (srcheight) / 2.0) == dstheight);
+
+ // mask needed later:
+ register __m128i mask = _mm_set_epi32 (0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF);
+ // to be added in order to make successive averaging correct:
+ register __m128i ones = _mm_set_epi32 (0x11111111, 0x11111111, 0x11111111, 0x11111111);
+
+ // data pointers:
+ const __m128i* p1 = reinterpret_cast<const __m128i*> (&srcimg[0]);
+ const __m128i* p2 = reinterpret_cast<const __m128i*> (&srcimg[0] + srcwidth);
+ __m128i* p_dest = reinterpret_cast<__m128i*> (&dstimg[0]);
+ unsigned char* p_dest_char;//=(unsigned char*)p_dest;
+
+ // size:
+ const unsigned int size = (srcwidth * srcheight) / 16;
+ const unsigned int hsize = srcwidth / 16;
+ const __m128i* p_end = p1 + size;
+ unsigned int row = 0;
+ const unsigned int end = hsize / 2;
+ bool half_end;
+ if (hsize % 2 == 0)
+ half_end = false;
+ else
+ half_end = true;
+ while (p2 < p_end)
+ {
+ for (unsigned int i = 0; i < end; i++)
+ {
+ // load the two blocks of memory:
+ __m128i upper;
+ __m128i lower;
+ if (noleftover)
+ {
+ upper = _mm_load_si128 (p1);
+ lower = _mm_load_si128 (p2);
+ }
+ else
+ {
+ upper = _mm_loadu_si128 (p1);
+ lower = _mm_loadu_si128 (p2);
+ }
+
+ __m128i result1 = _mm_adds_epu8 (upper, ones);
+ result1 = _mm_avg_epu8 (upper, lower);
+
+ // increment the pointers:
+ p1++;
+ p2++;
+
+ // load the two blocks of memory:
+ upper = _mm_loadu_si128 (p1);
+ lower = _mm_loadu_si128 (p2);
+ __m128i result2 = _mm_adds_epu8 (upper, ones);
+ result2 = _mm_avg_epu8 (upper, lower);
+ // calculate the shifted versions:
+ __m128i result1_shifted = _mm_srli_si128 (result1, 1);
+ __m128i result2_shifted = _mm_srli_si128 (result2, 1);
+ // pack:
+ __m128i result = _mm_packus_epi16 (_mm_and_si128 (result1, mask), _mm_and_si128 (result2, mask));
+ __m128i result_shifted = _mm_packus_epi16 (_mm_and_si128 (result1_shifted, mask),_mm_and_si128 (result2_shifted, mask));
+ // average for the second time:
+ result = _mm_avg_epu8 (result, result_shifted);
+
+ // store to memory
+ _mm_storeu_si128 (p_dest, result);
+
+ // increment the pointers:
+ p1++;
+ p2++;
+ p_dest++;
+ //p_dest_char=(unsigned char*)p_dest;
+ }
+ // if we are not at the end of the row, do the rest:
+ if (half_end)
+ {
+ // load the two blocks of memory:
+ __m128i upper;
+ __m128i lower;
+ if (noleftover)
+ {
+ upper = _mm_load_si128 (p1);
+ lower = _mm_load_si128 (p2);
+ }
+ else
+ {
+ upper = _mm_loadu_si128 (p1);
+ lower = _mm_loadu_si128 (p2);
+ }
+
+ __m128i result1 = _mm_adds_epu8 (upper, ones);
+ result1 = _mm_avg_epu8 (upper, lower);
+
+ // increment the pointers:
+ p1++;
+ p2++;
+
+ // compute horizontal pairwise average and store
+ p_dest_char = reinterpret_cast<unsigned char*> (p_dest);
+#ifdef __GNUC__
+ typedef unsigned char __attribute__ ((__may_alias__)) UCHAR_ALIAS;
+#endif
+#ifdef _MSC_VER
+ // Todo: find the equivalent to may_alias
+ #define UCHAR_ALIAS unsigned char //__declspec(noalias)
+#endif
+ const UCHAR_ALIAS* result = reinterpret_cast<const UCHAR_ALIAS*> (&result1);
+ for (unsigned int j = 0; j < 8; j++)
+ {
+ *(p_dest_char++) = static_cast<unsigned char> ((*(result + 2 * j) + *(result + 2 * j + 1)) / 2);
+ }
+ //p_dest_char=(unsigned char*)p_dest;
+ }
+ else
+ p_dest_char = reinterpret_cast<unsigned char*> (p_dest);
+
+ if (noleftover)
+ {
+ row++;
+ p_dest = reinterpret_cast<__m128i*> (&dstimg[0] + row * dstwidth);
+ p1 = reinterpret_cast<const __m128i*> (&srcimg[0] + 2 * row * srcwidth);
+ //p2=(__m128i*)(srcimg.data+(2*row+1)*srcwidth);
+ //p1+=hsize;
+ p2 = p1 + hsize;
+ }
+ else
+ {
+ const unsigned char* p1_src_char = reinterpret_cast<const unsigned char*> (p1);
+ const unsigned char* p2_src_char = reinterpret_cast<const unsigned char*> (p2);
+ for (unsigned int k = 0; k < leftoverCols; k++)
+ {
+ unsigned short tmp = static_cast<unsigned short> (p1_src_char[k] + p1_src_char[k+1]+ p2_src_char[k] + p2_src_char[k+1]);
+ *(p_dest_char++) = static_cast<unsigned char>(tmp / 4);
+ }
+ // done with the two rows:
+ row++;
+ p_dest = reinterpret_cast<__m128i*> (&dstimg[0] + row * dstwidth);
+ p1 = reinterpret_cast<const __m128i*> (&srcimg[0] + 2 * row * srcwidth);
+ p2 = reinterpret_cast<const __m128i*> (&srcimg[0] + (2 * row + 1) * srcwidth);
+ }
+ }
+#else
+ (void) (srcimg);
+ (void) (srcwidth);
+ (void) (srcheight);
+ (void) (dstimg);
+ (void) (dstwidth);
+ PCL_ERROR("brisk without SSSE3 support not implemented");
+#endif
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::keypoints::brisk::Layer::twothirdsample (
+ const std::vector<unsigned char>& srcimg,
+ int srcwidth, int srcheight,
+ std::vector<unsigned char>& dstimg,
+ int dstwidth, int dstheight)
+{
+ (void)dstheight;
+#if defined(__SSSE3__) && !defined(__i386__)
+ const unsigned short leftoverCols = static_cast<unsigned short> (((srcwidth / 3) * 3) % 15);// take care with border...
+
+ // make sure the destination image is of the right size:
+ assert (floor (double (srcwidth) / 3.0 * 2.0) == dstwidth);
+ assert (floor (double (srcheight) / 3.0 * 2.0) == dstheight);
+
+ // masks:
+ register __m128i mask1 = _mm_set_epi8 (char(0x80),char(0x80),char(0x80),char(0x80),char(0x80),char(0x80),char(0x80),12,char(0x80),10,char(0x80),7,char(0x80),4,char(0x80),1);
+ register __m128i mask2 = _mm_set_epi8 (char(0x80),char(0x80),char(0x80),char(0x80),char(0x80),char(0x80),12,char(0x80),10,char(0x80),7,char(0x80),4,char(0x80),1,char(0x80));
+ register __m128i mask = _mm_set_epi8 (char(0x80),char(0x80),char(0x80),char(0x80),char(0x80),char(0x80),14,12,11,9,8,6,5,3,2,0);
+ register __m128i store_mask = _mm_set_epi8 (0x0,0x0,0x0,0x0,0x0,0x0,char(0x80),char(0x80),char(0x80),char(0x80),char(0x80),char(0x80),char(0x80),char(0x80),char(0x80),char(0x80));
+
+ // data pointers:
+ const unsigned char* p1 = &srcimg[0];
+ const unsigned char* p2 = p1 + srcwidth;
+ const unsigned char* p3 = p2 + srcwidth;
+ unsigned char* p_dest1 = &dstimg[0];
+ unsigned char* p_dest2 = p_dest1 + dstwidth;
+ const unsigned char* p_end = p1 + (srcwidth * srcheight);
+
+ unsigned int row = 0;
+ unsigned int row_dest = 0;
+ int hsize = srcwidth / 15;
+ while (p3 < p_end)
+ {
+ for (int i = 0; i < hsize; i++)
+ {
+ // load three rows
+ const __m128i first = _mm_loadu_si128 (reinterpret_cast<const __m128i*> (p1));
+ const __m128i second = _mm_loadu_si128 (reinterpret_cast<const __m128i*> (p2));
+ const __m128i third = _mm_loadu_si128 (reinterpret_cast<const __m128i*> (p3));
+
+ // upper row:
+ __m128i upper = _mm_avg_epu8 (_mm_avg_epu8 (first,second),first);
+ __m128i temp1_upper = _mm_or_si128 (_mm_shuffle_epi8 (upper,mask1), _mm_shuffle_epi8 (upper,mask2));
+ __m128i temp2_upper = _mm_shuffle_epi8 (upper,mask);
+ __m128i result_upper = _mm_avg_epu8 (_mm_avg_epu8 (temp2_upper, temp1_upper), temp2_upper);
+
+ // lower row:
+ __m128i lower = _mm_avg_epu8 (_mm_avg_epu8 (third, second), third);
+ __m128i temp1_lower = _mm_or_si128 (_mm_shuffle_epi8 (lower, mask1), _mm_shuffle_epi8 (lower, mask2));
+ __m128i temp2_lower = _mm_shuffle_epi8 (lower,mask);
+ __m128i result_lower = _mm_avg_epu8 (_mm_avg_epu8 (temp2_lower, temp1_lower), temp2_lower);
+
+ // store:
+ if (i * 10 + 16 > dstwidth)
+ {
+ _mm_maskmoveu_si128 (result_upper, store_mask, reinterpret_cast<char*> (p_dest1));
+ _mm_maskmoveu_si128 (result_lower, store_mask, reinterpret_cast<char*> (p_dest2));
+ }
+ else
+ {
+ _mm_storeu_si128 (reinterpret_cast<__m128i*> (p_dest1), result_upper);
+ _mm_storeu_si128 (reinterpret_cast<__m128i*> (p_dest2), result_lower);
+ }
+
+ // shift pointers:
+ p1 += 15;
+ p2 += 15;
+ p3 += 15;
+ p_dest1 += 10;
+ p_dest2 += 10;
+ }
+
+ // fill the remainder:
+ for (unsigned int j = 0; j < leftoverCols; j+= 3)
+ {
+ const unsigned short A1 = *(p1++);
+ const unsigned short A2 = *(p1++);
+ const unsigned short A3 = *(p1++);
+ const unsigned short B1 = *(p2++);
+ const unsigned short B2 = *(p2++);
+ const unsigned short B3 = *(p2++);
+ const unsigned short C1 = *(p3++);
+ const unsigned short C2 = *(p3++);
+ const unsigned short C3 = *(p3++);
+
+ *(p_dest1++) = static_cast<unsigned char> (((4 * A1 + 2 * (A2 + B1) + B2) / 9) & 0x00FF);
+ *(p_dest1++) = static_cast<unsigned char> (((4 * A3 + 2 * (A2 + B3) + B2) / 9) & 0x00FF);
+ *(p_dest2++) = static_cast<unsigned char> (((4 * C1 + 2 * (C2 + B1) + B2) / 9) & 0x00FF);
+ *(p_dest2++) = static_cast<unsigned char> (((4 * C3 + 2 * (C2 + B3) + B2) / 9) & 0x00FF);
+ }
+
+ // increment row counter:
+ row += 3;
+ row_dest += 2;
+
+ // reset pointers
+ p1 = &srcimg[0] + row * srcwidth;
+ p2 = p1 + srcwidth;
+ p3 = p2 + srcwidth;
+ p_dest1 = &dstimg[0] + row_dest * dstwidth;
+ p_dest2 = p_dest1 + dstwidth;
+ }
+#else
+ (void) (srcimg);
+ (void) (srcwidth);
+ (void) (srcheight);
+ (void) (dstimg);
+ (void) (dstwidth);
+ PCL_ERROR("brisk without SSSE3 support not implemented");
+#endif
+}
+
typedef double RealForPolynomial;
PolynomialCalculationsT<RealForPolynomial> polynomial_calculations;
BivariatePolynomialT<RealForPolynomial> polynomial (2);
- std::vector<Eigen::Matrix<RealForPolynomial, 3, 1> > sample_points;
+ std::vector<Eigen::Matrix<RealForPolynomial, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<RealForPolynomial, 3, 1> > > sample_points;
std::vector<RealForPolynomial> x_values, y_values;
std::vector<int> types;
std::vector<bool> invalid_beams, old_invalid_beams;
+++ /dev/null
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2010-2011, Willow Garage, Inc.
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * $Id$
- */
-
-#include <pcl/keypoints/uniform_sampling.h>
-#include <pcl/keypoints/impl/uniform_sampling.hpp>
-#include <pcl/point_types.h>
-#include <pcl/impl/instantiate.hpp>
-
-
-// Instantiations of specific point types
-PCL_INSTANTIATE_PRODUCT(UniformSampling, (PCL_XYZ_POINT_TYPES))
--- /dev/null
+set(SUBSYS_NAME ml)
+set(SUBSYS_DESC "Point cloud machine learning library")
+set(SUBSYS_DEPS common)
+
+set(build TRUE)
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS})
+
+PCL_ADD_DOC("${SUBSYS_NAME}")
+
+if(build)
+ set(incs
+ "include/pcl/${SUBSYS_NAME}/dt/decision_forest.h"
+ "include/pcl/${SUBSYS_NAME}/dt/decision_forest_evaluator.h"
+ "include/pcl/${SUBSYS_NAME}/dt/decision_forest_trainer.h"
+ "include/pcl/${SUBSYS_NAME}/dt/decision_tree.h"
+ "include/pcl/${SUBSYS_NAME}/dt/decision_tree_evaluator.h"
+ "include/pcl/${SUBSYS_NAME}/dt/decision_tree_trainer.h"
+ "include/pcl/${SUBSYS_NAME}/dt/decision_tree_data_provider.h"
+ "include/pcl/${SUBSYS_NAME}/ferns/fern.h"
+ "include/pcl/${SUBSYS_NAME}/ferns/fern_evaluator.h"
+ "include/pcl/${SUBSYS_NAME}/ferns/fern_trainer.h"
+ "include/pcl/${SUBSYS_NAME}/branch_estimator.h"
+ "include/pcl/${SUBSYS_NAME}/feature_handler.h"
+ "include/pcl/${SUBSYS_NAME}/multi_channel_2d_comparison_feature.h"
+ "include/pcl/${SUBSYS_NAME}/multi_channel_2d_comparison_feature_handler.h"
+ "include/pcl/${SUBSYS_NAME}/multi_channel_2d_data_set.h"
+ "include/pcl/${SUBSYS_NAME}/multiple_data_2d_example_index.h"
+ "include/pcl/${SUBSYS_NAME}/point_xy_32i.h"
+ "include/pcl/${SUBSYS_NAME}/point_xy_32f.h"
+ "include/pcl/${SUBSYS_NAME}/regression_variance_stats_estimator.h"
+ "include/pcl/${SUBSYS_NAME}/stats_estimator.h"
+ "include/pcl/${SUBSYS_NAME}/densecrf.h"
+ "include/pcl/${SUBSYS_NAME}/pairwise_potential.h"
+ "include/pcl/${SUBSYS_NAME}/permutohedral.h"
+ #"include/pcl/${SUBSYS_NAME}/util.h"
+ #"include/pcl/${SUBSYS_NAME}/permutohedralORI.h"
+ #"include/pcl/${SUBSYS_NAME}/fastmath.h"
+ #"include/pcl/${SUBSYS_NAME}/densecrfORI.h"
+ "include/pcl/${SUBSYS_NAME}/svm_wrapper.h"
+ "include/pcl/${SUBSYS_NAME}/svm.h"
+ "include/pcl/${SUBSYS_NAME}/kmeans.h"
+ )
+
+ set(impl_incs
+ "include/pcl/${SUBSYS_NAME}/impl/dt/decision_forest_evaluator.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/dt/decision_forest_trainer.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/dt/decision_tree_evaluator.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/dt/decision_tree_trainer.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/ferns/fern_evaluator.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/ferns/fern_trainer.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/svm/svm_wrapper.hpp"
+ #"include/pcl/${SUBSYS_NAME}/impl/kmeans.hpp"
+ )
+
+ set(srcs
+ src/point_xy_32i.cpp
+ src/point_xy_32f.cpp
+ src/densecrf.cpp
+ src/pairwise_potential.cpp
+ src/permutohedral.cpp
+ #src/permutohedralORI.cpp
+ #src/filter.cpp
+ #src/util.cpp
+ #src/densecrfORI.cpp
+ src/svm_wrapper.cpp
+ src/svm.cpp
+ src/kmeans.cpp
+ )
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
+ PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs})
+ SET_TARGET_PROPERTIES("${LIB_NAME}" PROPERTIES LINKER_LANGUAGE CXX)
+ target_link_libraries("${LIB_NAME}" pcl_common)
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "")
+ # Install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs})
+
+endif(build)
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_BRANCH_ESTIMATOR_H_
+#define PCL_ML_BRANCH_ESTIMATOR_H_
+
+#include <pcl/common/common.h>
+#include <pcl/ml/stats_estimator.h>
+
+#include <istream>
+#include <ostream>
+
+namespace pcl
+{
+
+ /** \brief Interface for branch estimators. */
+ class PCL_EXPORTS BranchEstimator
+ {
+ public:
+ /** \brief Destructor. */
+ virtual ~BranchEstimator () {}
+
+ /** \brief Returns the number of branches the corresponding tree has. */
+ virtual size_t
+ getNumOfBranches () const = 0;
+
+ /** \brief Computes the branch index for the specified result.
+ * \param[in] result The result the branch index will be computed for.
+ * \param[in] flag The flag corresponding to the specified result.
+ * \param[in] threshold The threshold used to compute the branch index.
+ * \param[out] branch_index The destination for the computed branch index.
+ */
+ virtual void
+ computeBranchIndex(
+ const float result,
+ const unsigned char flag,
+ const float threshold,
+ unsigned char & branch_index) const = 0;
+ };
+
+ /** \brief Branch estimator for binary trees where the branch is computed only from the threshold. */
+ class PCL_EXPORTS BinaryTreeThresholdBasedBranchEstimator
+ : public BranchEstimator
+ {
+ public:
+ /** \brief Constructor. */
+ inline BinaryTreeThresholdBasedBranchEstimator () {}
+ /** \brief Destructor. */
+ inline virtual ~BinaryTreeThresholdBasedBranchEstimator () {}
+
+ /** \brief Returns the number of branches the corresponding tree has. */
+ inline size_t
+ getNumOfBranches () const
+ {
+ return 2;
+ }
+
+ /** \brief Computes the branch index for the specified result.
+ * \param[in] result The result the branch index will be computed for.
+ * \param[in] flag The flag corresponding to the specified result.
+ * \param[in] threshold The threshold used to compute the branch index.
+ * \param[out] branch_index The destination for the computed branch index.
+ */
+ inline void
+ computeBranchIndex(
+ const float result,
+ const unsigned char flag,
+ const float threshold,
+ unsigned char & branch_index) const
+ {
+ (void)flag;
+ branch_index = (result > threshold) ? 1 : 0;
+ }
+ };
+
+ /** \brief Branch estimator for ternary trees where one branch is used for missing data (indicated by flag != 0). */
+ class PCL_EXPORTS TernaryTreeMissingDataBranchEstimator
+ : public BranchEstimator
+ {
+ public:
+ /** \brief Constructor. */
+ inline TernaryTreeMissingDataBranchEstimator () {}
+ /** \brief Destructor. */
+ inline virtual ~TernaryTreeMissingDataBranchEstimator () {}
+
+ /** \brief Returns the number of branches the corresponding tree has. */
+ inline size_t
+ getNumOfBranches () const
+ {
+ return 3;
+ }
+
+ /** \brief Computes the branch index for the specified result.
+ * \param[in] result The result the branch index will be computed for.
+ * \param[in] flag The flag corresponding to the specified result.
+ * \param[in] threshold The threshold used to compute the branch index.
+ * \param[out] branch_index The destination for the computed branch index.
+ */
+ inline void
+ computeBranchIndex(
+ const float result,
+ const unsigned char flag,
+ const float threshold,
+ unsigned char & branch_index) const
+ {
+ if (flag == 0)
+ branch_index = (result > threshold) ? 1 : 0;
+ else
+ branch_index = 2;
+ }
+ };
+
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author : Christian Potthast
+ * Email : potthast@usc.edu
+ *
+ */
+
+#ifndef PCL_DENSE_CRF_H_
+#define PCL_DENSE_CRF_H_
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <pcl/ml/pairwise_potential.h>
+
+namespace pcl
+{
+ /** \brief
+ *
+ */
+ class PCL_EXPORTS DenseCrf
+ {
+ public:
+
+ /** \brief Constructor for DenseCrf class */
+ DenseCrf (int N, int m);
+
+ /** \brief Deconstructor for DenseCrf class */
+ ~DenseCrf ();
+
+ /** \brief set the input data vector.
+ * The input data vector holds the measurements
+ * coordinates as ijk of the voxel grid
+ */
+ void
+ setDataVector (const std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > data);
+
+ /** \brief The associated color of the data
+ */
+ void
+ setColorVector (const std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > color);
+
+ void
+ setUnaryEnergy (const std::vector<float> unary);
+
+ /** \brief */
+ void
+ addPairwiseEnergy (const std::vector<float> &feature, const int feature_dimension, const float w);
+
+
+ /** \brief Add a pairwise gaussian kernel
+ *
+ */
+ void
+ addPairwiseGaussian (float sx, float sy, float sz, float w);
+
+ /** \brief Add a bilateral gaussian kernel
+ *
+ */
+ void
+ addPairwiseBilateral (float sx, float sy, float sz,
+ float sr, float sg, float sb,
+ float w);
+
+
+ void
+ addPairwiseNormals (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > &coord,
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &normals,
+ float sx, float sy, float sz,
+ float snx, float sny, float snz,
+ float w);
+
+
+ void
+ inference (int n_iterations, std::vector<float> &result, float relax = 1.0f);
+
+ void
+ mapInference (int n_iterations, std::vector<int> &result, float relax = 1.0f);
+
+ void
+ expAndNormalize (std::vector<float> &out, const std::vector<float> &in,
+ float scale, float relax = 1.0f);
+
+ void
+ expAndNormalizeORI ( float* out, const float* in, float scale=1.0f, float relax=1.0f );
+ void map ( int n_iterations, std::vector<int> result, float relax=1.0f );
+ std::vector<float> runInference( int n_iterations, float relax );
+ void startInference();
+ void stepInference( float relax );
+
+
+ void
+ runInference (float relax);
+
+
+ void
+ getBarycentric (int idx, std::vector<float> &bary);
+
+ void
+ getFeatures (int idx, std::vector<float> &features);
+
+
+
+ protected:
+
+ /** \brief Number of variables and labels */
+ int N_, M_;
+
+ /** \brief Data vector */
+ std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > data_;
+
+ /** \brief Color vector */
+ std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > color_;
+
+ /** TODO: double might use to much memory */
+ /** \brief CRF unary potentials */
+ std::vector<float> unary_;
+
+ std::vector<float> current_;
+ std::vector<float> next_;
+ std::vector<float> tmp_;
+
+ /** \brief pairwise potentials */
+ std::vector<PairwisePotential*> pairwise_potential_;
+
+ /** \brief input types */
+ bool xyz_, rgb_, normal_;
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+}
+
+
+
+
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_DT_DECISION_FOREST_H_
+#define PCL_ML_DT_DECISION_FOREST_H_
+
+#include <pcl/common/common.h>
+
+#include <pcl/ml/dt/decision_tree.h>
+
+#include <istream>
+#include <ostream>
+
+namespace pcl
+{
+
+ /** \brief Class representing a decision forest. */
+ template <class NodeType>
+ class PCL_EXPORTS DecisionForest
+ : public std::vector<pcl::DecisionTree<NodeType> >
+ {
+
+ public:
+
+ /** \brief Constructor. */
+ DecisionForest () {}
+ /** \brief Destructor. */
+ virtual
+ ~DecisionForest () {}
+
+ /* \brief Adds the specified tree to the forest.
+ * \param[in] tree The tree to be added to the forest. */
+ //
+ //void
+ //addTree (DecisionTree<NodeType> & tree)
+ //{
+ // trees_.push_back (tree);
+ //}
+
+ ///brief Returns the number of trees in the forest.
+ //inline size_t
+
+ /** \brief Serializes the decision tree.
+ * \param[out] stream The destination for the serialization.
+ */
+ void
+ serialize (::std::ostream &stream) const
+ {
+ const int num_of_trees = static_cast<int> (this->size ());
+ stream.write (reinterpret_cast<const char*> (&num_of_trees), sizeof (num_of_trees));
+
+ for (size_t tree_index = 0; tree_index < this->size (); ++tree_index)
+ {
+ (*this) [tree_index].serialize (stream);
+ }
+
+ //const int num_of_trees = static_cast<int> (trees_.size ());
+ //stream.write (reinterpret_cast<const char*> (&num_of_trees), sizeof (num_of_trees));
+
+ //for (size_t tree_index = 0; tree_index < trees_.size (); ++tree_index)
+ //{
+ // tree_[tree_index].serialize (stream);
+ //}
+ }
+
+ /** \brief Deserializes the decision tree.
+ * \param[in] stream The source for the deserialization.
+ */
+ void
+ deserialize (::std::istream & stream)
+ {
+ int num_of_trees;
+ stream.read (reinterpret_cast<char*> (&num_of_trees), sizeof (num_of_trees));
+ this->resize (num_of_trees);
+
+ for (size_t tree_index = 0; tree_index < this->size (); ++tree_index)
+ {
+ (*this) [tree_index].deserialize (stream);
+ }
+
+ //int num_of_trees;
+ //stream.read (reinterpret_cast<char*> (&num_of_trees), sizeof (num_of_trees));
+ //trees_.resize (num_of_trees);
+
+ //for (size_t tree_index = 0; tree_index < trees_.size (); ++tree_index)
+ //{
+ // tree_[tree_index].deserialize (stream);
+ //}
+ }
+
+ private:
+
+ /** \brief The decision trees contained in the forest. */
+ //std::vector<DecisionTree<NodeType> > trees_;
+
+ };
+
+
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_DT_DECISION_FOREST_EVALUATOR_H_
+#define PCL_ML_DT_DECISION_FOREST_EVALUATOR_H_
+
+#include <pcl/common/common.h>
+
+#include <pcl/ml/dt/decision_tree_evaluator.h>
+#include <pcl/ml/dt/decision_forest.h>
+#include <pcl/ml/feature_handler.h>
+#include <pcl/ml/stats_estimator.h>
+
+#include <vector>
+
+namespace pcl
+{
+
+ /** \brief Utility class for evaluating a decision forests. */
+ template <
+ class FeatureType,
+ class DataSet,
+ class LabelType,
+ class ExampleIndex,
+ class NodeType >
+ class DecisionForestEvaluator
+ {
+ public:
+ /** \brief Constructor. */
+ DecisionForestEvaluator();
+ /** \brief Destructor. */
+ virtual
+ ~DecisionForestEvaluator();
+
+ /** \brief Evaluates the specified examples using the supplied forest.
+ * \param[in] DecisionForestEvaluator The decision forest.
+ * \param[in] feature_handler The feature handler used to train the tree.
+ * \param[in] stats_estimator The statistics estimation instance used while training the tree.
+ * \param[in] data_set The data set used for evaluation.
+ * \param[in] examples The examples that have to be evaluated.
+ * \param[out] label_data The destination for the resulting label data.
+ */
+ void
+ evaluate (pcl::DecisionForest<NodeType> & DecisionForestEvaluator,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator,
+ DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<LabelType> & label_data);
+
+ /** \brief Evaluates a specific patch using the supplied forest.
+ * \param[in] DecisionForestEvaluator The decision forest.
+ * \param[in] feature_handler The feature handler used to train the tree.
+ * \param[in] stats_estimator The statistics estimation instance used while training the tree.
+ * \param[in] data_set The data set used for evaluation.
+ * \param[in] example The examples that have to be evaluated.
+ * \param[out] leaves The leaves where the patch arrives
+ */
+ void
+ evaluate (pcl::DecisionForest<NodeType> & DecisionForestEvaluator,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator,
+ DataSet & data_set,
+ ExampleIndex example,
+ std::vector<NodeType> & leaves);
+
+ private:
+ /** \brief Evaluator for decision trees. */
+ DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType> tree_evaluator_;
+ };
+
+}
+
+#include <pcl/ml/impl/dt/decision_forest_evaluator.hpp>
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_DT_DECISION_FOREST_TRAINER_H_
+#define PCL_ML_DT_DECISION_FOREST_TRAINER_H_
+
+#include <pcl/common/common.h>
+
+#include <pcl/ml/dt/decision_forest.h>
+#include <pcl/ml/dt/decision_tree.h>
+#include <pcl/ml/dt/decision_tree_trainer.h>
+#include <pcl/ml/feature_handler.h>
+#include <pcl/ml/stats_estimator.h>
+
+#include <vector>
+
+namespace pcl
+{
+
+ /** \brief Trainer for decision trees. */
+ template <
+ class FeatureType,
+ class DataSet,
+ class LabelType,
+ class ExampleIndex,
+ class NodeType >
+ class PCL_EXPORTS DecisionForestTrainer
+ {
+
+ public:
+
+ /** \brief Constructor. */
+ DecisionForestTrainer ();
+ /** \brief Destructor. */
+ virtual
+ ~DecisionForestTrainer ();
+
+ /** \brief Sets the number of trees to train.
+ * \param[in] num_of_trees The number of trees.
+ */
+ inline void
+ setNumberOfTreesToTrain (const size_t num_of_trees)
+ {
+ num_of_trees_to_train_ = num_of_trees;
+ }
+
+ /** \brief Sets the feature handler used to create and evaluate features.
+ * \param[in] feature_handler The feature handler.
+ */
+ inline void
+ setFeatureHandler (pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler)
+ {
+ decision_tree_trainer_.setFeatureHandler (feature_handler);
+ }
+
+ /** \brief Sets the object for estimating the statistics for tree nodes.
+ * \param[in] stats_estimator The statistics estimator.
+ */
+ inline void
+ setStatsEstimator (pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator)
+ {
+ decision_tree_trainer_.setStatsEstimator (stats_estimator);
+ }
+
+ /** \brief Sets the maximum depth of the learned tree.
+ * \param[in] max_tree_depth Maximum depth of the learned tree.
+ */
+ inline void
+ setMaxTreeDepth (const size_t max_tree_depth)
+ {
+ decision_tree_trainer_.setMaxTreeDepth (max_tree_depth);
+ }
+
+ /** \brief Sets the number of features used to find optimal decision features.
+ * \param[in] num_of_features The number of features.
+ */
+ inline void
+ setNumOfFeatures (const size_t num_of_features)
+ {
+ decision_tree_trainer_.setNumOfFeatures (num_of_features);
+ }
+
+ /** \brief Sets the number of thresholds tested for finding the optimal decision threshold on the feature responses.
+ * \param[in] num_of_threshold The number of thresholds.
+ */
+ inline void
+ setNumOfThresholds (const size_t num_of_threshold)
+ {
+ decision_tree_trainer_.setNumOfThresholds (num_of_threshold);
+ }
+
+ /** \brief Sets the input data set used for training.
+ * \param[in] data_set The data set used for training.
+ */
+ inline void
+ setTrainingDataSet (DataSet & data_set)
+ {
+ decision_tree_trainer_.setTrainingDataSet (data_set);
+ }
+
+ /** \brief Example indices that specify the data used for training.
+ * \param[in] examples The examples.
+ */
+ inline void
+ setExamples (std::vector<ExampleIndex> & examples)
+ {
+ decision_tree_trainer_.setExamples (examples);
+ }
+
+ /** \brief Sets the label data corresponding to the example data.
+ * \param[in] label_data The label data.
+ */
+ inline void
+ setLabelData (std::vector<LabelType> & label_data)
+ {
+ decision_tree_trainer_.setLabelData (label_data);
+ }
+
+ /** \brief Sets the minimum number of examples to continue growing a tree.
+ * \param[in] n Number of examples
+ */
+ inline void
+ setMinExamplesForSplit(size_t n)
+ {
+ decision_tree_trainer_.setMinExamplesForSplit(n);
+ }
+
+ /** \brief Specify the thresholds to be used when evaluating features.
+ * \param[in] thres The threshold values.
+ */
+ void
+ setThresholds(std::vector<float> & thres)
+ {
+ decision_tree_trainer_.setThresholds(thres);
+ }
+
+ /** \brief Specify the data provider.
+ * \param[in] dtdp The data provider that should implement getDatasetAndLabels(...) function
+ */
+ void
+ setDecisionTreeDataProvider(boost::shared_ptr<pcl::DecisionTreeTrainerDataProvider<FeatureType, DataSet, LabelType, ExampleIndex, NodeType> > & dtdp)
+ {
+ decision_tree_trainer_.setDecisionTreeDataProvider(dtdp);
+ }
+
+ /** \brief Specify if the features are randomly generated at each split node.
+ * \param[in] b Do it or not.
+ */
+ void
+ setRandomFeaturesAtSplitNode(bool b)
+ {
+ decision_tree_trainer_.setRandomFeaturesAtSplitNode(b);
+ }
+
+ /** \brief Trains a decision forest using the set training data and settings.
+ * \param[out] forest Destination for the trained forest.
+ */
+ void
+ train (DecisionForest<NodeType> & forest);
+
+ private:
+
+ /** \brief The number of trees to train. */
+ size_t num_of_trees_to_train_;
+
+ /** \brief The trainer for the decision trees of the forest. */
+ pcl::DecisionTreeTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType> decision_tree_trainer_;
+
+ };
+
+}
+
+#include <pcl/ml/impl/dt/decision_forest_trainer.hpp>
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_DT_DECISION_TREE
+#define PCL_ML_DT_DECISION_TREE
+
+#include <pcl/common/common.h>
+
+#include <istream>
+#include <ostream>
+
+namespace pcl
+{
+
+ /** \brief Class representing a decision tree. */
+ template <class NodeType>
+ class PCL_EXPORTS DecisionTree
+ {
+
+ public:
+
+ /** \brief Constructor. */
+ DecisionTree () : root_ () {}
+ /** \brief Destructor. */
+ virtual
+ ~DecisionTree () {}
+
+ /** \brief Sets the root node of the tree.
+ * \param[in] root The root node.
+ */
+ void
+ setRoot (const NodeType & root)
+ {
+ root_ = root;
+ }
+
+ /** \brief Returns the root node of the tree. */
+ NodeType &
+ getRoot ()
+ {
+ return root_;
+ }
+
+ /** \brief Serializes the decision tree.
+ * \param[out] stream The destination for the serialization.
+ */
+ void
+ serialize (::std::ostream & stream) const
+ {
+ root_.serialize (stream);
+ }
+
+ /** \brief Deserializes the decision tree.
+ * \param[in] stream The source for the deserialization.
+ */
+ void deserialize (::std::istream & stream)
+ {
+ root_.deserialize (stream);
+ }
+
+ private:
+
+ /** \brief The root node of the decision tree. */
+ NodeType root_;
+
+ };
+
+
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef DECISION_TREE_DATA_PROVIDER_H_
+#define DECISION_TREE_DATA_PROVIDER_H_
+
+#include <pcl/common/common.h>
+
+namespace pcl
+{
+ template<class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+ class PCL_EXPORTS DecisionTreeTrainerDataProvider
+ {
+
+ /** \brief The training data set. */
+ DataSet data_set_;
+ /** \brief The label data. */
+ std::vector<LabelType> label_data_;
+
+ public:
+
+ /** \brief Constructor. */
+ DecisionTreeTrainerDataProvider()
+ {
+
+ }
+
+ /** \brief Destructor. */
+ ~DecisionTreeTrainerDataProvider()
+ {
+
+ }
+
+ /** \brief Virtual function called to obtain training examples and labels before training a specific tree */
+ virtual void
+ getDatasetAndLabels(DataSet & data_set, std::vector<LabelType> & label_data, std::vector<ExampleIndex> & examples) = 0;
+ };
+}
+
+#endif /* DECISION_TREE_DATA_PROVIDER_H_ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_DT_DECISION_TREE_EVALUATOR_H_
+#define PCL_ML_DT_DECISION_TREE_EVALUATOR_H_
+
+#include <pcl/common/common.h>
+
+#include <pcl/ml/dt/decision_tree.h>
+#include <pcl/ml/feature_handler.h>
+#include <pcl/ml/stats_estimator.h>
+
+#include <vector>
+
+namespace pcl
+{
+
+ /** \brief Utility class for evaluating a decision tree. */
+ template <
+ class FeatureType,
+ class DataSet,
+ class LabelType,
+ class ExampleIndex,
+ class NodeType >
+ class DecisionTreeEvaluator
+ {
+
+ public:
+
+ /** \brief Constructor. */
+ DecisionTreeEvaluator();
+ /** \brief Destructor. */
+ virtual
+ ~DecisionTreeEvaluator();
+
+ /** \brief Evaluates the specified examples using the supplied tree.
+ * \param[in] tree The decision tree.
+ * \param[in] feature_handler The feature handler used to train the tree.
+ * \param[in] stats_estimator The statistics estimation instance used while training the tree.
+ * \param[in] data_set The data set used for evaluation.
+ * \param[in] examples The examples that have to be evaluated.
+ * \param[out] label_data The destination for the resulting label data.
+ */
+ void
+ evaluate (pcl::DecisionTree<NodeType> & tree,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator,
+ DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<LabelType> & label_data);
+
+ /** \brief Evaluates the specified examples using the supplied tree and adds the results to the supplied results array.
+ * \param[in] tree The decision tree.
+ * \param[in] feature_handler The feature handler used to train the tree.
+ * \param[in] stats_estimator The statistics estimation instance used while training the tree.
+ * \param[in] data_set The data set used for evaluation.
+ * \param[in] examples The examples that have to be evaluated.
+ * \param[out] label_data The destination where the resulting label data is added to.
+ */
+ void
+ evaluateAndAdd (pcl::DecisionTree<NodeType> & tree,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator,
+ DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<LabelType> & label_data);
+
+ /** \brief Evaluates the specified examples using the supplied tree.
+ * \param[in] tree The decision tree.
+ * \param[in] feature_handler The feature handler used to train the tree.
+ * \param[in] stats_estimator The statistics estimation instance used while training the tree.
+ * \param[in] data_set The data set used for evaluation.
+ * \param[in] example The example that has to be evaluated.
+ * \param[out] leave The leave reached by the examples.
+ */
+ void
+ evaluate(pcl::DecisionTree<NodeType> & tree,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator,
+ DataSet & data_set,
+ ExampleIndex example,
+ NodeType & leave);
+
+ /** \brief Evaluates the specified examples using the supplied tree.
+ * \param[in] tree The decision tree.
+ * \param[in] feature_handler The feature handler used to train the tree.
+ * \param[in] stats_estimator The statistics estimation instance used while training the tree.
+ * \param[in] data_set The data set used for evaluation.
+ * \param[in] examples The examples that have to be evaluated.
+ * \param[out] nodes The leaf-nodes reached while evaluation.
+ */
+ void
+ getNodes (pcl::DecisionTree<NodeType> & tree,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator,
+ DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<NodeType*> & nodes);
+
+ };
+
+}
+
+#include <pcl/ml/impl/dt/decision_tree_evaluator.hpp>
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_DT_DECISION_TREE_TRAINER_H_
+#define PCL_ML_DT_DECISION_TREE_TRAINER_H_
+
+#include <pcl/common/common.h>
+
+#include <pcl/ml/dt/decision_tree.h>
+#include <pcl/ml/feature_handler.h>
+#include <pcl/ml/stats_estimator.h>
+#include <pcl/ml/dt/decision_tree_data_provider.h>
+
+#include <vector>
+
+namespace pcl
+{
+
+ /** \brief Trainer for decision trees. */
+ template <
+ class FeatureType,
+ class DataSet,
+ class LabelType,
+ class ExampleIndex,
+ class NodeType >
+ class PCL_EXPORTS DecisionTreeTrainer
+ {
+
+ public:
+
+ /** \brief Constructor. */
+ DecisionTreeTrainer ();
+ /** \brief Destructor. */
+ virtual
+ ~DecisionTreeTrainer ();
+
+ /** \brief Sets the feature handler used to create and evaluate features.
+ * \param[in] feature_handler The feature handler.
+ */
+ inline void
+ setFeatureHandler (pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler)
+ {
+ feature_handler_ = &feature_handler;
+ }
+
+ /** \brief Sets the object for estimating the statistics for tree nodes.
+ * \param[in] stats_estimator The statistics estimator.
+ */
+ inline void
+ setStatsEstimator (pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator)
+ {
+ stats_estimator_ = &stats_estimator;
+ }
+
+ /** \brief Sets the maximum depth of the learned tree.
+ * \param[in] max_tree_depth Maximum depth of the learned tree.
+ */
+ inline void
+ setMaxTreeDepth (const size_t max_tree_depth)
+ {
+ max_tree_depth_ = max_tree_depth;
+ }
+
+ /** \brief Sets the number of features used to find optimal decision features.
+ * \param[in] num_of_features The number of features.
+ */
+ inline void
+ setNumOfFeatures (const size_t num_of_features)
+ {
+ num_of_features_ = num_of_features;
+ }
+
+ /** \brief Sets the number of thresholds tested for finding the optimal decision threshold on the feature responses.
+ * \param[in] num_of_threshold The number of thresholds.
+ */
+ inline void
+ setNumOfThresholds (const size_t num_of_threshold)
+ {
+ num_of_thresholds_ = num_of_threshold;
+ }
+
+ /** \brief Sets the input data set used for training.
+ * \param[in] data_set The data set used for training.
+ */
+ inline void
+ setTrainingDataSet (DataSet & data_set)
+ {
+ data_set_ = data_set;
+ }
+
+ /** \brief Example indices that specify the data used for training.
+ * \param[in] examples The examples.
+ */
+ inline void
+ setExamples (std::vector<ExampleIndex> & examples)
+ {
+ examples_ = examples;
+ }
+
+ /** \brief Sets the label data corresponding to the example data.
+ * \param[in] label_data The label data.
+ */
+ inline void
+ setLabelData (std::vector<LabelType> & label_data)
+ {
+ label_data_ = label_data;
+ }
+
+ /** \brief Sets the minimum number of examples to continue growing a tree.
+ * \param[in] n Number of examples
+ */
+ inline void
+ setMinExamplesForSplit (size_t n)
+ {
+ min_examples_for_split_ = n;
+ }
+
+ /** \brief Specify the thresholds to be used when evaluating features.
+ * \param[in] thres The threshold values.
+ */
+ void
+ setThresholds (std::vector<float> & thres)
+ {
+ thresholds_ = thres;
+ }
+
+ /** \brief Specify the data provider.
+ * \param[in] dtdp The data provider that should implement getDatasetAndLabels(...) function
+ */
+ void
+ setDecisionTreeDataProvider (boost::shared_ptr<pcl::DecisionTreeTrainerDataProvider<FeatureType, DataSet, LabelType, ExampleIndex, NodeType> > & dtdp)
+ {
+ decision_tree_trainer_data_provider_ = dtdp;
+ }
+
+ /** \brief Specify if the features are randomly generated at each split node.
+ * \param[in] b Do it or not.
+ */
+ void
+ setRandomFeaturesAtSplitNode (bool b)
+ {
+ random_features_at_split_node_ = b;
+ }
+
+ /** \brief Trains a decision tree using the set training data and settings.
+ * \param[out] tree Destination for the trained tree.
+ */
+ void
+ train (DecisionTree<NodeType> & tree);
+
+ protected:
+
+ /** \brief Trains a decision tree node from the specified features, label data, and examples.
+ * \param[in] features The feature pool used for training.
+ * \param[in] examples The examples used for training.
+ * \param[in] label_data The label data corresponding to the examples.
+ * \param[in] max_depth The maximum depth of the remaining tree.
+ * \param[out] node The resulting node.
+ */
+ void
+ trainDecisionTreeNode (std::vector<FeatureType> & features,
+ std::vector<ExampleIndex> & examples,
+ std::vector<LabelType> & label_data,
+ size_t max_depth,
+ NodeType & node);
+
+ /** \brief Creates uniformely distrebuted thresholds over the range of the supplied values.
+ * \param[in] num_of_thresholds The number of thresholds to create.
+ * \param[in] values The values for estimating the expected value range.
+ * \param[out] thresholds The resulting thresholds.
+ */
+ static void
+ createThresholdsUniform (const size_t num_of_thresholds,
+ std::vector<float> & values,
+ std::vector<float> & thresholds);
+
+ private:
+
+ /** \brief Maximum depth of the learned tree. */
+ size_t max_tree_depth_;
+ /** \brief Number of features used to find optimal decision features. */
+ size_t num_of_features_;
+ /** \brief Number of thresholds. */
+ size_t num_of_thresholds_;
+
+ /** \brief FeatureHandler instance, responsible for creating and evaluating features. */
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> * feature_handler_;
+ /** \brief StatsEstimator instance, responsible for gathering stats about a node. */
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> * stats_estimator_;
+
+ /** \brief The training data set. */
+ DataSet data_set_;
+ /** \brief The label data. */
+ std::vector<LabelType> label_data_;
+ /** \brief The example data. */
+ std::vector<ExampleIndex> examples_;
+
+ /** \brief Minimum number of examples to split a node. */
+ size_t min_examples_for_split_;
+ /** \brief Thresholds to be used instead of generating uniform distributed thresholds. */
+ std::vector<float> thresholds_;
+ /** \brief The data provider which is called before training a specific tree, if pointer is NULL, then data_set_ is used. */
+ boost::shared_ptr<pcl::DecisionTreeTrainerDataProvider<FeatureType, DataSet, LabelType, ExampleIndex, NodeType> > decision_tree_trainer_data_provider_;
+ /** \brief If true, random features are generated at each node, otherwise, at start of training the tree */
+ bool random_features_at_split_node_;
+ };
+
+}
+
+#include <pcl/ml/impl/dt/decision_tree_trainer.hpp>
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_DT_FEATURE_HANDLER_H_
+#define PCL_ML_DT_FEATURE_HANDLER_H_
+
+#include <pcl/common/common.h>
+
+#include <vector>
+#include <ostream>
+
+namespace pcl
+{
+
+ /** \brief Utility class interface which is used for creating and evaluating features. */
+ template <
+ class FeatureType,
+ class DataSet,
+ class ExampleIndex >
+ class PCL_EXPORTS FeatureHandler
+ {
+ public:
+
+ /** \brief Destructor. */
+ virtual
+ ~FeatureHandler () {};
+
+ /** \brief Creates random features.
+ * \param[in] num_of_features The number of random features to create.
+ * \param[out] features The destination for the created features.
+ */
+ virtual void
+ createRandomFeatures (const size_t num_of_features, std::vector<FeatureType> & features) = 0;
+
+ /** \brief Evaluates a feature on the specified data.
+ * \param[in] feature The features to evaluate.
+ * \param[in] data_set The data set on which the feature is evaluated.
+ * \param[in] examples The examples which specify on which parts of the data set the feature is evaluated.
+ * \param[out] results The destination for the results of the feature evaluation.
+ * \param[out] flags Flags that are supplied together with the results.
+ */
+ virtual void
+ evaluateFeature (const FeatureType & feature,
+ DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<float> & results,
+ std::vector<unsigned char> & flags) const = 0;
+
+ /** \brief Evaluates a feature on the specified data.
+ * \param[in] feature The features to evaluate.
+ * \param[in] data_set The data set on which the feature is evaluated.
+ * \param[in] example The examples which specify on which parts of the data set the feature is evaluated.
+ * \param[out] result The destination for the results of the feature evaluation.
+ * \param[out] flag Flags that are supplied together with the results.
+ */
+ virtual void
+ evaluateFeature (const FeatureType & feature,
+ DataSet & data_set,
+ const ExampleIndex & example,
+ float & result,
+ unsigned char & flag) const = 0;
+
+ /** \brief Generates evaluation code for the specified feature and writes it to the specified stream.
+ * \param[in] feature The feature for which code is generated.
+ * \param[out] stream The destionation for the code.
+ */
+ virtual void
+ generateCodeForEvaluation (const FeatureType & feature,
+ ::std::ostream & stream) const = 0;
+
+ };
+
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_FERNS_FERN
+#define PCL_ML_FERNS_FERN
+
+#include <pcl/common/common.h>
+
+#include <istream>
+#include <ostream>
+
+namespace pcl
+{
+
+ /** \brief Class representing a Fern. */
+ template <class FeatureType, class NodeType>
+ class PCL_EXPORTS Fern
+ {
+ public:
+
+ /** \brief Constructor. */
+ Fern ()
+ : num_of_decisions_ (0)
+ , features_ (0)
+ , thresholds_ (0)
+ , nodes_ (1)
+ {}
+
+ /** \brief Destructor. */
+ virtual
+ ~Fern () {}
+
+ /** \brief Initializes the fern.
+ * \param num_of_decisions The number of decisions taken to access the nodes.
+ */
+ void
+ initialize (const size_t num_of_decisions)
+ {
+ num_of_decisions_ = num_of_decisions;
+ features_.resize (num_of_decisions_);
+ thresholds_.resize (num_of_decisions_, std::numeric_limits<float>::quiet_NaN ());
+ nodes_.resize (0x1 << num_of_decisions_);
+ }
+
+ /** \brief Returns the number of nodes the Fern has. */
+ inline size_t
+ getNumOfNodes ()
+ {
+ return 0x1U << num_of_decisions_;
+ }
+
+ /** \brief Returns the number of features the Fern has. */
+ inline size_t
+ getNumOfFeatures ()
+ {
+ return num_of_decisions_;
+ }
+
+ /** \brief Serializes the fern.
+ * \param[out] stream The destination for the serialization.
+ */
+ void
+ serialize (::std::ostream & stream) const
+ {
+ //const int tmp_value = static_cast<int> (num_of_decisions_);
+ //stream.write (reinterpret_cast<char*> (&tmp_value), sizeof (tmp_value));
+ stream.write (reinterpret_cast<const char*> (&num_of_decisions_), sizeof (num_of_decisions_));
+
+ for (size_t feature_index = 0; feature_index < features_.size (); ++feature_index)
+ {
+ features_[feature_index].serialize (stream);
+ }
+
+ for (size_t threshold_index = 0; threshold_index < thresholds_.size (); ++threshold_index)
+ {
+ stream.write (reinterpret_cast<const char*> (&(thresholds_[threshold_index])), sizeof (thresholds_[threshold_index]));
+ }
+
+ for (size_t node_index = 0; node_index < nodes_.size (); ++node_index)
+ {
+ nodes_[node_index].serialize (stream);
+ }
+ }
+
+ /** \brief Deserializes the fern.
+ * \param[in] stream The source for the deserialization.
+ */
+ void deserialize (::std::istream & stream)
+ {
+ stream.read (reinterpret_cast<char*> (&num_of_decisions_), sizeof (num_of_decisions_));
+
+ features_.resize (num_of_decisions_);
+ thresholds_.resize (num_of_decisions_);
+ nodes_.resize (0x1 << num_of_decisions_);
+
+ for (size_t feature_index = 0; feature_index < features_.size (); ++feature_index)
+ {
+ features_[feature_index].deserialize (stream);
+ }
+
+ for (size_t threshold_index = 0; threshold_index < thresholds_.size (); ++threshold_index)
+ {
+ stream.read (reinterpret_cast<char*> (&(thresholds_[threshold_index])), sizeof (thresholds_[threshold_index]));
+ }
+
+ for (size_t node_index = 0; node_index < nodes_.size (); ++node_index)
+ {
+ nodes_[node_index].deserialize (stream);
+ }
+ }
+
+ /** \brief Access operator for nodes.
+ * \param node_index The index of the node to access.
+ */
+ inline NodeType &
+ operator[] (const size_t node_index)
+ {
+ return nodes_[node_index];
+ }
+
+ /** \brief Access operator for nodes.
+ * \param node_index The index of the node to access.
+ */
+ inline const NodeType &
+ operator[] (const size_t node_index) const
+ {
+ return nodes_[node_index];
+ }
+
+ /** \brief Access operator for features.
+ * \param feature_index The index of the feature to access.
+ */
+ inline FeatureType &
+ accessFeature (const size_t feature_index)
+ {
+ return features_[feature_index];
+ }
+
+ /** \brief Access operator for features.
+ * \param feature_index The index of the feature to access.
+ */
+ inline const FeatureType &
+ accessFeature (const size_t feature_index) const
+ {
+ return features_[feature_index];
+ }
+
+ /** \brief Access operator for thresholds.
+ * \param threshold_index The index of the threshold to access.
+ */
+ inline float &
+ accessThreshold (const size_t threshold_index)
+ {
+ return thresholds_[threshold_index];
+ }
+
+ /** \brief Access operator for thresholds.
+ * \param threshold_index The index of the threshold to access.
+ */
+ inline const float &
+ accessThreshold (const size_t threshold_index) const
+ {
+ return thresholds_[threshold_index];
+ }
+
+ private:
+ /** The number of decisions. */
+ size_t num_of_decisions_;
+ /** The list of Features used to make the decisions. */
+ std::vector<FeatureType> features_;
+ /** The list of thresholds used to make the decisions. */
+ std::vector<float> thresholds_;
+
+ /** The list of Nodes accessed by the Fern. */
+ std::vector<NodeType> nodes_;
+ };
+
+
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_FERNS_FERN_EVALUATOR_H_
+#define PCL_ML_FERNS_FERN_EVALUATOR_H_
+
+#include <pcl/common/common.h>
+
+#include <pcl/ml/ferns/fern.h>
+#include <pcl/ml/feature_handler.h>
+#include <pcl/ml/stats_estimator.h>
+
+#include <vector>
+
+namespace pcl
+{
+
+ /** \brief Utility class for evaluating a fern. */
+ template <
+ class FeatureType,
+ class DataSet,
+ class LabelType,
+ class ExampleIndex,
+ class NodeType >
+ class FernEvaluator
+ {
+
+ public:
+
+ /** \brief Constructor. */
+ FernEvaluator();
+ /** \brief Destructor. */
+ virtual
+ ~FernEvaluator();
+
+ /** \brief Evaluates the specified examples using the supplied tree.
+ * \param[in] fern The decision tree.
+ * \param[in] feature_handler The feature handler used to train the tree.
+ * \param[in] stats_estimator The statistics estimation instance used while training the tree.
+ * \param[in] data_set The data set used for evaluation.
+ * \param[in] examples The examples that have to be evaluated.
+ * \param[out] label_data The destination for the resulting label data.
+ */
+ void
+ evaluate (pcl::Fern<FeatureType, NodeType> & fern,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator,
+ DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<LabelType> & label_data);
+
+ /** \brief Evaluates the specified examples using the supplied tree and adds the results to the supplied results array.
+ * \param[in] fern The decision tree.
+ * \param[in] feature_handler The feature handler used to train the tree.
+ * \param[in] stats_estimator The statistics estimation instance used while training the tree.
+ * \param[in] data_set The data set used for evaluation.
+ * \param[in] examples The examples that have to be evaluated.
+ * \param[out] label_data The destination where the resulting label data is added to.
+ */
+ void
+ evaluateAndAdd (pcl::Fern<FeatureType, NodeType> & fern,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator,
+ DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<LabelType> & label_data);
+
+ /** \brief Evaluates the specified examples using the supplied tree.
+ * \param[in] fern The decision tree.
+ * \param[in] feature_handler The feature handler used to train the tree.
+ * \param[in] stats_estimator The statistics estimation instance used while training the tree.
+ * \param[in] data_set The data set used for evaluation.
+ * \param[in] examples The examples that have to be evaluated.
+ * \param[out] nodes The leaf-nodes reached while evaluation.
+ */
+ void
+ getNodes (pcl::Fern<FeatureType, NodeType> & fern,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator,
+ DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<NodeType*> & nodes);
+
+ };
+
+}
+
+#include <pcl/ml/impl/ferns/fern_evaluator.hpp>
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_FERNS_FERN_TRAINER_H_
+#define PCL_ML_FERNS_FERN_TRAINER_H_
+
+#include <pcl/common/common.h>
+
+#include <pcl/ml/ferns/fern.h>
+#include <pcl/ml/feature_handler.h>
+#include <pcl/ml/stats_estimator.h>
+
+#include <vector>
+
+namespace pcl
+{
+
+ /** \brief Trainer for a Fern. */
+ template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+ class PCL_EXPORTS FernTrainer
+ {
+
+ public:
+
+ /** \brief Constructor. */
+ FernTrainer ();
+ /** \brief Destructor. */
+ virtual
+ ~FernTrainer ();
+
+ /** \brief Sets the feature handler used to create and evaluate features.
+ * \param[in] feature_handler The feature handler.
+ */
+ inline void
+ setFeatureHandler (pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler)
+ {
+ feature_handler_ = &feature_handler;
+ }
+
+ /** \brief Sets the object for estimating the statistics for tree nodes.
+ * \param[in] stats_estimator The statistics estimator.
+ */
+ inline void
+ setStatsEstimator (pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator)
+ {
+ stats_estimator_ = &stats_estimator;
+ }
+
+ /** \brief Sets the maximum depth of the learned tree.
+ * \param[in] fern_depth Maximum depth of the learned tree.
+ */
+ inline void
+ setFernDepth (const size_t fern_depth)
+ {
+ fern_depth_ = fern_depth;
+ }
+
+ /** \brief Sets the number of features used to find optimal decision features.
+ * \param[in] num_of_features The number of features.
+ */
+ inline void
+ setNumOfFeatures (const size_t num_of_features)
+ {
+ num_of_features_ = num_of_features;
+ }
+
+ /** \brief Sets the number of thresholds tested for finding the optimal decision threshold on the feature responses.
+ * \param[in] num_of_threshold The number of thresholds.
+ */
+ inline void
+ setNumOfThresholds (const size_t num_of_threshold)
+ {
+ num_of_thresholds_ = num_of_threshold;
+ }
+
+ /** \brief Sets the input data set used for training.
+ * \param[in] data_set The data set used for training.
+ */
+ inline void
+ setTrainingDataSet (DataSet & data_set)
+ {
+ data_set_ = data_set;
+ }
+
+ /** \brief Example indices that specify the data used for training.
+ * \param[in] examples The examples.
+ */
+ inline void
+ setExamples (std::vector<ExampleIndex> & examples)
+ {
+ examples_ = examples;
+ }
+
+ /** \brief Sets the label data corresponding to the example data.
+ * \param[in] label_data The label data.
+ */
+ inline void
+ setLabelData (std::vector<LabelType> & label_data)
+ {
+ label_data_ = label_data;
+ }
+
+ /** \brief Trains a decision tree using the set training data and settings.
+ * \param[out] fern Destination for the trained tree.
+ */
+ void
+ train (Fern<FeatureType, NodeType> & fern);
+
+ protected:
+
+ /** \brief Creates uniformely distrebuted thresholds over the range of the supplied values.
+ * \param[in] num_of_thresholds The number of thresholds to create.
+ * \param[in] values The values for estimating the expected value range.
+ * \param[out] thresholds The resulting thresholds.
+ */
+ static void
+ createThresholdsUniform (const size_t num_of_thresholds,
+ std::vector<float> & values,
+ std::vector<float> & thresholds);
+
+ private:
+
+ /** \brief Desired depth of the learned fern. */
+ size_t fern_depth_;
+ /** \brief Number of features used to find optimal decision features. */
+ size_t num_of_features_;
+ /** \brief Number of thresholds. */
+ size_t num_of_thresholds_;
+
+ /** \brief FeatureHandler instance, responsible for creating and evaluating features. */
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> * feature_handler_;
+ /** \brief StatsEstimator instance, responsible for gathering stats about a node. */
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> * stats_estimator_;
+
+ /** \brief The training data set. */
+ DataSet data_set_;
+ /** \brief The label data. */
+ std::vector<LabelType> label_data_;
+ /** \brief The example data. */
+ std::vector<ExampleIndex> examples_;
+
+ };
+
+}
+
+#include <pcl/ml/impl/ferns/fern_trainer.hpp>
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_DT_DECISION_FOREST_EVALUATOR_HPP_
+#define PCL_ML_DT_DECISION_FOREST_EVALUATOR_HPP_
+
+#include <pcl/common/common.h>
+
+#include <pcl/ml/dt/decision_forest_evaluator.h>
+#include <pcl/ml/dt/decision_forest.h>
+#include <pcl/ml/feature_handler.h>
+#include <pcl/ml/stats_estimator.h>
+
+#include <vector>
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+pcl::DecisionForestEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::DecisionForestEvaluator ()
+ : tree_evaluator_ ()
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+pcl::DecisionForestEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::~DecisionForestEvaluator ()
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+void
+pcl::DecisionForestEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::evaluate (
+ pcl::DecisionForest<NodeType> & forest,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator,
+ DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<LabelType> & label_data)
+{
+ const size_t num_of_examples = examples.size ();
+ label_data.resize (num_of_examples, 0);
+
+ for (size_t forest_index = 0; forest_index < forest.size (); ++forest_index)
+ {
+ tree_evaluator_.evaluateAndAdd (forest[forest_index], feature_handler, stats_estimator, data_set, examples, label_data);
+ }
+
+ const float inv_num_of_trees = 1.0f / static_cast<float> (forest.size ());
+ for (size_t label_index = 0; label_index < label_data.size (); ++label_index)
+ {
+ label_data[label_index] *= inv_num_of_trees;
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+void pcl::DecisionForestEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::evaluate (
+ pcl::DecisionForest<NodeType> & forest,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator,
+ DataSet & data_set,
+ ExampleIndex example,
+ std::vector<NodeType> & leaves)
+{
+ leaves.resize (forest.size ());
+ for (size_t forest_index = 0; forest_index < forest.size (); ++forest_index)
+ {
+ NodeType leave;
+ tree_evaluator_.evaluate (forest[forest_index], feature_handler, stats_estimator, data_set, example, leave);
+ leaves[forest_index] = leave;
+ }
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_DT_DECISION_FOREST_TRAINER_HPP_
+#define PCL_ML_DT_DECISION_FOREST_TRAINER_HPP_
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+pcl::DecisionForestTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::DecisionForestTrainer ()
+ : num_of_trees_to_train_ (1)
+ , decision_tree_trainer_ ()
+{
+
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+pcl::DecisionForestTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::~DecisionForestTrainer ()
+{
+
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+void
+pcl::DecisionForestTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::train (
+ pcl::DecisionForest<NodeType> & forest)
+{
+ for (size_t tree_index = 0; tree_index < num_of_trees_to_train_; ++tree_index)
+ {
+ pcl::DecisionTree<NodeType> tree;
+ decision_tree_trainer_.train (tree);
+
+ forest.push_back (tree);
+ }
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_DT_DECISION_TREE_EVALUATOR_HPP_
+#define PCL_ML_DT_DECISION_TREE_EVALUATOR_HPP_
+
+#include <pcl/common/common.h>
+
+#include <pcl/ml/dt/decision_tree.h>
+#include <pcl/ml/feature_handler.h>
+#include <pcl/ml/stats_estimator.h>
+
+#include <vector>
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+pcl::DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::DecisionTreeEvaluator ()
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+pcl::DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::~DecisionTreeEvaluator ()
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+void
+pcl::DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::evaluate (
+ pcl::DecisionTree<NodeType> & tree,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator,
+ DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<LabelType> & label_data)
+{
+ const size_t num_of_examples = examples.size ();
+ label_data.resize (num_of_examples);
+ for (int example_index = 0; example_index < num_of_examples; ++example_index)
+ {
+ NodeType * node = &(tree.getRoot ());
+
+ while (node->sub_nodes.size () != 0)
+ {
+ float feature_result = 0.0f;
+ unsigned char flag = 0;
+ unsigned char branch_index = 0;
+
+ feature_handler.evaluateFeature (node->feature, data_set, examples[example_index], feature_result, flag);
+ stats_estimator.computeBranchIndex (feature_result, flag, node->threshold, branch_index);
+
+ node = &(node->sub_nodes[branch_index]);
+ }
+
+ label_data[example_index] = stats_estimator.getLabelOfNode (*node);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+void
+pcl::DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::evaluateAndAdd (
+ pcl::DecisionTree<NodeType> & tree,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator,
+ DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<LabelType> & label_data)
+{
+ const size_t num_of_examples = examples.size ();
+ for (int example_index = 0; example_index < num_of_examples; ++example_index)
+ {
+ NodeType * node = &(tree.getRoot ());
+
+ while (node->sub_nodes.size () != 0)
+ {
+ float feature_result = 0.0f;
+ unsigned char flag = 0;
+ unsigned char branch_index = 0;
+
+ feature_handler.evaluateFeature (node->feature, data_set, examples[example_index], feature_result, flag);
+ stats_estimator.computeBranchIndex (feature_result, flag, node->threshold, branch_index);
+
+ node = &(node->sub_nodes[branch_index]);
+ }
+
+ label_data[example_index] += stats_estimator.getLabelOfNode (*node);
+ }
+}
+
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+void
+pcl::DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::evaluate (pcl::DecisionTree<NodeType> & tree,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator,
+ DataSet & data_set,
+ ExampleIndex example,
+ NodeType & leave)
+{
+
+ NodeType * node = &(tree.getRoot ());
+
+ while (node->sub_nodes.size () != 0)
+ {
+ float feature_result = 0.0f;
+ unsigned char flag = 0;
+ unsigned char branch_index = 0;
+
+ feature_handler.evaluateFeature (node->feature, data_set, example, feature_result, flag);
+ stats_estimator.computeBranchIndex (feature_result, flag, node->threshold, branch_index);
+
+ node = &(node->sub_nodes[branch_index]);
+ }
+
+ leave = *node;
+
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+void
+pcl::DecisionTreeEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::getNodes (
+ pcl::DecisionTree<NodeType> & tree,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator,
+ DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<NodeType*> & nodes)
+{
+ const size_t num_of_examples = examples.size ();
+ for (int example_index = 0; example_index < num_of_examples; ++example_index)
+ {
+ NodeType * node = &(tree.getRoot ());
+
+ while (node->sub_nodes.size () != 0)
+ {
+ float feature_result = 0.0f;
+ unsigned char flag = 0;
+ unsigned char branch_index = 0;
+
+ feature_handler.evaluateFeature (node->feature, data_set, examples[example_index], feature_result, flag);
+ stats_estimator.computeBranchIndex (feature_result, node->threshold, flag, branch_index);
+
+ node = &(node->subNodes[branch_index]);
+ }
+
+ nodes.push_back(node);
+ }
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_DT_DECISION_TREE_TRAINER_HPP_
+#define PCL_ML_DT_DECISION_TREE_TRAINER_HPP_
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+pcl::DecisionTreeTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::DecisionTreeTrainer ()
+ : max_tree_depth_ (15)
+ , num_of_features_ (1000)
+ , num_of_thresholds_ (10)
+ , feature_handler_ (NULL)
+ , stats_estimator_ (NULL)
+ , data_set_ ()
+ , label_data_ ()
+ , examples_ ()
+ , decision_tree_trainer_data_provider_ ()
+ , random_features_at_split_node_(false)
+{
+
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+pcl::DecisionTreeTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::~DecisionTreeTrainer ()
+{
+
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+void
+pcl::DecisionTreeTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::train (
+ pcl::DecisionTree<NodeType> & tree)
+{
+ // create random features
+ std::vector<FeatureType> features;
+
+ if (!random_features_at_split_node_)
+ feature_handler_->createRandomFeatures (num_of_features_, features);
+
+ // recursively build decision tree
+ NodeType root_node;
+ tree.setRoot (root_node);
+
+ if (decision_tree_trainer_data_provider_)
+ {
+ std::cerr << "use decision_tree_trainer_data_provider_" << std::endl;
+
+ decision_tree_trainer_data_provider_->getDatasetAndLabels (data_set_, label_data_, examples_);
+ trainDecisionTreeNode (features, examples_, label_data_, max_tree_depth_, tree.getRoot ());
+ label_data_.clear ();
+ data_set_.clear ();
+ examples_.clear ();
+ }
+ else
+ {
+ trainDecisionTreeNode (features, examples_, label_data_, max_tree_depth_, tree.getRoot ());
+ }
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+void
+pcl::DecisionTreeTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::trainDecisionTreeNode (
+ std::vector<FeatureType> & features,
+ std::vector<ExampleIndex> & examples,
+ std::vector<LabelType> & label_data,
+ const size_t max_depth,
+ NodeType & node)
+{
+ const size_t num_of_examples = examples.size ();
+ if (num_of_examples == 0)
+ {
+ PCL_ERROR ("Reached invalid point in decision tree training: Number of examples is 0!");
+ return;
+ };
+
+ if (max_depth == 0)
+ {
+ stats_estimator_->computeAndSetNodeStats(data_set_, examples, label_data, node);
+ return;
+ };
+
+ if(examples.size () < min_examples_for_split_) {
+ stats_estimator_->computeAndSetNodeStats (data_set_, examples, label_data, node);
+ return;
+ }
+
+ if(random_features_at_split_node_) {
+ features.clear ();
+ feature_handler_->createRandomFeatures (num_of_features_, features);
+ }
+
+ std::vector<float> feature_results;
+ std::vector<unsigned char> flags;
+
+ feature_results.reserve (num_of_examples);
+ flags.reserve (num_of_examples);
+
+ // find best feature for split
+ int best_feature_index = -1;
+ float best_feature_threshold = 0.0f;
+ float best_feature_information_gain = 0.0f;
+
+ const size_t num_of_features = features.size ();
+ for (size_t feature_index = 0; feature_index < num_of_features; ++feature_index)
+ {
+ // evaluate features
+ feature_handler_->evaluateFeature (features[feature_index],
+ data_set_,
+ examples,
+ feature_results,
+ flags );
+
+ // get list of thresholds
+ if (thresholds_.size () > 0)
+ {
+ // compute information gain for each threshold and store threshold with highest information gain
+ for (size_t threshold_index = 0; threshold_index < thresholds_.size (); ++threshold_index)
+ {
+
+ const float information_gain = stats_estimator_->computeInformationGain (data_set_,
+ examples,
+ label_data,
+ feature_results,
+ flags,
+ thresholds_[threshold_index]);
+
+ if (information_gain > best_feature_information_gain)
+ {
+ best_feature_information_gain = information_gain;
+ best_feature_index = static_cast<int> (feature_index);
+ best_feature_threshold = thresholds_[threshold_index];
+ }
+ }
+ }
+ else
+ {
+ std::vector<float> thresholds;
+ thresholds.reserve (num_of_thresholds_);
+ createThresholdsUniform (num_of_thresholds_, feature_results, thresholds);
+
+ // compute information gain for each threshold and store threshold with highest information gain
+ for (size_t threshold_index = 0; threshold_index < num_of_thresholds_; ++threshold_index)
+ {
+ const float threshold = thresholds[threshold_index];
+
+ // compute information gain
+ const float information_gain = stats_estimator_->computeInformationGain (data_set_,
+ examples,
+ label_data,
+ feature_results,
+ flags,
+ threshold);
+
+ if (information_gain > best_feature_information_gain)
+ {
+ best_feature_information_gain = information_gain;
+ best_feature_index = static_cast<int> (feature_index);
+ best_feature_threshold = threshold;
+ }
+ }
+ }
+ }
+
+ if (best_feature_index == -1)
+ {
+ stats_estimator_->computeAndSetNodeStats (data_set_, examples, label_data, node);
+ return;
+ }
+
+ // get branch indices for best feature and best threshold
+ std::vector<unsigned char> branch_indices;
+ branch_indices.reserve (num_of_examples);
+ {
+ feature_handler_->evaluateFeature (features[best_feature_index],
+ data_set_,
+ examples,
+ feature_results,
+ flags );
+
+ stats_estimator_->computeBranchIndices (feature_results,
+ flags,
+ best_feature_threshold,
+ branch_indices);
+ }
+
+ stats_estimator_->computeAndSetNodeStats (data_set_, examples, label_data, node);
+
+ // separate data
+ {
+ const size_t num_of_branches = stats_estimator_->getNumOfBranches ();
+
+ std::vector<size_t> branch_counts (num_of_branches, 0);
+ for (size_t example_index = 0; example_index < num_of_examples; ++example_index)
+ {
+ ++branch_counts[branch_indices[example_index]];
+ }
+
+ node.feature = features[best_feature_index];
+ node.threshold = best_feature_threshold;
+ node.sub_nodes.resize (num_of_branches);
+
+ for (size_t branch_index = 0; branch_index < num_of_branches; ++branch_index)
+ {
+ if (branch_counts[branch_index] == 0)
+ {
+ NodeType branch_node;
+ stats_estimator_->computeAndSetNodeStats (data_set_, examples, label_data, branch_node);
+ //branch_node->num_of_sub_nodes = 0;
+
+ node.sub_nodes[branch_index] = branch_node;
+
+ continue;
+ }
+
+ std::vector<LabelType> branch_labels;
+ std::vector<ExampleIndex> branch_examples;
+ branch_labels.reserve (branch_counts[branch_index]);
+ branch_examples.reserve (branch_counts[branch_index]);
+
+ for (size_t example_index = 0; example_index < num_of_examples; ++example_index)
+ {
+ if (branch_indices[example_index] == branch_index)
+ {
+ branch_examples.push_back (examples[example_index]);
+ branch_labels.push_back (label_data[example_index]);
+ }
+ }
+
+ trainDecisionTreeNode (features, branch_examples, branch_labels, max_depth-1, node.sub_nodes[branch_index]);
+ }
+ }
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+void
+pcl::DecisionTreeTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::createThresholdsUniform (
+ const size_t num_of_thresholds,
+ std::vector<float> & values,
+ std::vector<float> & thresholds)
+{
+ // estimate range of values
+ float min_value = ::std::numeric_limits<float>::max();
+ float max_value = -::std::numeric_limits<float>::max();
+
+ const size_t num_of_values = values.size ();
+ for (size_t value_index = 0; value_index < num_of_values; ++value_index)
+ {
+ const float value = values[value_index];
+
+ if (value < min_value) min_value = value;
+ if (value > max_value) max_value = value;
+ }
+
+ const float range = max_value - min_value;
+ const float step = range / static_cast<float>(num_of_thresholds+2);
+
+ // compute thresholds
+ thresholds.resize (num_of_thresholds);
+
+ for (size_t threshold_index = 0; threshold_index < num_of_thresholds; ++threshold_index)
+ {
+ thresholds[threshold_index] = min_value + step*(static_cast<float>(threshold_index+1));
+ }
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_FERNS_FERN_EVALUATOR_HPP_
+#define PCL_ML_FERNS_FERN_EVALUATOR_HPP_
+
+#include <pcl/common/common.h>
+
+#include <pcl/ml/ferns/fern.h>
+#include <pcl/ml/feature_handler.h>
+#include <pcl/ml/stats_estimator.h>
+
+#include <vector>
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+pcl::FernEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::FernEvaluator ()
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+pcl::FernEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::~FernEvaluator ()
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+void
+pcl::FernEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::evaluate (
+ pcl::Fern<FeatureType, NodeType> & fern,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator,
+ DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<LabelType> & label_data)
+{
+ const size_t num_of_examples = examples.size ();
+ const size_t num_of_branches = stats_estimator.getNumOfBranches ();
+ const size_t num_of_features = fern.getNumOfFeatures ();
+
+ label_data.resize (num_of_examples);
+
+ std::vector<std::vector<float> > results (num_of_features);
+ std::vector<std::vector<unsigned char> > flags (num_of_features);
+ std::vector<std::vector<unsigned char> > branch_indices (num_of_features);
+
+ for (size_t feature_index = 0; feature_index < num_of_features; ++feature_index)
+ {
+ results[feature_index].reserve (num_of_examples);
+ flags[feature_index].reserve (num_of_examples);
+ branch_indices[feature_index].reserve (num_of_examples);
+
+ feature_handler.evaluateFeature (fern.accessFeature (feature_index), data_set, examples, results[feature_index], flags[feature_index]);
+ stats_estimator.computeBranchIndices (results[feature_index], flags[feature_index], fern.accessThreshold (feature_index), branch_indices[feature_index]);
+ }
+
+ for (size_t example_index = 0; example_index < num_of_examples; ++example_index)
+ {
+ size_t node_index = 0;
+ for (size_t feature_index = 0; feature_index < num_of_features; ++feature_index)
+ {
+ node_index *= num_of_branches;
+ node_index += branch_indices[feature_index][example_index];
+ }
+
+ label_data[example_index] = stats_estimator.getLabelOfNode (fern[node_index]);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+void
+pcl::FernEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::evaluateAndAdd (
+ pcl::Fern<FeatureType, NodeType> & fern,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator,
+ DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<LabelType> & label_data)
+{
+ const size_t num_of_examples = examples.size ();
+ const size_t num_of_branches = stats_estimator.getNumOfBranches ();
+ const size_t num_of_features = fern.getNumOfFeatures ();
+
+ std::vector<std::vector<float> > results (num_of_features);
+ std::vector<std::vector<unsigned char> > flags (num_of_features);
+ std::vector<std::vector<unsigned char> > branch_indices (num_of_features);
+
+ for (size_t feature_index = 0; feature_index < num_of_features; ++feature_index)
+ {
+ results[feature_index].reserve (num_of_examples);
+ flags[feature_index].reserve (num_of_examples);
+ branch_indices[feature_index].reserve (num_of_examples);
+
+ feature_handler.evaluateFeature (fern.accessFeature (feature_index), data_set, examples, results[feature_index], flags[feature_index]);
+ stats_estimator.computeBranchIndices (results[feature_index], flags[feature_index], fern.accessThreshold (feature_index), branch_indices[feature_index]);
+ }
+
+ for (size_t example_index = 0; example_index < num_of_examples; ++example_index)
+ {
+ size_t node_index = 0;
+ for (size_t feature_index = 0; feature_index < num_of_features; ++feature_index)
+ {
+ node_index *= num_of_branches;
+ node_index += branch_indices[feature_index][example_index];
+ }
+
+ label_data[example_index] = stats_estimator.getLabelOfNode (fern[node_index]);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+void
+pcl::FernEvaluator<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::getNodes (
+ pcl::Fern<FeatureType, NodeType> & fern,
+ pcl::FeatureHandler<FeatureType, DataSet, ExampleIndex> & feature_handler,
+ pcl::StatsEstimator<LabelType, NodeType, DataSet, ExampleIndex> & stats_estimator,
+ DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<NodeType*> & nodes)
+{
+ const size_t num_of_examples = examples.size ();
+ const size_t num_of_branches = stats_estimator.getNumOfBranches ();
+ const size_t num_of_features = fern.getNumOfFeatures ();
+
+ nodes.reserve (num_of_examples);
+
+ std::vector<std::vector<float> > results (num_of_features);
+ std::vector<std::vector<unsigned char> > flags (num_of_features);
+ std::vector<std::vector<unsigned char> > branch_indices (num_of_features);
+
+ for (size_t feature_index = 0; feature_index < num_of_features; ++feature_index)
+ {
+ results[feature_index].reserve (num_of_examples);
+ flags[feature_index].reserve (num_of_examples);
+ branch_indices[feature_index].reserve (num_of_examples);
+
+ feature_handler.evaluateFeature (fern.accessFeature (feature_index), data_set, examples, results[feature_index], flags[feature_index]);
+ stats_estimator.computeBranchIndices (results[feature_index], flags[feature_index], fern.accessThreshold (feature_index), branch_indices[feature_index]);
+ }
+
+ for (size_t example_index = 0; example_index < num_of_examples; ++example_index)
+ {
+ size_t node_index = 0;
+ for (size_t feature_index = 0; feature_index < num_of_features; ++feature_index)
+ {
+ node_index *= num_of_branches;
+ node_index += branch_indices[feature_index][example_index];
+ }
+
+ nodes.push_back (&(fern[node_index]));
+ }
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_FERNS_FERN_TRAINER_HPP_
+#define PCL_ML_FERNS_FERN_TRAINER_HPP_
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+pcl::FernTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::FernTrainer ()
+ : fern_depth_ (10)
+ , num_of_features_ (1000)
+ , num_of_thresholds_ (10)
+ , feature_handler_ (NULL)
+ , stats_estimator_ (NULL)
+ , data_set_ ()
+ , label_data_ ()
+ , examples_ ()
+{
+
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+pcl::FernTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::~FernTrainer ()
+{
+
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+void
+pcl::FernTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::train (
+ pcl::Fern<FeatureType, NodeType> & fern)
+{
+ const size_t num_of_branches = stats_estimator_->getNumOfBranches ();
+ const size_t num_of_examples = examples_.size ();
+
+ // create random features
+ std::vector<FeatureType> features;
+ feature_handler_->createRandomFeatures (num_of_features_, features);
+
+ // setup fern
+ fern.initialize (fern_depth_);
+
+ // evaluate all features
+ std::vector<std::vector<float> > feature_results (num_of_features_);
+ std::vector<std::vector<unsigned char> > flags (num_of_features_);
+
+ for (size_t feature_index = 0; feature_index < num_of_features_; ++feature_index)
+ {
+ feature_results[feature_index].reserve (num_of_examples);
+ flags[feature_index].reserve (num_of_examples);
+
+ feature_handler_->evaluateFeature (features[feature_index],
+ data_set_,
+ examples_,
+ feature_results[feature_index],
+ flags[feature_index] );
+ }
+
+ // iteratively select features and thresholds
+ std::vector<std::vector<std::vector<float> > > branch_feature_results (num_of_features_); // [feature_index][branch_index][result_index]
+ std::vector<std::vector<std::vector<unsigned char> > > branch_flags (num_of_features_); // [feature_index][branch_index][flag_index]
+ std::vector<std::vector<std::vector<ExampleIndex> > > branch_examples (num_of_features_); // [feature_index][branch_index][result_index]
+ std::vector<std::vector<std::vector<LabelType> > > branch_label_data (num_of_features_); // [feature_index][branch_index][flag_index]
+
+ // - initialize branch feature results and flags
+ for (size_t feature_index = 0; feature_index < num_of_features_; ++feature_index)
+ {
+ branch_feature_results[feature_index].resize (1);
+ branch_flags[feature_index].resize (1);
+ branch_examples[feature_index].resize (1);
+ branch_label_data[feature_index].resize (1);
+
+ branch_feature_results[feature_index][0] = feature_results[feature_index];
+ branch_flags[feature_index][0] = flags[feature_index];
+ branch_examples[feature_index][0] = examples_;
+ branch_label_data[feature_index][0] = label_data_;
+ }
+
+ for (size_t depth_index = 0; depth_index < fern_depth_; ++depth_index)
+ {
+ // get thresholds
+ std::vector<std::vector<float> > thresholds (num_of_features_);
+
+ for (size_t feature_index = 0; feature_index < num_of_features_; ++feature_index)
+ {
+ thresholds.reserve (num_of_thresholds_);
+ createThresholdsUniform (num_of_thresholds_, feature_results[feature_index], thresholds[feature_index]);
+ }
+
+ // compute information gain
+ int best_feature_index = -1;
+ float best_feature_threshold = 0.0f;
+ float best_feature_information_gain = 0.0f;
+
+ for (size_t feature_index = 0; feature_index < num_of_features_; ++feature_index)
+ {
+ for (size_t threshold_index = 0; threshold_index < num_of_thresholds_; ++threshold_index)
+ {
+ float information_gain = 0.0f;
+ for (size_t branch_index = 0; branch_index < branch_feature_results[feature_index].size (); ++branch_index)
+ {
+ const float branch_information_gain = stats_estimator_->computeInformationGain (data_set_,
+ branch_examples[feature_index][branch_index],
+ branch_label_data[feature_index][branch_index],
+ branch_feature_results[feature_index][branch_index],
+ branch_flags[feature_index][branch_index],
+ thresholds[feature_index][threshold_index]);
+
+ information_gain += branch_information_gain * branch_feature_results[feature_index][branch_index].size ();
+ }
+
+ if (information_gain > best_feature_information_gain)
+ {
+ best_feature_information_gain = information_gain;
+ best_feature_index = static_cast<int> (feature_index);
+ best_feature_threshold = thresholds[feature_index][threshold_index];
+ }
+ }
+ }
+
+ // add feature to the feature list of the fern
+ fern.accessFeature (depth_index) = features[best_feature_index];
+ fern.accessThreshold (depth_index) = best_feature_threshold;
+
+ // update branch feature results and flags
+ for (size_t feature_index = 0; feature_index < num_of_features_; ++feature_index)
+ {
+ std::vector<std::vector<float> > & cur_branch_feature_results = branch_feature_results[feature_index];
+ std::vector<std::vector<unsigned char> > & cur_branch_flags = branch_flags[feature_index];
+ std::vector<std::vector<ExampleIndex> > & cur_branch_examples = branch_examples[feature_index];
+ std::vector<std::vector<LabelType> > & cur_branch_label_data = branch_label_data[feature_index];
+
+ const size_t total_num_of_new_branches = num_of_branches * cur_branch_feature_results.size ();
+
+ std::vector<std::vector<float> > new_branch_feature_results (total_num_of_new_branches); // [branch_index][example_index]
+ std::vector<std::vector<unsigned char> > new_branch_flags (total_num_of_new_branches); // [branch_index][example_index]
+ std::vector<std::vector<ExampleIndex> > new_branch_examples (total_num_of_new_branches); // [branch_index][example_index]
+ std::vector<std::vector<LabelType> > new_branch_label_data (total_num_of_new_branches); // [branch_index][example_index]
+
+ for (size_t branch_index = 0; branch_index < cur_branch_feature_results.size (); ++branch_index)
+ {
+ const size_t num_of_examples_in_this_branch = cur_branch_feature_results[branch_index].size ();
+
+ std::vector<unsigned char> branch_indices;
+ branch_indices.reserve (num_of_examples_in_this_branch);
+
+ stats_estimator_->computeBranchIndices (cur_branch_feature_results[branch_index],
+ cur_branch_flags[branch_index],
+ best_feature_threshold,
+ branch_indices);
+
+ // split results into different branches
+ const size_t base_branch_index = branch_index * num_of_branches;
+ for (size_t example_index = 0; example_index < num_of_examples_in_this_branch; ++example_index)
+ {
+ const size_t combined_branch_index = base_branch_index + branch_indices[example_index];
+
+ new_branch_feature_results[combined_branch_index].push_back (cur_branch_feature_results[branch_index][example_index]);
+ new_branch_flags[combined_branch_index].push_back (cur_branch_flags[branch_index][example_index]);
+ new_branch_examples[combined_branch_index].push_back (cur_branch_examples[branch_index][example_index]);
+ new_branch_label_data[combined_branch_index].push_back (cur_branch_label_data[branch_index][example_index]);
+ }
+ }
+
+ branch_feature_results[feature_index] = new_branch_feature_results;
+ branch_flags[feature_index] = new_branch_flags;
+ branch_examples[feature_index] = new_branch_examples;
+ branch_label_data[feature_index] = new_branch_label_data;
+ }
+ }
+
+ // set node statistics
+ // - re-evaluate selected features
+ std::vector<std::vector<float> > final_feature_results (fern_depth_); // [feature_index][example_index]
+ std::vector<std::vector<unsigned char> > final_flags (fern_depth_); // [feature_index][example_index]
+ std::vector<std::vector<unsigned char> > final_branch_indices (fern_depth_); // [feature_index][example_index]
+ for (size_t depth_index = 0; depth_index < fern_depth_; ++depth_index)
+ {
+ final_feature_results[depth_index].reserve (num_of_examples);
+ final_flags[depth_index].reserve (num_of_examples);
+ final_branch_indices[depth_index].reserve (num_of_examples);
+
+ feature_handler_->evaluateFeature (fern.accessFeature (depth_index),
+ data_set_,
+ examples_,
+ final_feature_results[depth_index],
+ final_flags[depth_index] );
+
+ stats_estimator_->computeBranchIndices (final_feature_results[depth_index],
+ final_flags[depth_index],
+ fern.accessThreshold (depth_index),
+ final_branch_indices[depth_index]);
+ }
+
+ // - distribute examples to nodes
+ std::vector<std::vector<LabelType> > node_labels (0x1 << fern_depth_); // [node_index][example_index]
+ std::vector<std::vector<ExampleIndex> > node_examples (0x1 << fern_depth_); // [node_index][example_index]
+
+ for (size_t example_index = 0; example_index < num_of_examples; ++example_index)
+ {
+ size_t node_index = 0;
+ for (size_t depth_index = 0; depth_index < fern_depth_; ++depth_index)
+ {
+ node_index *= num_of_branches;
+ node_index += final_branch_indices[depth_index][example_index];
+ }
+
+ node_labels[node_index].push_back (label_data_[example_index]);
+ node_examples[node_index].push_back (examples_[example_index]);
+ }
+
+ // - compute and set statistics for every node
+ const size_t num_of_nodes = 0x1 << fern_depth_;
+ for (size_t node_index = 0; node_index < num_of_nodes; ++node_index)
+ {
+ stats_estimator_->computeAndSetNodeStats (data_set_, node_examples[node_index], node_labels[node_index], fern[node_index]);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+void
+pcl::FernTrainer<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::createThresholdsUniform (
+ const size_t num_of_thresholds,
+ std::vector<float> & values,
+ std::vector<float> & thresholds)
+{
+ // estimate range of values
+ float min_value = ::std::numeric_limits<float>::max();
+ float max_value = -::std::numeric_limits<float>::max();
+
+ const size_t num_of_values = values.size ();
+ for (int value_index = 0; value_index < num_of_values; ++value_index)
+ {
+ const float value = values[value_index];
+
+ if (value < min_value) min_value = value;
+ if (value > max_value) max_value = value;
+ }
+
+ const float range = max_value - min_value;
+ const float step = range / (num_of_thresholds+2);
+
+ // compute thresholds
+ thresholds.resize (num_of_thresholds);
+
+ for (int threshold_index = 0; threshold_index < num_of_thresholds; ++threshold_index)
+ {
+ thresholds[threshold_index] = min_value + step*(threshold_index+1);
+ }
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author : Christian Potthast
+ * Email : potthast@usc.edu
+ *
+ */
+
+#ifndef PCL_KMEANS_HPP_
+#define PCL_KMEANS_HPP_
+
+#include <pcl/ml/kmeans.h>
+
+//#include <pcl/common/io.h>
+
+//#include <stdio.h>
+//#include <stdlib.h>
+//#include <time.h>
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT>
+pcl::Kmeans<PointT>::Kmeans ()
+ : cluster_field_name_ ("")
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT>
+pcl::Kmeans<PointT>::~Kmeans ()
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::Kmeans<PointT>::k_means ()
+{
+}
+
+template <typename PointT> void
+pcl::Kmeans<PointT>::cluster (std::vector<PointIndices> &clusters)
+{
+ if (!initCompute () ||
+ (input_ != 0 && input_->points.empty ()) ||
+ (indices_ != 0 && indices_->empty ()))
+ {
+ clusters.clear ();
+ return;
+ }
+
+ pcl::PointCloud <PointT> point;
+ std::vector<pcl::PCLPointField> fields;
+
+ int user_index = -1;
+ // if no cluster field name is set, check for X Y Z
+ if (strcmp (cluster_field_name_.c_str (), "") == 0)
+ {
+ int x_index = -1;
+ int y_index = -1;
+ int z_index = -1;
+ x_index = pcl::getFieldIndex (point, "x", fields);
+ if (y_index != -1)
+ y_index = pcl::getFieldIndex (point, "y", fields);
+ if (z_index != -1)
+ z_index = pcl::getFieldIndex (point, "z", fields);
+
+ if (x_index == -1 && y_index == -1 && z_index == -1)
+ {
+ PCL_ERROR ("Failed to find match for field 'x y z'\n" );
+ return;
+ }
+
+ PCL_INFO ("Use X Y Z as input data\n");
+ // create input data
+/*
+ for (size_t i = 0; i < input_->points.size (); i++)
+ {
+ DataPoint data (3);
+ data[0] = input_->points[i].data[0];
+
+
+
+ }
+*/
+
+ std::cout << "x index: " << x_index << std::endl;
+
+ float x = 0.0;
+ memcpy (&x, &input_->points[0] + fields[x_index].offset, sizeof(float));
+
+ std::cout << "xxx: " << x << std::endl;
+
+
+ //memcpy (&x, reinterpret_cast<float*> (&input_->points[0]) + x_index, sizeof (float));
+
+
+ //int rgba_index = 1;
+
+ //pcl::RGB rgb;
+ //memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[index_vector[i].cloud_point_index]) + rgba_index, sizeof (RGB));
+
+
+
+ }
+ // if cluster field name is set, check if field name is valied
+ else
+ {
+ user_index = pcl::getFieldIndex (point, cluster_field_name_.c_str (), fields);
+
+ if (user_index == -1)
+ {
+ PCL_ERROR ("Failed to find match for field '%s'\n", cluster_field_name_.c_str ());
+ return;
+ }
+ }
+
+
+
+
+/*
+ int xyz_index = -1;
+ pcl::PointCloud <PointT> point;
+ xyz_index = pcl::getFieldIndex (point, "r", fields);
+
+
+ if (xyz_index == -1 && strcmp (cluster_field_name_.c_str (), "") == 0)
+ {
+ PCL_ERROR ("Failed to find match for field '%s'\n", cluster_field_name_.c_str ());
+ }
+
+
+ std::cout << "index: " << xyz_index << std::endl;
+
+ std::string t = pcl::getFieldsList (point);
+ std::cout << "t: " << t << std::endl;
+*/
+
+ //std::vector <pcl::PCLPointField> fields;
+ //pcl::getFieldIndex (*input_, "xyz", fields);
+
+
+ //std::cout << "field: " << fields[xyz_index].count << std::endl;
+
+
+/*
+ for (size_t i = 0; i < fields[vfh_idx].count; ++i)
+ {
+
+ //vfh.second[i] = point.points[0].histogram[i];
+
+ }
+*/
+
+
+
+ deinitCompute ();
+}
+
+
+
+
+#define PCL_INSTANTIATE_Kmeans(T) template class PCL_EXPORTS pcl::Kmeans<T>;
+
+#endif // PCL_KMEANS_HPP_
--- /dev/null
+ /*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ * Copyright (c) 2000-2012 Chih-Chung Chang and Chih-Jen Lin
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of copyright holders nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+ #ifndef PCL_SVM_WRAPPER_IMPL_H_
+ #define PCL_SVM_WRAPPER_IMPL_H_
+
+ #include <pcl/ml/svm_wrapper.h>
+
+ #endif // PCL_SVM_WRAPPER_H_
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author : Christian Potthast
+ * Email : potthast@usc.edu
+ *
+ */
+
+#ifndef PCL_KMEANS_H_
+#define PCL_KMEANS_H_
+
+#include <set>
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
+#include <pcl/common/io.h>
+
+#include <pcl/pcl_base.h>
+
+namespace pcl
+{
+ /** \brief @b K-means clustering.
+ * \author Christian Potthast
+ * \ingroup ML
+ */
+ //template <typename PointT>
+ //class Kmeans : public PCLBase<PointT>
+ class PCL_EXPORTS Kmeans
+ {
+/*
+ typedef PCLBase<PointT> BasePCLBase;
+
+ public:
+ typedef pcl::PointCloud<PointT> PointCloud;
+ typedef typename PointCloud::Ptr PointCloudPtr;
+ typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+ typedef PointIndices::Ptr PointIndicesPtr;
+ typedef PointIndices::ConstPtr PointIndicesConstPtr;
+*/
+
+ public:
+
+ typedef unsigned int PointId; // the id of this point
+ typedef unsigned int ClusterId; // the id of this cluster
+
+
+ //typedef std::vector<Coord> Point; // a point (a centroid)
+
+ typedef std::set<PointId> SetPoints; // set of points
+
+ typedef std::vector<float> Point;
+
+ // ClusterId -> (PointId, PointId, PointId, .... )
+ typedef std::vector<SetPoints> ClustersToPoints;
+ // PointId -> ClusterId
+ typedef std::vector<ClusterId> PointsToClusters;
+ // coll of centroids
+ typedef std::vector<Point> Centroids;
+
+
+ /** \brief Empty constructor. */
+ Kmeans (unsigned int num_points, unsigned int num_dimensions);
+
+ /** \brief This destructor destroys
+ *
+ */
+ ~Kmeans ();
+
+ /** \brief This method sets the k-means cluster size.
+ * \param[in] k number of clusters
+ */
+ void
+ setClusterSize (unsigned int k) {num_clusters_ = k;};
+
+/*
+ void
+ setClusterField (std::string field_name)
+ {
+ cluster_field_name_ = field_name;
+ };
+*/
+
+ //void
+ //getClusterCentroids (PointT &out);
+
+ //void
+ //cluster (std::vector<PointIndices> &clusters);
+
+ void
+ kMeans ();
+
+ void
+ setInputData (std::vector<Point> &data)
+ {
+ if (num_points_ != data.size ())
+ std::cout << "Data vector not the same" << std::endl;
+
+ data_ = data;
+ }
+
+ void
+ addDataPoint (Point &data_point)
+ {
+ if (num_dimensions_ != data_point.size ())
+ std::cout << "Dimensions not the same" << std::endl;
+
+
+ data_.push_back (data_point);
+ }
+
+ // Initial partition points among available clusters
+ void initialClusterPoints();
+
+ void
+ computeCentroids();
+
+ // distance between two points
+ float distance(const Point& x, const Point& y)
+ {
+ float total = 0.0;
+ float diff;
+
+ Point::const_iterator cpx=x.begin();
+ Point::const_iterator cpy=y.begin();
+ Point::const_iterator cpx_end=x.end();
+ for(;cpx!=cpx_end;++cpx,++cpy){
+ diff = *cpx - *cpy;
+ total += (diff * diff);
+ }
+ return total; // no need to take sqrt, which is monotonic
+ }
+
+
+ Centroids get_centroids (){return centroids_;}
+
+
+ protected:
+ // Members derived from the base class
+/*
+ using BasePCLBase::input_;
+ using BasePCLBase::indices_;
+ using BasePCLBase::initCompute;
+ using BasePCLBase::deinitCompute;
+*/
+
+ unsigned int num_points_;
+ unsigned int num_dimensions_;
+
+
+ /** \brief The number of clusters. */
+ unsigned int num_clusters_;
+
+ /** \brief The cluster centroids. */
+ //std::vector
+
+ //std::string cluster_field_name_;
+
+ // one data point
+
+ // all data points
+ std::vector<Point> data_;
+
+ ClustersToPoints clusters_to_points_;
+ PointsToClusters points_to_clusters_;
+ Centroids centroids_;
+
+
+
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_MULTI_CHANNEL_2D_COMPARISON_FEATURE_H_
+#define PCL_ML_MULTI_CHANNEL_2D_COMPARISON_FEATURE_H_
+
+#include <pcl/common/common.h>
+
+#include <istream>
+#include <ostream>
+
+namespace pcl
+{
+
+ /** \brief Feature for comparing two sample points in 2D multi-channel data. */
+ template <class PointT>
+ class PCL_EXPORTS MultiChannel2DComparisonFeature
+ {
+ public:
+ /** \brief Constructor. */
+ MultiChannel2DComparisonFeature () : p1 (), p2 (), channel (0) {}
+ /** \brief Destructor. */
+ virtual ~MultiChannel2DComparisonFeature () {}
+
+ /** \brief Serializes the feature to a stream.
+ * \param[out] stream The destination for the serialization.
+ */
+ inline void
+ serialize (std::ostream & stream) const
+ {
+ p1.serialize (stream);
+ p2.serialize (stream);
+ stream.write (reinterpret_cast<const char*> (&channel), sizeof (channel));
+ }
+
+ /** \brief Deserializes the feature from a stream.
+ * \param[in] stream The source for the deserialization.
+ */
+ inline void
+ deserialize (std::istream & stream)
+ {
+ p1.deserialize (stream);
+ p2.deserialize (stream);
+ stream.read (reinterpret_cast<char*> (&channel), sizeof (channel));
+ }
+
+ public:
+ /** \brief First sample point. */
+ PointT p1;
+ /** \brief Second sample point. */
+ PointT p2;
+
+ /** \brief Specifies which channel is used for comparison. */
+ unsigned char channel;
+ };
+
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_MULTI_CHANNEL_2D_COMPARISON_FEATURE_HANDLER_H_
+#define PCL_ML_MULTI_CHANNEL_2D_COMPARISON_FEATURE_HANDLER_H_
+
+#include <pcl/common/common.h>
+
+#include <pcl/ml/feature_handler.h>
+#include <pcl/ml/multi_channel_2d_data_set.h>
+#include <pcl/ml/multi_channel_2d_comparison_feature.h>
+#include <pcl/ml/multiple_data_2d_example_index.h>
+#include <pcl/ml/point_xy_32i.h>
+#include <pcl/ml/point_xy_32f.h>
+
+#include <istream>
+#include <ostream>
+
+namespace pcl
+{
+
+ /** \brief Feature utility class that handles the creation and evaluation of RGBD comparison features. */
+ template <class DATA_TYPE, size_t NUM_OF_CHANNELS>
+ class PCL_EXPORTS MultiChannel2DComparisonFeatureHandler
+ : public pcl::FeatureHandler<pcl::MultiChannel2DComparisonFeature<pcl::PointXY32i>, pcl::MultiChannel2DDataSet<DATA_TYPE, NUM_OF_CHANNELS>, pcl::MultipleData2DExampleIndex>
+ {
+
+ public:
+
+ /** \brief Constructor. */
+ MultiChannel2DComparisonFeatureHandler (
+ const int feature_window_width,
+ const int feature_window_height)
+ : feature_window_width_ (feature_window_width), feature_window_height_ (feature_window_height)
+ {}
+ /** \brief Destructor. */
+ virtual ~MultiChannel2DComparisonFeatureHandler () {}
+
+ /** \brief Sets the feature window size.
+ * \param[in] width The width of the feature window.
+ * \param[in] height The height of the feature window.
+ */
+ inline void
+ setFeatureWindowSize (
+ int width,
+ int height)
+ {
+ feature_window_width_ = width;
+ feature_window_height_ = height;
+ }
+
+ /** \brief Creates random features.
+ * \param[in] num_of_features The number of random features to create.
+ * \param[out] features The destination for the created random features.
+ */
+ inline void
+ createRandomFeatures (
+ const size_t num_of_features,
+ std::vector<MultiChannel2DComparisonFeature<PointXY32i> > & features)
+ {
+ features.resize (num_of_features);
+ for (size_t feature_index = 0; feature_index < num_of_features; ++feature_index)
+ {
+ features[feature_index].p1 = PointXY32i::randomPoint(-feature_window_width_/2, feature_window_width_/2, -feature_window_height_/2, feature_window_height_/2);
+ features[feature_index].p2 = PointXY32i::randomPoint(-feature_window_width_/2, feature_window_width_/2, -feature_window_height_/2, feature_window_height_/2);
+ features[feature_index].channel = static_cast<unsigned char>(NUM_OF_CHANNELS*(static_cast<float>(rand()) / (RAND_MAX+1)));
+ }
+ }
+
+ /** \brief Evaluates a feature for a set of examples on the specified data set.
+ * \param[in] feature The feature to evaluate.
+ * \param[in] data_set The data set the feature is evaluated on.
+ * \param[in] examples The examples the feature is evaluated for.
+ * \param[out] results The destination for the evaluation results.
+ * \param[out] flags The destination for the flags corresponding to the evaluation results.
+ */
+ inline void
+ evaluateFeature (
+ const MultiChannel2DComparisonFeature<PointXY32i> & feature,
+ MultiChannel2DDataSet<DATA_TYPE, NUM_OF_CHANNELS> & data_set,
+ std::vector<MultipleData2DExampleIndex> & examples,
+ std::vector<float> & results,
+ std::vector<unsigned char> & flags) const
+ {
+ results.resize (examples.size ());
+ flags.resize (examples.size ());
+ for (int example_index = 0; example_index < examples.size (); ++example_index)
+ {
+ const MultipleData2DExampleIndex & example = examples[example_index];
+
+ evaluateFeature (feature, data_set, example, results[example_index], flags[example_index]);
+ }
+ }
+
+ /** \brief Evaluates a feature for one examples on the specified data set.
+ * \param[in] feature The feature to evaluate.
+ * \param[in] data_set The data set the feature is evaluated on.
+ * \param[in] example The example the feature is evaluated for.
+ * \param[out] result The destination for the evaluation result.
+ * \param[out] flag The destination for the flag corresponding to the evaluation result.
+ */
+ inline void
+ evaluateFeature (
+ const MultiChannel2DComparisonFeature<PointXY32i> & feature,
+ MultiChannel2DDataSet<DATA_TYPE, NUM_OF_CHANNELS> & data_set,
+ const MultipleData2DExampleIndex & example,
+ float & result,
+ unsigned char & flag) const
+ {
+ const int center_col_index = example.x;
+ const int center_row_index = example.y;
+
+ const size_t p1_col = static_cast<size_t> (feature.p1.x + center_col_index);
+ const size_t p1_row = static_cast<size_t> (feature.p1.y + center_row_index);
+
+ const size_t p2_col = static_cast<size_t> (feature.p2.x + center_col_index);
+ const size_t p2_row = static_cast<size_t> (feature.p2.y + center_row_index);
+
+ const unsigned char channel = feature.channel;
+
+ const float value1 = static_cast<float> (data_set (example.data_set_id, p1_col, p1_row)[channel]);
+ const float value2 = static_cast<float> (data_set (example.data_set_id, p2_col, p2_row)[channel]);
+
+ result = value1 - value2;
+ flag = (pcl_isfinite (value1) && pcl_isfinite (value2)) ? 0 : 1;
+ }
+
+ /** \brief Generates code for feature evaluation.
+ * \param[in] feature The feature for which code is generated.
+ * \param[out] stream The destination for the generated code.
+ */
+ void
+ generateCodeForEvaluation (
+ const MultiChannel2DComparisonFeature<PointXY32i> & feature,
+ std::ostream & stream) const
+ {
+ stream << "ERROR: RegressionVarianceStatsEstimator does not implement generateCodeForBranchIndex(...)";
+ //stream << "const float value = ( (*dataSet)(dataSetId, centerY+" << feature.p1.y << ", centerX+" << feature.p1.x << ")[" << static_cast<int>(feature.colorChannel) << "]"
+ // << " - " << "(*dataSet)(dataSetId, centerY+" << feature.p2.y << ", centerX+" << feature.p2.x << ")[" << static_cast<int>(feature.colorChannel) << "] );" << ::std::endl;
+ }
+
+ private:
+ /** \brief The width of the feature window. */
+ int feature_window_width_;
+ /** \brief The height of the feature window. */
+ int feature_window_height_;
+
+ };
+
+
+ /** \brief Feature utility class that handles the creation and evaluation of RGBD comparison features. */
+ template <class DATA_TYPE, size_t NUM_OF_CHANNELS, size_t SCALE_CHANNEL, bool INVERT_SCALE>
+ class PCL_EXPORTS ScaledMultiChannel2DComparisonFeatureHandler
+ : public pcl::FeatureHandler<pcl::MultiChannel2DComparisonFeature<pcl::PointXY32f>, pcl::MultiChannel2DDataSet<DATA_TYPE, NUM_OF_CHANNELS>, pcl::MultipleData2DExampleIndex>
+ {
+
+ public:
+
+ /** \brief Constructor. */
+ ScaledMultiChannel2DComparisonFeatureHandler (
+ const int feature_window_width,
+ const int feature_window_height)
+ : feature_window_width_ (feature_window_width), feature_window_height_ (feature_window_height)
+ {}
+ /** \brief Destructor. */
+ virtual ~ScaledMultiChannel2DComparisonFeatureHandler () {}
+
+ /** \brief Sets the feature window size.
+ * \param[in] width The width of the feature window.
+ * \param[in] height The height of the feature window.
+ */
+ inline void
+ setFeatureWindowSize (
+ int width,
+ int height)
+ {
+ feature_window_width_ = width;
+ feature_window_height_ = height;
+ }
+
+ /** \brief Creates random features.
+ * \param[in] num_of_features The number of random features to create.
+ * \param[out] features The destination for the created random features.
+ */
+ inline void
+ createRandomFeatures (
+ const size_t num_of_features,
+ std::vector<MultiChannel2DComparisonFeature<PointXY32f> > & features)
+ {
+ features.resize (num_of_features);
+ for (size_t feature_index = 0; feature_index < num_of_features; ++feature_index)
+ {
+ features[feature_index].p1 = PointXY32f::randomPoint(-feature_window_width_/2, feature_window_width_/2, -feature_window_height_/2, feature_window_height_/2);
+ features[feature_index].p2 = PointXY32f::randomPoint(-feature_window_width_/2, feature_window_width_/2, -feature_window_height_/2, feature_window_height_/2);
+ features[feature_index].channel = static_cast<unsigned char>(NUM_OF_CHANNELS*(static_cast<float>(rand()) / (RAND_MAX+1)));
+ }
+ }
+
+ /** \brief Evaluates a feature for a set of examples on the specified data set.
+ * \param[in] feature The feature to evaluate.
+ * \param[in] data_set The data set the feature is evaluated on.
+ * \param[in] examples The examples the feature is evaluated for.
+ * \param[out] results The destination for the evaluation results.
+ * \param[out] flags The destination for the flags corresponding to the evaluation results.
+ */
+ inline void
+ evaluateFeature (
+ const MultiChannel2DComparisonFeature<PointXY32f> & feature,
+ MultiChannel2DDataSet<DATA_TYPE, NUM_OF_CHANNELS> & data_set,
+ std::vector<MultipleData2DExampleIndex> & examples,
+ std::vector<float> & results,
+ std::vector<unsigned char> & flags) const
+ {
+ results.resize (examples.size ());
+ flags.resize (examples.size ());
+ for (int example_index = 0; example_index < examples.size (); ++example_index)
+ {
+ const MultipleData2DExampleIndex & example = examples[example_index];
+
+ evaluateFeature (feature, data_set, example, results[example_index], flags[example_index]);
+ }
+ }
+
+ /** \brief Evaluates a feature for one examples on the specified data set.
+ * \param[in] feature The feature to evaluate.
+ * \param[in] data_set The data set the feature is evaluated on.
+ * \param[in] example The example the feature is evaluated for.
+ * \param[out] result The destination for the evaluation result.
+ * \param[out] flag The destination for the flag corresponding to the evaluation result.
+ */
+ inline void
+ evaluateFeature (
+ const MultiChannel2DComparisonFeature<PointXY32f> & feature,
+ MultiChannel2DDataSet<DATA_TYPE, NUM_OF_CHANNELS> & data_set,
+ const MultipleData2DExampleIndex & example,
+ float & result,
+ unsigned char & flag) const
+ {
+ const int center_col_index = example.x;
+ const int center_row_index = example.y;
+
+ float scale;
+ if (INVERT_SCALE)
+ scale = 1.0f / static_cast<float> (data_set (example.data_set_id, center_col_index, center_row_index)[SCALE_CHANNEL]);
+ else
+ scale = static_cast<float> (data_set (example.data_set_id, center_col_index, center_row_index)[SCALE_CHANNEL]);
+
+
+
+
+ const size_t p1_col = static_cast<size_t> (scale * feature.p1.x + center_col_index);
+ const size_t p1_row = static_cast<size_t> (scale * feature.p1.y + center_row_index);
+
+ const size_t p2_col = static_cast<size_t> (scale * feature.p2.x + center_col_index);
+ const size_t p2_row = static_cast<size_t> (scale * feature.p2.y + center_row_index);
+
+ const unsigned char channel = feature.channel;
+
+ const float value1 = static_cast<float> (data_set (example.data_set_id, p1_col, p1_row)[channel]);
+ const float value2 = static_cast<float> (data_set (example.data_set_id, p2_col, p2_row)[channel]);
+
+ result = value1 - value2;
+ flag = (pcl_isfinite (value1) && pcl_isfinite (value2)) ? 0 : 1;
+ }
+
+ /** \brief Generates code for feature evaluation.
+ * \param[in] feature The feature for which code is generated.
+ * \param[out] stream The destination for the generated code.
+ */
+ void
+ generateCodeForEvaluation (
+ const MultiChannel2DComparisonFeature<PointXY32f> & feature,
+ std::ostream & stream) const
+ {
+ stream << "ERROR: ScaledMultiChannel2DComparisonFeatureHandler does not implement generateCodeForBranchIndex(...)" << std::endl;
+
+ //pcl::PointXY32f p1 = feature.p1;
+ //pcl::PointXY32f p2 = feature.p2;
+
+ //stream << "const float eval_value = data_ptr + " << p1.x << " + " << p1.y << " * width;
+
+ //stream << "const float value = ( (*dataSet)(dataSetId, centerY+" << feature.p1.y << ", centerX+" << feature.p1.x << ")[" << static_cast<int>(feature.colorChannel) << "]"
+ // << " - " << "(*dataSet)(dataSetId, centerY+" << feature.p2.y << ", centerX+" << feature.p2.x << ")[" << static_cast<int>(feature.colorChannel) << "] );" << ::std::endl;
+ }
+
+ private:
+ /** \brief The width of the feature window. */
+ int feature_window_width_;
+ /** \brief The height of the feature window. */
+ int feature_window_height_;
+
+ };
+
+
+ template <class DATA_TYPE, size_t NUM_OF_CHANNELS, size_t SCALE_CHANNEL, bool INVERT_SCALE>
+ class PCL_EXPORTS ScaledMultiChannel2DComparisonFeatureHandlerCCodeGenerator
+ : public pcl::FeatureHandlerCodeGenerator<pcl::MultiChannel2DComparisonFeature<pcl::PointXY32f>, pcl::MultiChannel2DDataSet<DATA_TYPE, NUM_OF_CHANNELS>, pcl::MultipleData2DExampleIndex>
+ {
+ public:
+ ScaledMultiChannel2DComparisonFeatureHandlerCCodeGenerator () {}
+ virtual ~ScaledMultiChannel2DComparisonFeatureHandlerCCodeGenerator () {}
+
+ void
+ generateEvalFunctionCode (
+ std::ostream & stream) const;
+
+ void
+ generateEvalCode (
+ const MultiChannel2DComparisonFeature<PointXY32f> & feature,
+ std::ostream & stream) const;
+ };
+
+ template <class DATA_TYPE, size_t NUM_OF_CHANNELS, size_t SCALE_CHANNEL, bool INVERT_SCALE>
+ void
+ ScaledMultiChannel2DComparisonFeatureHandlerCCodeGenerator<DATA_TYPE, NUM_OF_CHANNELS, SCALE_CHANNEL, INVERT_SCALE>::generateEvalFunctionCode (
+ std::ostream & stream) const
+ {
+ if (NUM_OF_CHANNELS == 1 && SCALE_CHANNEL == 0 && INVERT_SCALE)
+ {
+ stream << "const float scale = 1.0f / static_cast<float> (*data_ptr);" << std::endl;
+ stream << "" << std::endl;
+ stream << "struct LocalFeatureHandler" << std::endl;
+ stream << "{" << std::endl;
+ stream << " static inline void eval (" << typeid (DATA_TYPE).name () << " * a_ptr, const float a_x1, const float a_y1, const float a_x2, const float a_y2, const float a_scale, const int a_width, float & a_result, unsigned char & a_flags)" << std::endl;
+ stream << " {" << std::endl;
+ stream << " a_result = *(a_ptr + static_cast<int> (a_scale*a_x1) + (static_cast<int> (a_scale*a_y1)*a_width)) - *(a_ptr + static_cast<int> (a_scale*a_x2) + (static_cast<int> (a_scale*a_y2)*a_width));" << std::endl;
+ stream << " }" << std::endl;
+ stream << "};" << std::endl;
+ }
+ else
+ {
+ stream << "ERROR: generateEvalFunctionCode not implemented" << std::endl;
+ }
+ }
+
+ template <class DATA_TYPE, size_t NUM_OF_CHANNELS, size_t SCALE_CHANNEL, bool INVERT_SCALE>
+ void
+ ScaledMultiChannel2DComparisonFeatureHandlerCCodeGenerator<DATA_TYPE, NUM_OF_CHANNELS, SCALE_CHANNEL, INVERT_SCALE>::generateEvalCode (
+ const MultiChannel2DComparisonFeature<PointXY32f> & feature,
+ std::ostream & stream) const
+ {
+ stream << "LocalFeatureHandler::eval (data_ptr, "
+ << feature.p1.x << ", "
+ << feature.p1.y << ", "
+ << feature.p2.x << ", "
+ << feature.p2.y << ", "
+ << "scale, width, result, flags);" << std::endl;
+ }
+
+
+ typedef MultiChannel2DComparisonFeatureHandler<float, 1> Depth2DComparisonFeatureHandler;
+ typedef MultiChannel2DComparisonFeatureHandler<float, 2> IntensityDepth2DComparisonFeatureHandler;
+ typedef MultiChannel2DComparisonFeatureHandler<float, 3> RGB2DComparisonFeatureHandler;
+ typedef MultiChannel2DComparisonFeatureHandler<float, 4> RGBD2DComparisonFeatureHandler;
+
+ typedef ScaledMultiChannel2DComparisonFeatureHandler<float, 1, 0, true> ScaledDepth2DComparisonFeatureHandler;
+ typedef ScaledMultiChannel2DComparisonFeatureHandler<float, 2, 1, true> ScaledIntensityDepth2DComparisonFeatureHandler;
+ typedef ScaledMultiChannel2DComparisonFeatureHandler<float, 4, 3, true> ScaledRGBD2DComparisonFeatureHandler;
+
+ typedef ScaledMultiChannel2DComparisonFeatureHandlerCCodeGenerator<float, 1, 0, true> ScaledDepth2DComparisonFeatureHandlerCCodeGenerator;
+
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_MULTI_CHANNEL_2D_DATA_SET_H_
+#define PCL_ML_MULTI_CHANNEL_2D_DATA_SET_H_
+
+#include <pcl/common/common.h>
+
+#include <istream>
+#include <ostream>
+
+namespace pcl
+{
+
+ /** \brief Holds two-dimensional multi-channel data. */
+ template <class DATA_TYPE, size_t NUM_OF_CHANNELS>
+ class PCL_EXPORTS MultiChannel2DData
+ {
+ public:
+
+ /** \brief Constructor. */
+ inline MultiChannel2DData () : data_ (NULL), width_ (0), height_ (0) {}
+ /** \brief Destructor. */
+ virtual ~MultiChannel2DData () {}
+
+ /** \brief Resizes the internal data storage.
+ * \param[in] width The width of the resized 2D data array.
+ * \param[in] height The height of the resized 2D data array.
+ */
+ inline void
+ resize (size_t width, size_t height)
+ {
+ data_.resize (NUM_OF_CHANNELS*width*height);
+ width_ = width;
+ height_ = height;
+ }
+
+ /** \brief Clears the internal data storage and sets width and height to 0.
+ */
+ void
+ clear ()
+ {
+ width_ = 0;
+ height_ = 0;
+ data_.clear ();
+ }
+
+ /** \brief Returns a pointer to the internal data at the specified location.
+ * \param[in] col_index The column index.
+ * \param[in] row_index The row index.
+ */
+ inline DATA_TYPE *
+ operator() (const size_t col_index, const size_t row_index)
+ {
+ return &(data_[NUM_OF_CHANNELS*(row_index*width_ + col_index)]);
+ };
+
+ /** \brief Returns a pointer to the internal data at the specified location.
+ * \param[in] col_index The column index.
+ * \param[in] row_index The row index.
+ */
+ inline const DATA_TYPE *
+ operator() (const size_t col_index, const size_t row_index) const
+ {
+ return &(data_[NUM_OF_CHANNELS*(row_index*width_ + col_index)]);
+ };
+
+ /** \brief Returns a reference to the internal data at the specified location.
+ * \param[in] col_index The column index.
+ * \param[in] row_index The row index.
+ * \param[in] channel The channel index.
+ */
+ inline DATA_TYPE &
+ operator() (const size_t col_index, const size_t row_index, const size_t channel)
+ {
+ return data_[NUM_OF_CHANNELS*(row_index*width_ + col_index) + channel];
+ };
+
+ /** \brief Returns a reference to the internal data at the specified location.
+ * \param[in] col_index The column index.
+ * \param[in] row_index The row index.
+ * \param[in] channel The channel index.
+ */
+ inline const DATA_TYPE &
+ operator() (const size_t col_index, const size_t row_index, const size_t channel) const
+ {
+ return data_[NUM_OF_CHANNELS*(row_index*width_ + col_index) + channel];
+ };
+
+ private:
+
+ /** \brief The internal data storage. */
+ std::vector<DATA_TYPE> data_;
+
+ /** \brief The width of the data storage. */
+ size_t width_;
+ /** \brief The height of the data storage. */
+ size_t height_;
+ };
+
+
+ /** \brief Holds a set of two-dimensional multi-channel data. */
+ template <class DATA_TYPE, size_t NUM_OF_CHANNELS>
+ class PCL_EXPORTS MultiChannel2DDataSet
+ {
+ public:
+
+ /** \brief Constructor. */
+ inline MultiChannel2DDataSet () : data_set_ () {}
+ /** \brief Destructor. */
+ virtual ~MultiChannel2DDataSet () {}
+
+ /** \brief Adds a new two-dimensional data block to the data set.
+ * \param[in] width The width of the new data block.
+ * \param[in] height The height of the new data block.
+ */
+ void
+ addData (const size_t width, const size_t height)
+ {
+ MultiChannel2DData<DATA_TYPE, NUM_OF_CHANNELS> * data = new MultiChannel2DData<DATA_TYPE, NUM_OF_CHANNELS> ();
+ data->resize (width, height);
+
+ data_set_.push_back (data);
+ };
+
+ /** \brief Releases the data stored in the data set. */
+ void
+ releaseDataSet ()
+ {
+ for (size_t data_set_index = 0; data_set_index < data_set_.size (); ++data_set_index)
+ {
+ delete data_set_[data_set_index];
+ }
+ }
+
+ /** \brief Releases the data stored in the data set. */
+ void
+ clear ()
+ {
+ releaseDataSet ();
+ }
+
+ /** \brief Returns a pointer to the specified data block at the specified location.
+ * \param[in] data_set_id The index of the data block.
+ * \param[in] col The column of the desired location.
+ * \param[in] row The row of the desired location.
+ */
+ inline DATA_TYPE *
+ operator() (const size_t data_set_id, const size_t col, const size_t row)
+ {
+ return (*data_set_[data_set_id]) (col, row);
+ };
+
+ /** \brief Returns a pointer to the specified data block at the specified location.
+ * \param[in] data_set_id The index of the data block.
+ * \param[in] col The column of the desired location.
+ * \param[in] row The row of the desired location.
+ */
+ inline const DATA_TYPE *
+ operator() (const size_t data_set_id, const size_t col, const size_t row) const
+ {
+ return (*data_set_[data_set_id]) (col, row);
+ };
+
+ /** \brief Returns a reference to the specified data block at the specified location.
+ * \param[in] data_set_id The index of the data block.
+ * \param[in] col The column of the desired location.
+ * \param[in] row The row of the desired location.
+ * \param[in] channel The channel index.
+ */
+ inline DATA_TYPE &
+ operator() (const size_t data_set_id, const size_t col, const size_t row, const size_t channel)
+ {
+ return (*data_set_[data_set_id]) (col, row, channel);
+ };
+
+ /** \brief Returns a reference to the specified data block at the specified location.
+ * \param[in] data_set_id The index of the data block.
+ * \param[in] col The column of the desired location.
+ * \param[in] row The row of the desired location.
+ * \param[in] channel The channel index.
+ */
+ inline const DATA_TYPE &
+ operator() (const size_t data_set_id, const size_t col, const size_t row, const size_t channel) const
+ {
+ return (*data_set_[data_set_id]) (col, row, channel);
+ };
+
+ private:
+
+ /** \brief The data set. */
+ std::vector<MultiChannel2DData<DATA_TYPE, NUM_OF_CHANNELS>*> data_set_;
+ };
+
+ typedef MultiChannel2DDataSet<float, 1> Depth2DDataSet;
+ typedef MultiChannel2DDataSet<float, 2> IntensityDepth2DDataSet;
+ typedef MultiChannel2DDataSet<float, 3> RGB2DDataSet;
+ typedef MultiChannel2DDataSet<float, 4> RGBD2DDataSet;
+
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_MULTIPLE_DATA_2D_EXAMPLE_INDEX_H_
+#define PCL_ML_MULTIPLE_DATA_2D_EXAMPLE_INDEX_H_
+
+#include <pcl/common/common.h>
+
+#include <istream>
+#include <ostream>
+
+namespace pcl
+{
+
+ /** \brief Example index for a set of 2D data blocks. */
+ struct MultipleData2DExampleIndex
+ {
+ /** \brief The data set index. */
+ int data_set_id;
+
+ /** \brief The x-coordinate. */
+ int x;
+ /** \brief The y-coordinate. */
+ int y;
+ };
+
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author : Christian Potthast
+ * Email : potthast@usc.edu
+ *
+ */
+
+#ifndef PCL_PAIRWISE_POTENTIAL_H_
+#define PCL_PAIRWISE_POTENTIAL_H_
+
+#include <vector>
+
+#include <pcl/ml/permutohedral.h>
+
+namespace pcl
+{
+ /** \brief
+ *
+ */
+ class PairwisePotential
+ {
+ public:
+
+ /** \brief Constructor for DenseCrf class */
+ PairwisePotential (const std::vector<float> &feature, const int D, const int N, const float w);
+
+ /** \brief Deconstructor for DenseCrf class */
+ ~PairwisePotential () {};
+
+ /** \brief */
+ void
+ compute (std::vector<float> &out, const std::vector<float> &in,
+ std::vector<float> &tmp, int value_size) const;
+
+ protected:
+ /** \brief Permutohedral lattice */
+ Permutohedral lattice_;
+
+ /** \brief Number of variables */
+ int N_;
+
+ /** \brief weight */
+ float w_;
+
+ /** \brief norm */
+ std::vector<float> norm_;
+
+ //DBUG
+ public:
+ std::vector<float> bary_;
+ std::vector<float> features_;
+
+ };
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_PERMUTOHEDRAL_H_
+#define PCL_ML_PERMUTOHEDRAL_H_
+
+#ifdef __GNUC__
+#pragma GCC system_header
+#endif
+
+#include <vector>
+#include <map>
+#include <pcl/common/eigen.h>
+#include <boost/intrusive/hashtable.hpp>
+
+// TODO: SWAP with Boost intrusive hash table
+#include <cstdlib>
+#include <cstring>
+#include <cassert>
+#include <cstdio>
+#include <cmath>
+
+namespace pcl
+{
+ /** \brief Implementation of a high-dimensional gaussian filtering using the permutohedral lattice
+ * \author Christian Potthast (potthast@usc.edu)
+ *
+ * Adams_fasthigh-dimensional
+ * author = {Andrew Adams and Jongmin Baek and Myers Abraham Davis},
+ * title = {Fast high-dimensional filtering using the permutohedral lattice},
+ * booktitle = {Computer Graphics Forum (EG 2010 Proceedings},
+ * year = {},
+ * pages = {2010}
+ * }
+ */
+ class Permutohedral
+ {
+ protected:
+ struct Neighbors
+ {
+ int n1, n2;
+ Neighbors (int n1 = 0, int n2 = 0) : n1 (n1), n2 (n2) {}
+ };
+
+ public:
+
+ /** \brief Constructor for Permutohedral class */
+ Permutohedral ();
+
+ /** \brief Deconstructor for Permutohedral class */
+ ~Permutohedral () {};
+
+ /** \brief initialization */
+ void
+ init (const std::vector<float> &feature, const int feature_dimension, const int N);
+
+ void
+ compute (std::vector<float> &out, const std::vector<float> &in,
+ int value_size,
+ int in_offset=0, int out_offset=0,
+ int in_size = -1, int out_size = -1) const;
+ void
+ initOLD (const std::vector<float> &feature, const int feature_dimension, const int N);
+
+ void
+ computeOLD (std::vector<float> &out, const std::vector<float> &in,
+ int value_size,
+ int in_offset=0, int out_offset=0,
+ int in_size = -1, int out_size = -1) const;
+
+ void
+ debug ();
+
+ // Pseudo radnom generator
+ inline
+ size_t generateHashKey (const std::vector<short> &k)
+ {
+ size_t r = 0;
+ for (int i = 0; i < d_; i++)
+ {
+ r += k[i];
+ r *= 1664525;
+ //r *= 5;
+ }
+ return r;// % (2* N_ * (d_+1));
+ }
+
+ public:
+
+ /** \brief Number of variables */
+ int N_;
+
+ std::vector<Neighbors> blur_neighbors_;
+
+ /** \brief size of sparse discretized space */
+ int M_;
+
+ /** \brief dimension of feature */
+ int d_;
+
+ std::vector<float> offset_;
+ std::vector<float> offsetTMP_;
+ std::vector<float> barycentric_;
+
+ Neighbors * blur_neighborsOLD_;
+ int * offsetOLD_;
+ float * barycentricOLD_;
+ std::vector<float> baryOLD_;
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ };
+
+ class HashTableOLD
+ {
+ // Don't copy!
+ HashTableOLD( const HashTableOLD & o ): key_size_ ( o.key_size_ ), filled_(0), capacity_(o.capacity_) {
+ table_ = new int[ capacity_ ];
+ keys_ = new short[ (capacity_/2+10)*key_size_ ];
+ memset( table_, -1, capacity_*sizeof(int) );
+ }
+ protected:
+ size_t key_size_, filled_, capacity_;
+ short * keys_;
+ int * table_;
+ void grow(){
+ std::cout << "GROW" << std::endl;
+
+ // Swap out the old memory
+ short * old_keys = keys_;
+ int * old_table = table_;
+ int old_capacity = static_cast<int> (capacity_);
+ capacity_ *= 2;
+ // Allocate the new memory
+ keys_ = new short[ (old_capacity+10)*key_size_ ];
+ table_ = new int[ capacity_ ];
+ memset( table_, -1, capacity_*sizeof(int) );
+ memcpy( keys_, old_keys, filled_*key_size_*sizeof(short) );
+
+ // Reinsert each element
+ for( int i=0; i<old_capacity; i++ )
+ if (old_table[i] >= 0){
+ int e = old_table[i];
+ size_t h = hash( old_keys+(getKey(e)-keys_) ) % capacity_;
+ for (; table_[h] >= 0; h = h<capacity_-1 ? h+1 : 0) { };
+ table_[h] = e;
+ }
+
+ delete [] old_keys;
+ delete [] old_table;
+ }
+ size_t hash( const short * k ) {
+ size_t r = 0;
+ for( size_t i=0; i<key_size_; i++ ){
+ r += k[i];
+ r *= 1664525;
+ }
+ return r;
+ }
+ public:
+ explicit HashTableOLD( int key_size, int n_elements ) : key_size_ ( key_size ), filled_(0), capacity_(2*n_elements) {
+ table_ = new int[ capacity_ ];
+ keys_ = new short[ (capacity_/2+10)*key_size_ ];
+ memset( table_, -1, capacity_*sizeof(int) );
+ }
+ ~HashTableOLD() {
+ delete [] keys_;
+ delete [] table_;
+ }
+ int size() const {
+ return static_cast<int> (filled_);
+ }
+ void reset() {
+ filled_ = 0;
+ memset( table_, -1, capacity_*sizeof(int) );
+ }
+ int find( const short * k, bool create = false ){
+ if (2*filled_ >= capacity_) grow();
+ // Get the hash value
+ size_t h = hash( k ) % capacity_;
+ // Find the element with he right key, using linear probing
+ while(1){
+ int e = table_[h];
+ if (e==-1){
+ if (create){
+ // Insert a new key and return the new id
+ for( size_t i=0; i<key_size_; i++ )
+ keys_[ filled_*key_size_+i ] = k[i];
+ return table_[h] = static_cast<int> (filled_++);
+ }
+ else
+ return -1;
+ }
+ // Check if the current key is The One
+ bool good = true;
+ for( size_t i=0; i<key_size_ && good; i++ )
+ if (keys_[ e*key_size_+i ] != k[i])
+ good = false;
+ if (good)
+ return e;
+ // Continue searching
+ h++;
+ if (h==capacity_) h = 0;
+ }
+ }
+ const short * getKey( int i ) const{
+ return keys_+i*key_size_;
+ }
+ };
+ /*
+ class HashTable
+ {
+ public:
+ HashTable ( int N ) : N_ (2 * N) {};
+
+ find (const std::vector<short> &k, bool create = false;)
+ {
+
+
+
+
+
+ }
+
+
+
+ protected:
+ std::multimap<size_t, int> table_;
+
+ std::vector<std::vector<short> > keys;
+ //keys.reserve ( (d_+1) * N_ );
+ // number of elements
+ int N_;
+ };*/
+
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_POINT_XY_32F_H_
+#define PCL_ML_POINT_XY_32F_H_
+
+#include <pcl/common/common.h>
+
+#include <istream>
+#include <ostream>
+
+namespace pcl
+{
+
+ /** \brief 2D point with float x- and y-coordinates. */
+ class PCL_EXPORTS PointXY32f
+ {
+ public:
+ /** \brief Constructor. */
+ inline PointXY32f () : x (0.0f), y (0.0f) {}
+ /** \brief Destructor. */
+ inline virtual ~PointXY32f () {}
+
+ /** \brief Serializes the point to the specified stream.
+ * \param[out] stream The destination for the serialization.
+ */
+ inline void
+ serialize (std::ostream & stream) const
+ {
+ stream.write (reinterpret_cast<const char*> (&x), sizeof (x));
+ stream.write (reinterpret_cast<const char*> (&y), sizeof (y));
+ }
+
+ /** \brief Deserializes the point from the specified stream.
+ * \param[in] stream The source for the deserialization.
+ */
+ inline void
+ deserialize (std::istream & stream)
+ {
+ stream.read (reinterpret_cast<char*> (&x), sizeof (x));
+ stream.read (reinterpret_cast<char*> (&y), sizeof (y));
+ }
+
+ /** \brief Creates a random point within the specified window.
+ * \param[in] min_x The minimum value for the x-coordinate of the point.
+ * \param[in] max_x The maximum value for the x-coordinate of the point.
+ * \param[in] min_y The minimum value for the y-coordinate of the point.
+ * \param[in] max_y The maximum value for the y-coordinate of the point.
+ */
+ static PointXY32f
+ randomPoint (const int min_x, const int max_x, const int min_y, const int max_y);
+
+ public:
+ /** \brief The x-coordinate of the point. */
+ float x;
+ /** \brief The y-coordinate of the point. */
+ float y;
+ };
+
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_POINT_XY_32I_H_
+#define PCL_ML_POINT_XY_32I_H_
+
+#include <pcl/common/common.h>
+
+#include <istream>
+#include <ostream>
+
+namespace pcl
+{
+
+ /** \brief 2D point with integer x- and y-coordinates. */
+ class PCL_EXPORTS PointXY32i
+ {
+ public:
+ /** \brief Constructor. */
+ inline PointXY32i () : x (0), y (0) {}
+ /** \brief Destructor. */
+ inline virtual ~PointXY32i () {}
+
+ /** \brief Serializes the point to the specified stream.
+ * \param[out] stream The destination for the serialization.
+ */
+ inline void
+ serialize (std::ostream & stream) const
+ {
+ stream.write (reinterpret_cast<const char*> (&x), sizeof (x));
+ stream.write (reinterpret_cast<const char*> (&y), sizeof (y));
+ }
+
+ /** \brief Deserializes the point from the specified stream.
+ * \param[in] stream The source for the deserialization.
+ */
+ inline void
+ deserialize (std::istream & stream)
+ {
+ stream.read (reinterpret_cast<char*> (&x), sizeof (x));
+ stream.read (reinterpret_cast<char*> (&y), sizeof (y));
+ }
+
+ /** \brief Creates a random point within the specified window.
+ * \param[in] min_x The minimum value for the x-coordinate of the point.
+ * \param[in] max_x The maximum value for the x-coordinate of the point.
+ * \param[in] min_y The minimum value for the y-coordinate of the point.
+ * \param[in] max_y The maximum value for the y-coordinate of the point.
+ */
+ static PointXY32i
+ randomPoint (const int min_x, const int max_x, const int min_y, const int max_y);
+
+ public:
+ /** \brief The x-coordinate of the point. */
+ int x;
+ /** \brief The y-coordinate of the point. */
+ int y;
+ };
+
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_REGRESSION_VARIANCE_STATS_ESTIMATOR_H_
+#define PCL_ML_REGRESSION_VARIANCE_STATS_ESTIMATOR_H_
+
+#include <pcl/common/common.h>
+#include <pcl/ml/stats_estimator.h>
+#include <pcl/ml/branch_estimator.h>
+
+#include <istream>
+#include <ostream>
+
+namespace pcl
+{
+
+ /** \brief Node for a regression trees which optimizes variance. */
+ template <class FeatureType, class LabelType>
+ class PCL_EXPORTS RegressionVarianceNode
+ {
+ public:
+ /** \brief Constructor. */
+ RegressionVarianceNode () : value(0), variance(0), threshold(0), sub_nodes() {}
+ /** \brief Destructor. */
+ virtual ~RegressionVarianceNode () {}
+
+ /** \brief Serializes the node to the specified stream.
+ * \param[out] stream The destination for the serialization.
+ */
+ inline void
+ serialize (std::ostream & stream) const
+ {
+ feature.serialize (stream);
+
+ stream.write (reinterpret_cast<const char*> (&threshold), sizeof (threshold));
+
+ stream.write (reinterpret_cast<const char*> (&value), sizeof (value));
+ stream.write (reinterpret_cast<const char*> (&variance), sizeof (variance));
+
+ const int num_of_sub_nodes = static_cast<int> (sub_nodes.size ());
+ stream.write (reinterpret_cast<const char*> (&num_of_sub_nodes), sizeof (num_of_sub_nodes));
+ for (int sub_node_index = 0; sub_node_index < num_of_sub_nodes; ++sub_node_index)
+ {
+ sub_nodes[sub_node_index].serialize (stream);
+ }
+ }
+
+ /** \brief Deserializes a node from the specified stream.
+ * \param[in] stream The source for the deserialization.
+ */
+ inline void
+ deserialize (std::istream & stream)
+ {
+ feature.deserialize (stream);
+
+ stream.read (reinterpret_cast<char*> (&threshold), sizeof (threshold));
+
+ stream.read (reinterpret_cast<char*> (&value), sizeof (value));
+ stream.read (reinterpret_cast<char*> (&variance), sizeof (variance));
+
+ int num_of_sub_nodes;
+ stream.read (reinterpret_cast<char*> (&num_of_sub_nodes), sizeof (num_of_sub_nodes));
+ sub_nodes.resize (num_of_sub_nodes);
+
+ if (num_of_sub_nodes > 0)
+ {
+ for (int sub_node_index = 0; sub_node_index < num_of_sub_nodes; ++sub_node_index)
+ {
+ sub_nodes[sub_node_index].deserialize (stream);
+ }
+ }
+ }
+
+ public:
+ /** \brief The feature associated with the node. */
+ FeatureType feature;
+ /** \brief The threshold applied on the feature response. */
+ float threshold;
+
+ /** \brief The label value of this node. */
+ LabelType value;
+ /** \brief The variance of the labels that ended up at this node during training. */
+ LabelType variance;
+
+ /** \brief The child nodes. */
+ std::vector<RegressionVarianceNode> sub_nodes;
+ };
+
+ /** \brief Statistics estimator for regression trees which optimizes variance. */
+ template <class LabelDataType, class NodeType, class DataSet, class ExampleIndex>
+ class PCL_EXPORTS RegressionVarianceStatsEstimator
+ : public pcl::StatsEstimator<LabelDataType, NodeType, DataSet, ExampleIndex>
+ {
+
+ public:
+ /** \brief Constructor. */
+ RegressionVarianceStatsEstimator (BranchEstimator * branch_estimator)
+ : branch_estimator_ (branch_estimator)
+ {}
+ /** \brief Destructor. */
+ virtual ~RegressionVarianceStatsEstimator () {}
+
+ /** \brief Returns the number of branches the corresponding tree has. */
+ inline size_t
+ getNumOfBranches () const
+ {
+ //return 2;
+ return branch_estimator_->getNumOfBranches ();
+ }
+
+ /** \brief Returns the label of the specified node.
+ * \param[in] node The node which label is returned.
+ */
+ inline LabelDataType
+ getLabelOfNode (
+ NodeType & node) const
+ {
+ return node.value;
+ }
+
+ /** \brief Computes the information gain obtained by the specified threshold.
+ * \param[in] data_set The data set corresponding to the supplied result data.
+ * \param[in] examples The examples used for extracting the supplied result data.
+ * \param[in] label_data The label data corresponding to the specified examples.
+ * \param[in] results The results computed using the specifed examples.
+ * \param[in] flags The flags corresponding to the results.
+ * \param[in] threshold The threshold for which the information gain is computed.
+ */
+ float
+ computeInformationGain (
+ DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<LabelDataType> & label_data,
+ std::vector<float> & results,
+ std::vector<unsigned char> & flags,
+ const float threshold) const
+ {
+ const size_t num_of_examples = examples.size ();
+ const size_t num_of_branches = getNumOfBranches();
+
+ // compute variance
+ std::vector<LabelDataType> sums (num_of_branches+1, 0);
+ std::vector<LabelDataType> sqr_sums (num_of_branches+1, 0);
+ std::vector<size_t> branch_element_count (num_of_branches+1, 0);
+
+ for (size_t branch_index = 0; branch_index < num_of_branches; ++branch_index)
+ {
+ branch_element_count[branch_index] = 1;
+ ++branch_element_count[num_of_branches];
+ }
+
+ for (size_t example_index = 0; example_index < num_of_examples; ++example_index)
+ {
+ unsigned char branch_index;
+ computeBranchIndex (results[example_index], flags[example_index], threshold, branch_index);
+
+ LabelDataType label = label_data[example_index];
+
+ sums[branch_index] += label;
+ sums[num_of_branches] += label;
+
+ sqr_sums[branch_index] += label*label;
+ sqr_sums[num_of_branches] += label*label;
+
+ ++branch_element_count[branch_index];
+ ++branch_element_count[num_of_branches];
+ }
+
+ std::vector<float> variances (num_of_branches+1, 0);
+ for (size_t branch_index = 0; branch_index < num_of_branches+1; ++branch_index)
+ {
+ const float mean_sum = static_cast<float>(sums[branch_index]) / branch_element_count[branch_index];
+ const float mean_sqr_sum = static_cast<float>(sqr_sums[branch_index]) / branch_element_count[branch_index];
+ variances[branch_index] = mean_sqr_sum - mean_sum*mean_sum;
+ }
+
+ float information_gain = variances[num_of_branches];
+ for (size_t branch_index = 0; branch_index < num_of_branches; ++branch_index)
+ {
+ //const float weight = static_cast<float>(sums[branchIndex]) / sums[numOfBranches];
+ const float weight = static_cast<float>(branch_element_count[branch_index]) / static_cast<float>(branch_element_count[num_of_branches]);
+ information_gain -= weight*variances[branch_index];
+ }
+
+ return information_gain;
+ }
+
+ /** \brief Computes the branch indices for all supplied results.
+ * \param[in] results The results the branch indices will be computed for.
+ * \param[in] flags The flags corresponding to the specified results.
+ * \param[in] threshold The threshold used to compute the branch indices.
+ * \param[out] branch_indices The destination for the computed branch indices.
+ */
+ void
+ computeBranchIndices (
+ std::vector<float> & results,
+ std::vector<unsigned char> & flags,
+ const float threshold,
+ std::vector<unsigned char> & branch_indices) const
+ {
+ const size_t num_of_results = results.size ();
+ const size_t num_of_branches = getNumOfBranches();
+
+ branch_indices.resize (num_of_results);
+ for (size_t result_index = 0; result_index < num_of_results; ++result_index)
+ {
+ unsigned char branch_index;
+ computeBranchIndex (results[result_index], flags[result_index], threshold, branch_index);
+ branch_indices[result_index] = branch_index;
+ }
+ }
+
+ /** \brief Computes the branch index for the specified result.
+ * \param[in] result The result the branch index will be computed for.
+ * \param[in] flag The flag corresponding to the specified result.
+ * \param[in] threshold The threshold used to compute the branch index.
+ * \param[out] branch_index The destination for the computed branch index.
+ */
+ inline void
+ computeBranchIndex(
+ const float result,
+ const unsigned char flag,
+ const float threshold,
+ unsigned char & branch_index) const
+ {
+ branch_estimator_->computeBranchIndex (result, flag, threshold, branch_index);
+ //branch_index = (result > threshold) ? 1 : 0;
+ }
+
+ /** \brief Computes and sets the statistics for a node.
+ * \param[in] data_set The data set which is evaluated.
+ * \param[in] examples The examples which define which parts of the data set are used for evaluation.
+ * \param[in] label_data The label_data corresponding to the examples.
+ * \param[out] node The destination node for the statistics.
+ */
+ void
+ computeAndSetNodeStats (
+ DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<LabelDataType> & label_data,
+ NodeType & node) const
+ {
+ const size_t num_of_examples = examples.size ();
+
+ LabelDataType sum = 0.0f;
+ LabelDataType sqr_sum = 0.0f;
+ for (size_t example_index = 0; example_index < num_of_examples; ++example_index)
+ {
+ const LabelDataType label = label_data[example_index];
+
+ sum += label;
+ sqr_sum += label*label;
+ }
+
+ sum /= num_of_examples;
+ sqr_sum /= num_of_examples;
+
+ const float variance = sqr_sum - sum*sum;
+
+ node.value = sum;
+ node.variance = variance;
+ }
+
+ /** \brief Generates code for branch index computation.
+ * \param[in] node The node for which code is generated.
+ * \param[out] stream The destination for the generated code.
+ */
+ void
+ generateCodeForBranchIndexComputation (
+ NodeType & node,
+ std::ostream & stream) const
+ {
+ stream << "ERROR: RegressionVarianceStatsEstimator does not implement generateCodeForBranchIndex(...)";
+ }
+
+ /** \brief Generates code for label output.
+ * \param[in] node The node for which code is generated.
+ * \param[out] stream The destination for the generated code.
+ */
+ void
+ generateCodeForOutput (
+ NodeType & node,
+ std::ostream & stream) const
+ {
+ stream << "ERROR: RegressionVarianceStatsEstimator does not implement generateCodeForBranchIndex(...)";
+ }
+
+ private:
+ /** \brief The branch estimator. */
+ pcl::BranchEstimator * branch_estimator_;
+ };
+
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_ML_DT_STATS_ESTIMATOR_H_
+#define PCL_ML_DT_STATS_ESTIMATOR_H_
+
+#include <pcl/common/common.h>
+
+#include <ostream>
+#include <vector>
+
+namespace pcl
+{
+
+ /** \brief Class interface for gathering statistics for decision tree learning. */
+ template <
+ class LabelDataType,
+ class NodeType,
+ class DataSet,
+ class ExampleIndex >
+ class PCL_EXPORTS StatsEstimator
+ {
+
+ public:
+
+ /** \brief Destructor. */
+ virtual
+ ~StatsEstimator () {};
+
+ /** \brief Returns the number of brances a node can have (e.g. a binary tree has 2). */
+ virtual size_t
+ getNumOfBranches () const = 0;
+
+ /** \brief Computes and sets the statistics for a node.
+ * \param[in] data_set The data set used for training.
+ * \param[in] examples The examples used for computing the statistics for the specified node.
+ * \param[in] label_data The labels corresponding to the examples.
+ * \param[out] node The destination node for the statistics.
+ */
+ virtual void
+ computeAndSetNodeStats (DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<LabelDataType> & label_data,
+ NodeType & node ) const = 0;
+
+ /** \brief Returns the label of the specified node.
+ * \param[in] node The node from which the label is extracted. */
+ virtual LabelDataType
+ getLabelOfNode (NodeType & node) const = 0;
+
+ /** \brief Computes the information gain obtained by the specified threshold on the supplied feature evaluation results.
+ * \param[in] data_set The data set used for extracting the supplied result values.
+ * \param[in] examples The examples used to extract the supplied result values.
+ * \param[in] label_data The labels corresponding to the examples.
+ * \param[in] results The results obtained from the feature evaluation.
+ * \param[in] flags The flags obtained together with the results.
+ * \param[in] threshold The threshold which is used to compute the information gain.
+ */
+ virtual float
+ computeInformationGain (DataSet & data_set,
+ std::vector<ExampleIndex> & examples,
+ std::vector<LabelDataType> & label_data,
+ std::vector<float> & results,
+ std::vector<unsigned char> & flags,
+ const float threshold) const = 0;
+
+ /** \brief Computes the branch indices obtained by the specified threshold on the supplied feature evaluation results.
+ * \param[in] results The results obtained from the feature evaluation.
+ * \param[in] flags The flags obtained together with the results.
+ * \param[in] threshold The threshold which is used to compute the branch indices.
+ * \param[out] branch_indices The destination for the computed branch indices.
+ */
+ virtual void
+ computeBranchIndices (std::vector<float> & results,
+ std::vector<unsigned char> & flags,
+ const float threshold,
+ std::vector<unsigned char> & branch_indices) const = 0;
+
+ /** \brief Computes the branch indices obtained by the specified threshold on the supplied feature evaluation results.
+ * \param[in] result The result obtained from the feature evaluation.
+ * \param[in] flag The flag obtained together with the result.
+ * \param[in] threshold The threshold which is used to compute the branch index.
+ * \param[out] branch_index The destination for the computed branch index.
+ */
+ virtual void
+ computeBranchIndex (const float result,
+ const unsigned char flag,
+ const float threshold,
+ unsigned char & branch_index) const = 0;
+
+ /** \brief Generates code for computing the branch indices for the specified node and writes it to the specified stream.
+ * \param[in] node The node for which the branch index estimation code is generated.
+ * \param[out] stream The destionation for the code.
+ */
+ virtual void
+ generateCodeForBranchIndexComputation (NodeType & node,
+ std::ostream & stream) const = 0;
+
+ /** \brief Generates code for computing the output for the specified node and writes it to the specified stream.
+ * \param[in] node The node for which the output estimation code is generated.
+ * \param[out] stream The destionation for the code.
+ */
+ virtual void
+ generateCodeForOutput (NodeType & node,
+ std::ostream & stream ) const = 0;
+
+ };
+
+}
+
+#endif
--- /dev/null
+ /*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ * Copyright (c) 2000-2012 Chih-Chung Chang and Chih-Jen Lin
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of copyright holders nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+
+#ifndef _LIBSVM_H_
+#define _LIBSVM_H_
+
+#define LIBSVM_VERSION 311
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+ extern int libsvm_version;
+
+ struct svm_node
+ {
+ int index;
+ double value;
+ };
+
+ struct svm_problem
+ {
+ int l;
+ double *y;
+
+ struct svm_node **x;
+ };
+
+ struct svm_scaling
+ {
+ // index = 1 if usable, index = 0 if not
+
+ struct svm_node *obj;
+
+ // max features scaled
+ int max;
+
+ svm_scaling() : max(0)
+ {
+ }
+ };
+
+ enum { C_SVC, NU_SVC, ONE_CLASS, EPSILON_SVR, NU_SVR }; /* svm_type */
+ enum { LINEAR, POLY, RBF, SIGMOID, PRECOMPUTED }; /* kernel_type */
+
+ struct svm_parameter
+ {
+ int svm_type;
+ int kernel_type;
+ int degree; /* for poly */
+ double gamma; /* for poly/rbf/sigmoid */
+ double coef0; /* for poly/sigmoid */
+
+ /* these are for training only */
+ double cache_size; /* in MB */
+ double eps; /* stopping criteria */
+ double C; /* for C_SVC, EPSILON_SVR and NU_SVR */
+ int nr_weight; /* for C_SVC */
+ int *weight_label; /* for C_SVC */
+ double* weight; /* for C_SVC */
+ double nu; /* for NU_SVC, ONE_CLASS, and NU_SVR */
+ double p; /* for EPSILON_SVR */
+ int shrinking; /* use the shrinking heuristics */
+ int probability; /* do probability estimates */
+ };
+
+//
+// svm_model
+//
+
+ struct svm_model
+ {
+
+ struct svm_parameter param; /* parameter */
+ int nr_class; /* number of classes, = 2 in regression/one class svm */
+ int l; /* total #SV */
+
+ struct svm_node **SV; /* SVs (SV[l]) */
+ double **sv_coef; /* coefficients for SVs in decision functions (sv_coef[k-1][l]) */
+ double *rho; /* constants in decision functions (rho[k*(k-1)/2]) */
+ double *probA; /* pariwise probability information */
+ double *probB;
+
+ /* for classification only */
+
+ int *label; /* label of each class (label[k]) */
+ int *nSV; /* number of SVs for each class (nSV[k]) */
+ /* nSV[0] + nSV[1] + ... + nSV[k-1] = l */
+ /* XXX */
+ int free_sv; /* 1 if svm_model is created by svm_load_model*/
+ /* 0 if svm_model is created by svm_train */
+
+ /* for scaling */
+
+ struct svm_node *scaling;
+ };
+
+ struct svm_model *svm_train (const struct svm_problem *prob, const struct svm_parameter *param);
+ void svm_cross_validation (const struct svm_problem *prob, const struct svm_parameter *param, int nr_fold, double *target);
+
+ int svm_save_model (const char *model_file_name, const struct svm_model *model);
+
+ struct svm_model *svm_load_model (const char *model_file_name);
+
+ int svm_get_svm_type (const struct svm_model *model);
+ int svm_get_nr_class (const struct svm_model *model);
+ void svm_get_labels (const struct svm_model *model, int *label);
+ double svm_get_svr_probability (const struct svm_model *model);
+
+ double svm_predict_values (const struct svm_model *model, const struct svm_node *x, double* dec_values);
+ double svm_predict (const struct svm_model *model, const struct svm_node *x);
+ double svm_predict_probability (const struct svm_model *model, const struct svm_node *x, double* prob_estimates);
+
+ void svm_free_model_content (struct svm_model *model_ptr);
+ void svm_free_and_destroy_model (struct svm_model **model_ptr_ptr);
+ void svm_destroy_param (struct svm_parameter *param);
+
+ const char *svm_check_parameter (const struct svm_problem *prob, const struct svm_parameter *param);
+ int svm_check_probability_model (const struct svm_model *model);
+
+ void svm_set_print_string_function (void (*print_func) (const char *));
+
+#ifdef __cplusplus
+}
+
+#endif
+
+#endif /* _LIBSVM_H_ */
--- /dev/null
+ /*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ * Copyright (c) 2000-2012 Chih-Chung Chang and Chih-Jen Lin
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of copyright holders nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_SVM_WRAPPER_H_
+#define PCL_SVM_WRAPPER_H_
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <ctype.h>
+#include <errno.h>
+#include <iostream>
+#include <fstream>
+#include <pcl/common/eigen.h>
+#include <vector>
+
+#include <pcl/console/time.h>
+
+#include <pcl/ml/svm.h>
+#define Malloc(type,n) static_cast<type *> (malloc((n)*sizeof(type)))
+
+namespace pcl
+{
+
+ /** \brief The structure stores the parameters for the classificationa nd
+ * must be initialized and passed to the training method pcl::SVMTrain.
+ * \param svm_type {C_SVC, NU_SVC, ONE_CLASS, EPSILON_SVR, NU_SVR}
+ * \param kernel_type {LINEAR, POLY, RBF, SIGMOID, PRECOMPUTED}
+ * \param probability sets the probability estimates
+ */
+ struct SVMParam: svm_parameter
+ {
+ SVMParam ()
+ {
+ svm_type = C_SVC; // C_SVC, NU_SVC, ONE_CLASS, EPSILON_SVR, NU_SVR
+ kernel_type = RBF; // LINEAR, POLY, RBF, SIGMOID, PRECOMPUTED
+ degree = 3; // for poly
+ gamma = 0; // 1/num_features {for poly/rbf/sigmoid}
+ coef0 = 0; // for poly/sigmoid
+
+ nu = 0.5; // for NU_SVC, ONE_CLASS, and NU_SVR
+ cache_size = 100; // in MB
+ C = 1; // for C_SVC, EPSILON_SVR and NU_SVR
+ eps = 1e-3; // stopping criteria
+ p = 0.1; // for EPSILON_SVR
+ shrinking = 0; // use the shrinking heuristics
+ probability = 0; // do probability estimates
+
+ nr_weight = 0; // for C_SVC
+ weight_label = NULL; // for C_SVC
+ weight = NULL; // for C_SVC
+ }
+ };
+
+ /** \brief The structure initialize a model crated by the SVM (Support Vector Machines) classifier (pcl::SVMTrain)
+ */
+ struct SVMModel: svm_model
+ {
+ SVMModel ()
+ {
+ l = 0;
+ probA = NULL;
+ probB = NULL;
+ }
+ };
+
+ /** \brief The structure initialize a single feature value for the classification using SVM (Support Vector Machines).
+ */
+ struct SVMDataPoint
+ {
+ int idx; // It's the feature index. It has to be an integer number greater or equal to zero.
+ float value; // The value assigned to the correspondent feature.
+
+ SVMDataPoint () : idx (-1), value (0)
+ {
+ }
+ };
+
+ /** \brief The structure stores the features and the label of a single sample which has to be used
+ * for the training or the classification of the SVM (Support Vector Machines).
+ */
+ struct SVMData
+ {
+ double label; // Pointer to the label value. It is a mandatory to train the classifier.
+ std::vector<pcl::SVMDataPoint> SV; // Vector of features for the specific sample.
+
+ SVMData () : label (std::numeric_limits<double>::signaling_NaN())
+ {
+ }
+ };
+
+ /** \brief Base class for SVM SVM (Support Vector Machines).
+ */
+ class SVM
+ {
+ protected:
+ std::vector<SVMData> training_set_; // Basic training set
+ svm_problem prob_; // contains the problem (vector of samples with their features)
+ SVMModel model_; // model of the classifier
+ svm_scaling scaling_; // for the best model training, the input dataset is scaled and the scaling factors are stored here
+ SVMParam param_; // it stores the training parameters
+ std::string class_name_; // The SVM class name.
+
+ char *line_; // buffer for line reading
+ int max_line_len_; // max line length in the input file
+ bool labelled_training_set_; // it stores whether the input set of samples is labelled
+ /** \brief Set for output printings during classification. */
+ static void
+ printNull (const char *) {};
+
+ /** \brief To read a line from the input file. Stored in "line_". */
+ char*
+ readline (FILE *input);
+
+ /** \brief Outputs an error in file reading. */
+ void exitInputError (int line_num)
+ {
+ fprintf (stderr, "Wrong input format at line %d\n", line_num);
+ exit (1);
+ }
+
+ /** \brief Get a string representation of the name of this class. */
+ inline const std::string&
+ getClassName () const
+ {
+ return (class_name_);
+ }
+
+ /** \brief Convert the input format (vector of SVMData) into a readable format for libSVM. */
+ void adaptInputToLibSVM (std::vector<SVMData> training_set, svm_problem &prob);
+
+ /** \brief Convert the libSVM format (svm_problem) into a easier output format. */
+ void adaptLibSVMToInput (std::vector<SVMData> &training_set, svm_problem prob);
+
+ /** \brief Load a problem from an extern file. */
+ bool loadProblem (const char *filename, svm_problem &prob);
+
+ /** \brief Save the raw problem in an extern file.*/
+ bool saveProblem (const char *filename, bool labelled);
+
+ /** \brief Save the problem (with normalized values) in an extern file.*/
+ bool saveProblemNorm (const char *filename, svm_problem prob_, bool labelled);
+
+ public:
+ /** \brief Constructor. */
+ SVM () :
+ training_set_ (), prob_ (), model_ (), scaling_ (), param_ (),
+ class_name_ (), line_ (NULL), max_line_len_ (10000), labelled_training_set_ (1)
+ {
+ }
+
+ /** \brief Destructor. */
+ ~SVM ()
+ {
+ svm_destroy_param (¶m_); // delete parameters
+
+ if (scaling_.max > 0)
+ free (scaling_.obj); // delete scaling factors
+
+ // delete the problem
+ if (prob_.l > 0)
+ {
+ free (prob_.x);
+ free (prob_.y);
+ }
+ }
+
+ /** \brief Return the labels order from the classifier model. */
+ void
+ getLabel (std::vector<int> &labels)
+ {
+ int nr_class = svm_get_nr_class (&model_);
+ int *labels_ = static_cast<int *> (malloc (nr_class * sizeof (int)));
+ svm_get_labels (&model_, labels_);
+
+ for (int j = 0 ; j < nr_class; j++)
+ labels.push_back (labels_[j]);
+
+ free (labels_);
+ };
+
+ /** \brief Save the classifier model in an extern file (in svmlight format). */
+ void saveClassifierModel (const char *filename)
+ {
+ // exit if model has no data
+ if (model_.l == 0)
+ return;
+
+ if (svm_save_model (filename, &model_))
+ {
+ fprintf (stderr, "can't save model to file %s\n", filename);
+ exit (1);
+ }
+ };
+ };
+
+ /** \brief SVM (Support Vector Machines) training class for the SVM machine learning.
+ * It creates a model for the classifier from a labelled input dataset.
+ * OPTIONAL: pcl::SVMParam has to be given as input to vary the default training method and parameters.
+ */
+ class SVMTrain : public SVM
+ {
+ protected:
+ using SVM::labelled_training_set_;
+ using SVM::model_;
+ using SVM::line_;
+ using SVM::max_line_len_;
+ using SVM::training_set_;
+ using SVM::prob_;
+ using SVM::scaling_;
+ using SVM::param_;
+ using SVM::class_name_;
+
+ bool debug_; // Set to 1 to see the training output
+ int cross_validation_; // Set too 1 for cross validating the classifier
+ int nr_fold_; // Number of folds to be used during cross validation. It indicates in how many parts is split the input training set.
+
+ /** \brief To cross validate the classifier. It is automatic for probability estimate. */
+ void
+ doCrossValidation();
+
+ /** \brief It extracts scaling factors from the input training_set.
+ * The scaling of the training_set is a mandatory for a good training of the classifier. */
+ void
+ scaleFactors (std::vector<SVMData> training_set, svm_scaling &scaling);
+
+ public:
+ /** \brief Constructor. */
+ SVMTrain() : debug_ (0), cross_validation_ (0), nr_fold_ (0)
+ {
+ class_name_ = "SVMTrain";
+ svm_set_print_string_function (&printNull); // Default to NULL to not print debugging info
+ }
+
+ /** \brief Destructor. */
+ ~SVMTrain ()
+ {
+ if (model_.l > 0)
+ svm_free_model_content (&model_);
+ }
+
+ /** \brief Change default training parameters (pcl::SVMParam). */
+ void
+ setParameters (SVMParam param)
+ {
+ param_ = param;
+ }
+
+ /** \brief Return the current training parameters. */
+ SVMParam
+ getParameters ()
+ {
+ return param_;
+ }
+
+ /** \brief Return the result of the training. */
+ SVMModel
+ getClassifierModel ()
+ {
+ return model_;
+ }
+
+ /** \brief It adds/store the training set with labelled data. */
+ void
+ setInputTrainingSet (std::vector<SVMData> training_set)
+ {
+ training_set_.insert (training_set_.end(), training_set.begin(), training_set.end());
+ }
+
+ /** \brief Return the current training set. */
+ std::vector<SVMData>
+ getInputTrainingSet ()
+ {
+ return training_set_;
+ }
+
+ /** \brief Reset the training set. */
+ void
+ resetTrainingSet ()
+ {
+ training_set_.clear();
+ }
+
+ /** \brief Start the training of the SVM classifier.
+ \return false if fails. */
+ bool
+ trainClassifier ();
+
+ /** \brief Read in a problem (in svmlight format).
+ * \return false if fails. */
+ bool
+ loadProblem (const char *filename)
+ {
+ return SVM::loadProblem (filename, prob_);
+ };
+
+ /** \brief Set to 1 for debugging info. */
+ void
+ setDebugMode (bool in)
+ {
+ debug_ = in;
+
+ if (in)
+ svm_set_print_string_function (NULL);
+ else
+ svm_set_print_string_function (&printNull);
+ };
+
+ /** \brief Save the raw training set in a file (in svmlight format).
+ * \return false if fails. */
+ bool
+ saveTrainingSet (const char *filename)
+ {
+ return SVM::saveProblem (filename, 1);
+ };
+
+ /** \brief Save the normalized training set in a file (in svmlight format).
+ * \return false if fails. */
+ bool
+ saveNormTrainingSet (const char *filename)
+ {
+ return SVM::saveProblemNorm (filename, prob_, 1);
+ };
+ };
+
+ /** \brief SVM (Support Vector Machines) classification of a dataset.
+ * It can be used both for testing a classifier model and for classify of new data.
+ */
+ class SVMClassify : public SVM
+ {
+ protected:
+ using SVM::labelled_training_set_;
+ using SVM::model_;
+ using SVM::line_;
+ using SVM::max_line_len_;
+ using SVM::training_set_;
+ using SVM::prob_;
+ using SVM::scaling_;
+ using SVM::param_;
+ using SVM::class_name_;
+
+ bool model_extern_copied_; // Set to 0 if the model is loaded from an extern file.
+ bool predict_probability_; // Set to 1 to predict probabilities.
+ std::vector< std::vector<double> > prediction_; // It stores the resulting prediction.
+
+ /** \brief It scales the input dataset using the model information. */
+ void scaleProblem (svm_problem &input, svm_scaling scaling);
+
+ public:
+ /** \brief Constructor. */
+ SVMClassify () : model_extern_copied_ (0), predict_probability_ (0)
+ {
+ class_name_ = "SvmClassify";
+ }
+
+ /** \brief Destructor. */
+ ~SVMClassify ()
+ {
+ if (!model_extern_copied_ && model_.l > 0)
+ svm_free_model_content (&model_);
+ }
+
+ /** \brief It adds/store the training set with labelled data. */
+ void
+ setInputTrainingSet (std::vector<SVMData> training_set)
+ {
+ assert (training_set.size() > 0);
+
+ if (scaling_.max == 0)
+ {
+ // to be sure to have loaded the scaling
+ PCL_ERROR ("[pcl::%s::setInputTrainingSet] Classifier model not loaded!\n", getClassName ().c_str ());
+ return;
+ }
+
+ training_set_.insert (training_set_.end(), training_set.begin(), training_set.end());
+ SVM::adaptInputToLibSVM (training_set_, prob_);
+ }
+
+ /** \brief Return the current training set. */
+ std::vector<SVMData>
+ getInputTrainingSet ()
+ {
+ return training_set_;
+ }
+
+ /** \brief Reset the training set. */
+ void
+ resetTrainingSet()
+ {
+ training_set_.clear();
+ }
+
+ /** \brief Read in a classifier model (in svmlight format).
+ * \return false if fails. */
+ bool
+ loadClassifierModel (const char *filename);
+
+ /** \brief Get the result of the classification. */
+ void
+ getClassificationResult (std::vector< std::vector<double> > &out)
+ {
+ out.clear ();
+ out.insert (out.begin(), prediction_.begin(), prediction_.end());
+ }
+
+ /** \brief Save the classification result in an extern file. */
+ void
+ saveClassificationResult (const char *filename);
+
+ /** \brief Set the classifier model. */
+ void
+ setClassifierModel (SVMModel model)
+ {
+ // model (inner pointers are references)
+ model_ = model;
+ int i = 0;
+
+ while (model_.scaling[i].index != -1)
+ i++;
+
+ scaling_.max = i;
+ scaling_.obj = Malloc (struct svm_node, i + 1);
+ scaling_.obj[i].index = -1;
+
+ // Performing full scaling copy
+ for (int j = 0; j < i; j++)
+ {
+ scaling_.obj[j] = model_.scaling[j];
+ }
+
+ model_extern_copied_ = 1;
+ };
+
+ /** \brief Read in a raw classification problem (in svmlight format).
+ * The values are normalized using the classifier model information.
+ * \return false if fails. */
+ bool
+ loadClassProblem (const char *filename)
+ {
+ assert (model_.l != 0);
+
+ bool out = SVM::loadProblem (filename, prob_);
+ SVM::adaptLibSVMToInput (training_set_, prob_);
+ scaleProblem (prob_, scaling_);
+ return out;
+ };
+
+ /** \brief Read in a normalized classification problem (in svmlight format).
+ * The data are kept whitout normalizing.
+ * \return false if fails. */
+ bool
+ loadNormClassProblem (const char *filename)
+ {
+ bool out = SVM::loadProblem (filename, prob_);
+ SVM::adaptLibSVMToInput (training_set_, prob_);
+ return out;
+ };
+
+ /** \brief Set whether the classification has to be done with the probability estimate.
+ * (the classifier model has to support it). */
+ void
+ setProbabilityEstimates (bool set)
+ {
+ predict_probability_ = set;
+ };
+
+ /** \brief Start the classification on labelled input dataset. It returns the accuracy percentage.
+ * To get the classification result, use getClassificationResult.
+ * \return false if fails. */
+ bool
+ classificationTest ();
+
+ /** \brief Start the classification on un-labelled input dataset.
+ * To get the classification result, use getClassificationResult.
+ * \return false if fails. */
+ bool
+ classification ();
+
+ /** \brief Start the classification on a single set. */
+ std::vector<double>
+ classification (SVMData in);
+
+ /** \brief Save the raw classification problem in a file (in svmlight format).
+ * \return false if fails. */
+ bool
+ saveClassProblem (const char *filename)
+ {
+ return SVM::saveProblem (filename, 0);
+ };
+
+ /** \brief Save the normalized classification problem in a file (in svmlight format).
+ * \return false if fails. */
+ bool
+ saveNormClassProblem (const char *filename)
+ {
+ return SVM::saveProblemNorm (filename, prob_, 0);
+ };
+ };
+}
+
+#endif // PCL_SVM_WRAPPER_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author : Christian Potthast
+ * Email : potthast@usc.edu
+ *
+ */
+
+#include <pcl/ml/densecrf.h>
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::DenseCrf::DenseCrf (int N, int m) :
+ N_ (N), M_ (m),
+ xyz_ (false), rgb_ (false), normal_ (false)
+{
+ current_.resize (N_ * M_, 0.0f);
+ next_.resize (N_ * M_, 0.0f);
+ tmp_.resize (2 * N_ * M_, 0.0f);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::DenseCrf::~DenseCrf ()
+{
+ for(size_t i = 0; i < pairwise_potential_.size (); i++ )
+ delete pairwise_potential_[i];
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DenseCrf::setDataVector (const std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > data)
+{
+ xyz_ = true;
+ data_ = data;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DenseCrf::setColorVector (const std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > color)
+{
+ rgb_ = true;
+ color_ = color;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DenseCrf::setUnaryEnergy (const std::vector<float> unary)
+{
+ unary_ = unary;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DenseCrf::addPairwiseEnergy (const std::vector<float> &feature, const int feature_dimension, const float w)
+{
+ pairwise_potential_.push_back ( new PairwisePotential (feature, feature_dimension, N_, w) );
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DenseCrf::addPairwiseGaussian (float sx, float sy, float sz, float w)
+{
+ // create feature vector
+ std::vector<float> feature;
+ // reserve space for the three-dimensional Gaussian kernel
+ feature.resize (N_ * 3);
+
+ // fill the feature vector
+ for (size_t i = 0; i < data_.size (); i++)
+ {
+ feature[i * 3 ] = static_cast<float> (data_[i].x ()) / sx;
+ feature[i * 3 + 1] = static_cast<float> (data_[i].y ()) / sy;
+ feature[i * 3 + 2] = static_cast<float> (data_[i].z ()) / sz;
+ }
+ // add kernel
+ addPairwiseEnergy (feature, 3, w);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DenseCrf::addPairwiseBilateral (float sx, float sy, float sz,
+ float sr, float sg, float sb,
+ float w)
+{
+ // create feature vector
+ std::vector<float> feature;
+ // reserve space for the six-dimensional Gaussian kernel
+ feature.resize (N_ * 6);
+
+ // fill the feature vector
+ for (size_t i = 0; i < color_.size (); i++)
+ {
+ feature[i * 6 ] = static_cast<float> (data_[i].x ()) / sx;
+ feature[i * 6 + 1] = static_cast<float> (data_[i].y ()) / sy;
+ feature[i * 6 + 2] = static_cast<float> (data_[i].z ()) / sz;
+ feature[i * 6 + 3] = static_cast<float> (color_[i].x ()) / sr;
+ feature[i * 6 + 4] = static_cast<float> (color_[i].y ()) / sg;
+ feature[i * 6 + 5] = static_cast<float> (color_[i].z ()) / sb;
+ }
+ // add kernel
+ addPairwiseEnergy (feature, 6, w);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DenseCrf::addPairwiseNormals (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > &coord,
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &normals,
+ float sx, float sy, float sz,
+ float snx, float sny, float snz,
+ float w)
+{
+ std::cout << coord.size () << std::endl;
+ std::cout << normals.size () << std::endl;
+
+ // create feature vector
+ std::vector<float> feature;
+ // reserve space for the six-dimensional Gaussian kernel
+ feature.resize (N_ * 6);
+
+ // fill the feature vector
+ for (size_t i = 0; i < coord.size (); i++)
+ {
+ if (pcl_isnan (normals[i].x ()))
+ {
+ if (i > 0)
+ {
+ normals[i].x () = normals[i-1].x ();
+ normals[i].y () = normals[i-1].y ();
+ normals[i].z () = normals[i-1].z ();
+ }
+
+ //std::cout << "NaN" << std::endl;
+
+ }
+
+ feature[i * 6 ] = static_cast<float> (coord[i].x ()) / sx;
+ feature[i * 6 + 1] = static_cast<float> (coord[i].y ()) / sy;
+ feature[i * 6 + 2] = static_cast<float> (coord[i].z ()) / sz;
+ feature[i * 6 + 3] = static_cast<float> (normals[i].x ()) / snx;
+ feature[i * 6 + 4] = static_cast<float> (normals[i].y ()) / sny;
+ feature[i * 6 + 5] = static_cast<float> (normals[i].z ()) / snz;
+ }
+ // add kernel
+
+ std::cout << "TEEEEST" << std::endl;
+
+ addPairwiseEnergy (feature, 6, w);
+
+ std::cout << "TEEEEST2" << std::endl;
+
+}
+
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DenseCrf::inference (int n_iterations, std::vector<float> &result, float relax)
+{
+ // Start inference
+ // Initialize using the unary energies
+ expAndNormalize (current_, unary_, -1);
+
+ for (int i = 0; i < n_iterations; i++)
+ {
+ runInference (relax);
+ std::cout << "iteration: " << i+1 << " - DONE" << std::endl;
+ }
+
+ // Copy the data into the result vector
+ result = current_;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DenseCrf::mapInference (int n_iterations, std::vector<int> &result, float relax)
+{
+ // Start inference
+ // Initialize using the unary energies
+ expAndNormalize (current_, unary_, -1);
+
+ for (int i = 0; i < n_iterations; i++)
+ {
+ runInference (relax);
+ std::cout << "iteration: " << i+1 << " - DONE" << std::endl;
+ }
+
+ // Find the map
+ for (int i = 0; i < N_; i++)
+ {
+ const int prob_idx = i * M_;
+ // Find the max
+ float p_label = current_[prob_idx];
+ int idx = 0;
+ for (int j = 1; j < M_; j++)
+ {
+ if (p_label < current_[prob_idx + j])
+ {
+ p_label = current_[prob_idx + j];
+ idx = j;
+ }
+ }
+ result[i] = idx;
+ }
+
+
+
+/*
+ for( int i = 0; i < N_; i++ ){
+ const float * p = prob + i*M_;
+ // Find the max and subtract it so that the exp doesn't explode
+ float mx = p[0];
+ int imx = 0;
+ for( int j=1; j<M_; j++ )
+ if( mx < p[j] ){
+ mx = p[j];
+ imx = j;
+ }
+ result[i] = imx
+ }
+*/
+
+}
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DenseCrf::expAndNormalize (std::vector<float> &out, const std::vector<float> &in,
+ float scale, float relax)
+{
+ std::vector<float> V (N_ + 10);
+ for( int i = 0; i < N_; i++ ){
+ int b_idx = i*M_;
+ // Find the max and subtract it so that the exp doesn't explode
+ float mx = scale * in[b_idx];
+ for( int j = 1; j < M_; j++ )
+ if( mx < scale * in[b_idx + j] )
+ mx = scale * in[b_idx + j];
+ float tt = 0;
+ for( int j = 0; j < M_; j++ ){
+ V[j] = expf( scale * in[b_idx + j] - mx );
+ tt += V[j];
+ }
+ // Make it a probability
+ for( int j = 0; j < M_; j++ )
+ V[j] /= tt;
+
+ int a_idx = i*M_;
+ for( int j = 0; j < M_; j++ )
+ if (relax == 1)
+ out[a_idx + j] = V[j];
+ else
+ out[a_idx + j] = (1-relax) * out[a_idx + j] + relax * V[j];
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::DenseCrf::runInference (float relax)
+{
+ // set the unary potentials
+ for (size_t i = 0; i < unary_.size (); i++)
+ next_[i] = -unary_[i];
+
+ // Add up all pairwise potentials
+ for( unsigned int i = 0; i < pairwise_potential_.size(); i++ )
+ pairwise_potential_[i]->compute( next_, current_, tmp_, M_ );
+
+ // Exponentiate and normalize
+ expAndNormalize( current_, next_, 1.0, relax );
+}
+
+void
+pcl::DenseCrf::getBarycentric (int idx, std::vector<float> &bary)
+{
+ bary = pairwise_potential_[idx]->bary_;
+}
+
+void
+pcl::DenseCrf::getFeatures (int idx, std::vector<float> &features)
+{
+ features = pairwise_potential_[idx]->features_;
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author : Christian Potthast
+ * Email : potthast@usc.edu
+ *
+ */
+
+/*
+#include <pcl/point_types.h>
+#include <pcl/impl/instantiate.hpp>
+#include <pcl/ml/kmeans.h>
+#include <pcl/ml/impl/kmeans.hpp>
+
+// Instantiations of all point types
+PCL_INSTANTIATE(Kmeans, PCL_POINT_TYPES);
+*/
+
+#include <pcl/ml/kmeans.h>
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::Kmeans::Kmeans (unsigned int num_points, unsigned int num_dimensions)
+ : num_points_ (num_points), num_dimensions_ (num_dimensions),
+ points_to_clusters_(num_points_, 0)
+ //data_ (num_points_, Point (num_dimensions_))
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::Kmeans::~Kmeans ()
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::Kmeans::initialClusterPoints()
+{
+ ClusterId i = 0;
+ unsigned int dim;
+ for (; i < num_clusters_; i++){
+ Point point; // each centroid is a point
+ for (dim=0; dim<num_dimensions_; dim++)
+ point.push_back(0.0);
+ SetPoints set_of_points;
+
+ // init centroids
+ centroids_.push_back(point);
+
+ // init clusterId -> set of points
+ clusters_to_points_.push_back(set_of_points);
+ }
+
+
+
+ ClusterId cid;
+
+ for (PointId pid = 0; pid < num_points_; pid++){
+
+ cid = pid % num_clusters_;
+
+ points_to_clusters_[pid] = cid;
+ clusters_to_points_[cid].insert(pid);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::Kmeans::computeCentroids()
+{
+ unsigned int i;
+ ClusterId cid = 0;
+ PointId num_points_in_cluster;
+ // For each centroid
+ BOOST_FOREACH(Centroids::value_type& centroid, centroids_)
+ {
+ num_points_in_cluster = 0;
+
+ // For earch PointId in this set
+ BOOST_FOREACH(SetPoints::value_type pid, clusters_to_points_[cid])
+ {
+ Point p = data_[pid];
+ //Point p = ps__.getPoint(pid);
+ for (i=0; i<num_dimensions_; i++)
+ centroid[i] += p[i];
+ num_points_in_cluster++;
+ }
+ // if no point in the clusters, this goes to inf (correct!)
+ for (i=0; i<num_dimensions_; i++)
+ {
+ centroid[i] /= static_cast<float> (num_points_in_cluster);
+ //std::cout << centroid[i] << " ";
+ }
+ //std::cout << std::endl;
+
+ cid++;
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::Kmeans::kMeans ()
+{
+ bool not_converged = true;
+ bool move;
+ unsigned int num_iterations = 0;
+ PointId pid;
+ ClusterId cid, to_cluster;
+ float d, min;
+
+ // Initial partition of points
+ initialClusterPoints();
+
+ // Until not converge
+ while (not_converged){
+
+ not_converged = false;
+
+ computeCentroids();
+
+ // for each point
+ for (pid=0; pid<num_points_; pid++)
+ {
+ // distance from current cluster
+ min = distance(centroids_[points_to_clusters_[pid]], data_[pid]);
+
+ // foreach centroid
+ cid = 0;
+ move = false;
+ BOOST_FOREACH(Centroids::value_type c, centroids_)
+ {
+ d = distance(c, data_[pid]);
+ if (d < min)
+ {
+ min = d;
+ move = true;
+ to_cluster = cid;
+
+ // remove from current cluster
+ clusters_to_points_[points_to_clusters_[pid]].erase(pid);
+
+ not_converged = true;
+ }
+ cid++;
+ }
+
+ // move towards a closer centroid
+ if (move){
+ // insert
+ points_to_clusters_[pid] = to_cluster;
+ clusters_to_points_[to_cluster].insert(pid);
+ }
+ }
+ num_iterations++;
+ } // end while
+}
+
+
+
+
+/*
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::Kmeans::cluster (std::vector<PointIndices> &clusters)
+{
+ if (!initCompute () ||
+ (input_ != 0 && input_->points.empty ()) ||
+ (indices_ != 0 && indices_->empty ()))
+ {
+ clusters.clear ();
+ return;
+ }
+
+ pcl::PointCloud <PointT> point;
+ std::vector<pcl::PCLPointField> fields;
+
+ int user_index = -1;
+ // if no cluster field name is set, check for X Y Z
+ if (strcmp (cluster_field_name_.c_str (), "") == 0)
+ {
+ int x_index = -1;
+ int y_index = -1;
+ int z_index = -1;
+ x_index = pcl::getFieldIndex (point, "x", fields);
+ if (y_index != -1)
+ y_index = pcl::getFieldIndex (point, "y", fields);
+ if (z_index != -1)
+ z_index = pcl::getFieldIndex (point, "z", fields);
+
+ if (x_index == -1 && y_index == -1 && z_index == -1)
+ {
+ PCL_ERROR ("Failed to find match for field 'x y z'\n" );
+ return;
+ }
+
+ PCL_INFO ("Use X Y Z as input data\n");
+*/
+
+ // create input data
+/*
+ for (size_t i = 0; i < input_->points.size (); i++)
+ {
+ DataPoint data (3);
+ data[0] = input_->points[i].data[0];
+
+
+
+ }
+*/
+
+ /*
+ std::cout << "x index: " << x_index << std::endl;
+
+ float x = 0.0;
+ memcpy (&x, &input_->points[0] + fields[x_index].offset, sizeof(float));
+
+ std::cout << "xxx: " << x << std::endl;
+ */
+
+ //memcpy (&x, reinterpret_cast<float*> (&input_->points[0]) + x_index, sizeof (float));
+
+
+ //int rgba_index = 1;
+
+ //pcl::RGB rgb;
+ //memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[index_vector[i].cloud_point_index]) + rgba_index, sizeof (RGB));
+
+
+ /*
+ }
+ // if cluster field name is set, check if field name is valied
+ else
+ {
+ user_index = pcl::getFieldIndex (point, cluster_field_name_.c_str (), fields);
+
+ if (user_index == -1)
+ {
+ PCL_ERROR ("Failed to find match for field '%s'\n", cluster_field_name_.c_str ());
+ return;
+ }
+ }
+ */
+
+
+
+/*
+ int xyz_index = -1;
+ pcl::PointCloud <PointT> point;
+ xyz_index = pcl::getFieldIndex (point, "r", fields);
+
+
+ if (xyz_index == -1 && strcmp (cluster_field_name_.c_str (), "") == 0)
+ {
+ PCL_ERROR ("Failed to find match for field '%s'\n", cluster_field_name_.c_str ());
+ }
+
+
+ std::cout << "index: " << xyz_index << std::endl;
+
+ std::string t = pcl::getFieldsList (point);
+ std::cout << "t: " << t << std::endl;
+*/
+
+ //std::vector <pcl::PCLPointField> fields;
+ //pcl::getFieldIndex (*input_, "xyz", fields);
+
+
+ //std::cout << "field: " << fields[xyz_index].count << std::endl;
+
+
+/*
+ for (size_t i = 0; i < fields[vfh_idx].count; ++i)
+ {
+
+ //vfh.second[i] = point.points[0].histogram[i];
+
+ }
+*/
+
+ /*
+
+ deinitCompute ();
+}
+ */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author : Christian Potthast
+ * Email : potthast@usc.edu
+ *
+ */
+
+#include <pcl/ml/pairwise_potential.h>
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+pcl::PairwisePotential::PairwisePotential (const std::vector<float> &feature,
+ const int feature_dimension,
+ const int N, const float w) :
+ N_ (N), w_ (w)
+{
+ //lattice_.init (feature, feature_dimension, N);
+ std::cout << "0---------" << std::endl;
+ lattice_.init (feature, feature_dimension, N);
+
+ std::cout << "1---------" << std::endl;
+
+ //lattice_.debug ();
+
+ norm_.resize (N);
+ for (int i = 0; i < N; i++)
+ norm_[i] = 1;
+
+ // Compute the normalization factor
+
+ //lattice_.compute (norm_, norm_, 1);
+
+ /*
+ std::vector<float> normOLD;
+ normOLD.resize (N);
+ for (int i = 0; i < N; i++)
+ normOLD[i] = 1;
+ */
+
+ std::cout << "2---------" << std::endl;
+
+ lattice_.compute (norm_, norm_, 1);
+
+
+ std::cout << "3---------" << std::endl;
+
+/*
+ ///////////
+ // DEBUG //
+ bool same = true;
+ for (size_t i = 0; i < normOLD.size (); i++)
+ {
+ if (norm_[i] != normOLD[i])
+ same = false;
+ }
+ if (same)
+ std::cout << "DEBUG norm - OK" << std::endl;
+ else
+ std::cout << "DEBUG norm - ERROR" << std::endl;
+*/
+
+
+ // per pixel normalization
+ for (int i = 0; i < N; i++)
+ norm_[i] = 1.0f / (norm_[i] + 1e-20f);
+
+ std::cout << "4---------" << std::endl;
+
+ bary_ = lattice_.barycentric_;
+
+ std::cout << "5---------" << std::endl;
+
+ features_ = feature;
+
+ std::cout << "6---------" << std::endl;
+
+
+/*
+ std::cout << "bary size: " << bary_.size () << std::endl;
+ for (int g = 0; g < 25; g++)
+ std::cout << bary_[g] << std::endl;
+*/
+
+
+
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::PairwisePotential::compute (std::vector<float> &out, const std::vector<float> &in,
+ std::vector<float> &tmp, int value_size) const
+{
+ lattice_.compute (tmp, in, value_size);
+ for (int i = 0, k = 0; i < N_; i++)
+ for (int j = 0; j < value_size; j++, k++)
+ out[k] += w_ * norm_[i] * tmp[k];
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/ml/permutohedral.h>
+
+///////////////////////////////////////////////////////////////////////////////////////////
+pcl::Permutohedral::Permutohedral () :
+ N_ (0), M_ (0), d_ (0),
+ blur_neighborsOLD_(NULL), offsetOLD_ (NULL), barycentricOLD_ (NULL)
+{}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::Permutohedral::init (const std::vector<float> &feature, const int feature_dimension, const int N)
+{
+ N_ = N;
+ d_ = feature_dimension;
+
+ // Create hash table
+ std::vector<std::vector<short> > keys;
+ keys.reserve ((d_+1) * N_);
+ std::multimap<size_t, int> hash_table;
+
+ // reserve class memory
+ if (offset_.size () > 0)
+ offset_.clear ();
+ offset_.resize ((d_ + 1) * N_);
+
+ if (barycentric_.size () > 0)
+ barycentric_.clear ();
+ barycentric_.resize ((d_ + 1) * N_);
+
+ // create vectors and matrices
+ Eigen::VectorXf scale_factor = Eigen::VectorXf::Zero (d_);
+ Eigen::VectorXf elevated = Eigen::VectorXf::Zero (d_ + 1);
+ Eigen::VectorXf rem0 = Eigen::VectorXf::Zero (d_+1);
+ Eigen::VectorXf barycentric = Eigen::VectorXf::Zero (d_+2);
+ Eigen::VectorXi rank = Eigen::VectorXi::Zero (d_+1);
+ Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic> canonical;
+ canonical = Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic>::Zero (d_+1, d_+1);
+ //short * key = new short[d_+1];
+ std::vector<short> key (d_+1);
+
+ // Compute the canonical simple
+ for (int i = 0; i <= d_; i++)
+ {
+ for (int j = 0; j <= (d_ - i); j++)
+ canonical (j, i) = i;
+ for (int j = (d_ - i + 1); j <= d_; j++)
+ canonical (j, i) = i - (d_ + 1);
+ }
+
+ // Expected standard deviation of our filter (p.6 in [Adams etal 2010])
+ float inv_std_dev = sqrtf (2.0f / 3.0f) * static_cast<float> (d_ + 1);
+
+ // Compute the diagonal part of E (p.5 in [Adams etal 2010])
+ for (int i = 0; i < d_; i++)
+ scale_factor (i) = 1.0f / sqrtf (static_cast<float> (i + 2) * static_cast<float> (i + 1)) * inv_std_dev;
+
+ // Compute the simplex each feature lies in
+ for (int k = 0; k < N_; k++)
+ //for (int k = 0; k < 5; k++)
+ {
+
+ // Elevate the feature (y = Ep, see p.5 in [Adams etal 2010])
+ int index = k * feature_dimension;
+ // sm contains the sum of 1..n of our faeture vector
+ float sm = 0;
+ for (int j = d_; j > 0; j--)
+ {
+ float cf = feature[index + j-1] * scale_factor (j-1);
+ elevated (j) = sm - static_cast<float> (j) * cf;
+ sm += cf;
+ }
+ elevated (0) = sm;
+
+ // Find the closest 0-colored simplex through rounding
+ float down_factor = 1.0f / static_cast<float>(d_+1);
+ float up_factor = static_cast<float>(d_+1);
+ int sum = 0;
+ for (int j = 0; j <= d_; j++){
+ float rd = floorf (0.5f + (down_factor * elevated (j))) ;
+ rem0 (j) = rd * up_factor;
+ sum += static_cast<int> (rd);
+ }
+
+ // rank differential to find the permutation between this simplex and the canonical one.
+ // (See pg. 3-4 in paper.)
+ rank.setZero ();
+ Eigen::VectorXf tmp = elevated - rem0;
+ for (int i = 0; i < d_; i++){
+ for (int j = i+1; j <= d_; j++)
+ if (tmp (i) < tmp (j))
+ rank (i)++;
+ else
+ rank (j)++;
+ }
+
+ // If the point doesn't lie on the plane (sum != 0) bring it back
+ for (int j = 0; j <= d_; j++){
+ rank (j) += sum;
+ if (rank (j) < 0){
+ rank (j) += d_+1;
+ rem0 (j) += static_cast<float> (d_ + 1);
+ }
+ else if (rank (j) > d_){
+ rank (j) -= d_+1;
+ rem0 (j) -= static_cast<float> (d_ + 1);
+ }
+ }
+
+ // Compute the barycentric coordinates (p.10 in [Adams etal 2010])
+ barycentric.setZero ();
+ Eigen::VectorXf v = (elevated - rem0) * down_factor;
+ for (int j = 0; j <= d_; j++){
+ barycentric (d_ - rank (j) ) += v (j);
+ barycentric (d_ + 1 - rank (j)) -= v (j);
+ }
+ // Wrap around
+ barycentric (0) += 1.0f + barycentric (d_+1);
+
+ // Compute all vertices and their offset
+ for (int remainder = 0; remainder <= d_; remainder++)
+ {
+ for (int j = 0; j < d_; j++)
+ key[j] = static_cast<short> (rem0 (j) + static_cast<float> (canonical ( rank (j), remainder)));
+
+ // insert key in hash table
+ size_t hash_key = generateHashKey (key);
+ std::multimap<size_t ,int>::iterator it = hash_table.find (hash_key);
+ int key_index = -1;
+ if (it != hash_table.end ())
+ {
+ key_index = it->second;
+
+ // check if key is the right one
+ int tmp_key_index = -1;
+ //for (int ii = key_index; ii < keys.size (); ii++)
+ for (it = hash_table.find (hash_key); it != hash_table.end (); ++it)
+ {
+ int ii = it->second;
+ bool same = true;
+ std::vector<short> k = keys[ii];
+ for (size_t i_k = 0; i_k < k.size (); i_k++)
+ {
+ if (key[i_k] != k[i_k])
+ {
+ same = false;
+ break;
+ }
+ }
+
+ if (same)
+ {
+ tmp_key_index = ii;
+ break;
+ }
+ }
+
+ if (tmp_key_index == -1)
+ {
+ key_index = static_cast<int> (keys.size ());
+ keys.push_back (key);
+ hash_table.insert (std::pair<size_t, int> (hash_key, key_index));
+ }
+ else
+ key_index = tmp_key_index;
+ }
+
+ else
+ {
+ key_index = static_cast<int> (keys.size ());
+ keys.push_back (key);
+ hash_table.insert (std::pair<size_t, int> (hash_key, key_index));
+ }
+ offset_[ k * (d_ + 1) + remainder ] = static_cast<float> (key_index);
+
+ barycentric_[ k * (d_ + 1) + remainder ] = barycentric (remainder);
+ }
+ }
+
+ // Find the Neighbors of each lattice point
+
+ // Get the number of vertices in the lattice
+ M_ = static_cast<int> (hash_table.size());
+
+ // Create the neighborhood structure
+ if (blur_neighbors_.size () > 0)
+ blur_neighbors_.clear ();
+ blur_neighbors_.resize ((d_+1)*M_);
+
+ std::vector<short> n1 (d_+1);
+ std::vector<short> n2 (d_+1);
+
+ // For each of d+1 axes,
+ for (int j = 0; j <= d_; j++)
+ {
+ for (int i = 0; i < M_; i++)
+ {
+ std::vector<short> key = keys[i];
+
+ for (int k=0; k<d_; k++){
+ n1[k] = static_cast<short> (key[k] - 1);
+ n2[k] = static_cast<short> (key[k] + 1);
+ }
+ n1[j] = static_cast<short> (key[j] + d_);
+ n2[j] = static_cast<short> (key[j] - d_);
+
+ std::multimap<size_t ,int>::iterator it;
+ size_t hash_key;
+ int key_index = -1;
+ hash_key = generateHashKey (n1);
+ it = hash_table.find (hash_key);
+ if (it != hash_table.end ())
+ key_index = it->second;
+ blur_neighbors_[j*M_+i].n1 = key_index;
+
+ key_index = -1;
+ hash_key = generateHashKey (n2);
+ it = hash_table.find (hash_key);
+ if (it != hash_table.end ())
+ key_index = it->second;
+ blur_neighbors_[j*M_+i].n2 = key_index;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::Permutohedral::compute (std::vector<float> &out, const std::vector<float> &in,
+ int value_size,
+ int in_offset, int out_offset,
+ int in_size, int out_size) const
+{
+ if (in_size == -1) in_size = N_ - in_offset;
+ if (out_size == -1) out_size = N_ - out_offset;
+
+ // Shift all values by 1 such that -1 -> 0 (used for blurring)
+ std::vector<float> values ((M_+2)*value_size, 0.0f);
+ std::vector<float> new_values ((M_+2)*value_size, 0.0f);
+
+ // Splatting
+ for (int i = 0; i < in_size; i++)
+ {
+ for (int j = 0; j <= d_; j++)
+ {
+ int o = static_cast<int> (offset_[(in_offset + i) * (d_ + 1) + j]) + 1;
+ float w = barycentric_[(in_offset + i) * (d_ + 1) + j];
+ for (int k = 0; k < value_size; k++)
+ values[ o * value_size + k ] += w * in[ i * value_size + k ];
+ }
+ }
+
+ for (int j = 0; j <= d_; j++)
+ {
+ for (int i = 0; i < M_; i++)
+ {
+ int old_val_idx = (i+1) * value_size;
+ int new_val_idx = (i+1) * value_size;
+
+ int n1 = blur_neighbors_[j*M_+i].n1+1;
+ int n2 = blur_neighbors_[j*M_+i].n2+1;
+ int n1_val_idx = n1 * value_size;
+ int n2_val_idx = n2 * value_size;
+
+ for (int k = 0; k < value_size; k++)
+ new_values[new_val_idx + k] = values[old_val_idx + k] +
+ 0.5f * (values[n1_val_idx + k] + values[n2_val_idx + k]);
+ }
+ values.swap (new_values);
+ }
+
+ // Alpha is a magic scaling constant (write Andrew if you really wanna understand this)
+ float alpha = 1.0f / (1.0f + static_cast<float> (pow(2.0f, -d_)));
+
+ // Slicing
+ for (int i = 0; i < out_size; i++){
+ for (int k = 0; k < value_size; k++)
+ out[i * value_size + k] = 0;
+ for (int j = 0; j <= d_; j++){
+ int o = static_cast<int> (offset_[(out_offset + i) * (d_ + 1) + j]) + 1;
+ float w = barycentric_[(out_offset + i) * (d_ + 1) + j];
+ for (int k = 0; k <value_size; k++)
+ out[ i* value_size + k ] += w * values[ o * value_size + k ] * alpha;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::Permutohedral::initOLD (const std::vector<float> &feature, const int feature_dimension, const int N)
+{
+ // Compute the lattice coordinates for each feature [there is going to be a lot of magic here
+ N_ = N;
+ d_ = feature_dimension;
+ HashTableOLD hash_table(d_, N_*(d_+1));
+
+ // Allocate the class memory
+ if (offsetOLD_) delete [] offsetOLD_;
+ offsetOLD_ = new int[ (d_+1)*N_ ];
+ if (barycentricOLD_) delete [] barycentricOLD_;
+ barycentricOLD_ = new float[ (d_+1)*N_ ];
+
+ // Allocate the local memory
+ float * scale_factor = new float[d_];
+ float * elevated = new float[d_+1];
+ float * rem0 = new float[d_+1];
+ float * barycentric = new float[d_+2];
+ int * rank = new int[d_+1];
+ short * canonical = new short[(d_+1)*(d_+1)];
+ short * key = new short[d_+1];
+
+ // Compute the canonical simplex
+ for (int i=0; i<=d_; i++){
+ for (int j=0; j<=d_-i; j++)
+ canonical[i*(d_+1)+j] = static_cast<short> (i);
+ for (int j=d_-i+1; j<=d_; j++)
+ canonical[i*(d_+1)+j] = static_cast<short> (i - (d_+1));
+ }
+
+ // Expected standard deviation of our filter (p.6 in [Adams etal 2010])
+ float inv_std_dev = sqrtf (2.0f / 3.0f)* static_cast<float>(d_+1);
+ // Compute the diagonal part of E (p.5 in [Adams etal 2010])
+ for (int i=0; i<d_; i++)
+ scale_factor[i] = 1.0f / sqrtf (static_cast<float>(i+2)*static_cast<float>(i+1)) * inv_std_dev;
+
+ // Compute the simplex each feature lies in
+ for (int k=0; k<N_; k++)
+ {
+ //for (int k = 0; k < 500; k++){
+ // Elevate the feature (y = Ep, see p.5 in [Adams etal 2010])
+ //const float * f = feature + k*feature_size;
+ int index = k * feature_dimension;
+
+
+ // sm contains the sum of 1..n of our faeture vector
+ float sm = 0;
+ for (int j = d_; j > 0; j--)
+ {
+ //float cf = f[j-1]*scale_factor[j-1];
+ float cf = feature[index + j-1] * scale_factor[j-1];
+ elevated[j] = sm - static_cast<float>(j)*cf;
+ sm += cf;
+ }
+ elevated[0] = sm;
+
+ // Find the closest 0-colored simplex through rounding
+ float down_factor = 1.0f / static_cast<float>(d_+1);
+ float up_factor = static_cast<float>(d_+1);
+ int sum = 0;
+ for (int i=0; i<=d_; i++){
+ int rd = static_cast<int> (pcl_round (down_factor * elevated[i]));
+ rem0[i] = static_cast<float >(rd) * up_factor;
+ sum += rd;
+ }
+
+ // Find the simplex we are in and store it in rank (where rank describes what position coorinate i has in the sorted order of the features values)
+ for (int i=0; i<=d_; i++)
+ rank[i] = 0;
+ for (int i=0; i<d_; i++)
+ {
+ double di = elevated[i] - rem0[i];
+ for (int j=i+1; j<=d_; j++)
+ if (di < elevated[j] - rem0[j])
+ rank[i]++;
+ else
+ rank[j]++;
+ }
+
+ // If the point doesn't lie on the plane (sum != 0) bring it back
+ for (int i=0; i<=d_; i++){
+ rank[i] += sum;
+ if (rank[i] < 0){
+ rank[i] += d_+1;
+ rem0[i] += static_cast<float> (d_+1);
+ }
+ else if (rank[i] > d_){
+ rank[i] -= d_+1;
+ rem0[i] -= static_cast<float> (d_+1);
+ }
+ }
+
+ // Compute the barycentric coordinates (p.10 in [Adams etal 2010])
+ for (int i=0; i<=d_+1; i++)
+ barycentric[i] = 0;
+ for (int i=0; i<=d_; i++){
+ float v = (elevated[i] - rem0[i])*down_factor;
+ barycentric[d_-rank[i] ] += v;
+ barycentric[d_-rank[i]+1] -= v;
+ }
+ // Wrap around
+ barycentric[0] += 1.0f + barycentric[d_+1];
+
+ // Compute all vertices and their offset
+ for (int remainder = 0; remainder <= d_; remainder++)
+ {
+ for (int i = 0; i < d_; i++)
+ key[i] = static_cast<short int> (rem0[i] + canonical[remainder * (d_ + 1) + rank[i]]);
+ offsetOLD_[k*(d_+1)+remainder] = hash_table.find (key, true);
+ barycentricOLD_[k*(d_+1)+remainder] = barycentric[remainder];
+ baryOLD_.push_back (barycentric[remainder]);
+ }
+ }
+
+
+
+
+ delete [] scale_factor;
+ delete [] elevated;
+ delete [] rem0;
+ delete [] barycentric;
+ delete [] rank;
+ delete [] canonical;
+ delete [] key;
+
+ // Find the Neighbors of each lattice point
+
+ // Get the number of vertices in the lattice
+ M_ = hash_table.size();
+
+ // Create the neighborhood structure
+ if(blur_neighborsOLD_) delete[] blur_neighborsOLD_;
+ blur_neighborsOLD_ = new Neighbors[ (d_+1)*M_ ];
+
+ short * n1 = new short[d_+1];
+ short * n2 = new short[d_+1];
+
+ // For each of d+1 axes,
+ for (int j = 0; j <= d_; j++){
+ for (int i=0; i<M_; i++){
+ const short * key = hash_table.getKey(i);
+ //std::cout << *key << std::endl;
+ for (int k=0; k<d_; k++){
+ n1[k] = static_cast<short int> (key[k] - 1);
+ n2[k] = static_cast<short int> (key[k] + 1);
+ }
+ n1[j] = static_cast<short int> (key[j] + d_);
+ n2[j] = static_cast<short int> (key[j] - d_);
+
+ //std::cout << *n1 << " | " << *n2 << std::endl;
+
+ blur_neighborsOLD_[j*M_+i].n1 = hash_table.find(n1);
+ blur_neighborsOLD_[j*M_+i].n2 = hash_table.find(n2);
+/*
+ std::cout << hash_table.find(n1) << " | " <<
+ hash_table.find(n2) << std::endl;
+*/
+
+ }
+ }
+ delete[] n1;
+ delete[] n2;
+
+
+
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::Permutohedral::computeOLD (std::vector<float> &out, const std::vector<float> &in,
+ int value_size,
+ int in_offset, int out_offset,
+ int in_size, int out_size) const
+{
+ if (in_size == -1) in_size = N_ - in_offset;
+ if (out_size == -1) out_size = N_ - out_offset;
+
+ // Shift all values by 1 such that -1 -> 0 (used for blurring)
+ float * values = new float[ (M_+2)*value_size ];
+ float * new_values = new float[ (M_+2)*value_size ];
+
+ for (int i=0; i<(M_+2)*value_size; i++)
+ values[i] = new_values[i] = 0;
+
+ // Splatting
+ for (int i=0; i<in_size; i++){
+ for (int j=0; j<=d_; j++){
+ int o = static_cast<int> (offsetOLD_[(in_offset+i)*(d_+1)+j]) + 1;
+ float w = barycentricOLD_[(in_offset+i)*(d_+1)+j];
+ for (int k=0; k<value_size; k++)
+ values[ o*value_size+k ] += w * in[ i*value_size+k ];
+ }
+ }
+
+ for (int j=0; j<=d_; j++){
+ for (int i=0; i<M_; i++){
+ float * old_val = values + (i+1)*value_size;
+ float * new_val = new_values + (i+1)*value_size;
+
+ int n1 = blur_neighborsOLD_[j*M_+i].n1+1;
+ int n2 = blur_neighborsOLD_[j*M_+i].n2+1;
+ float * n1_val = values + n1*value_size;
+ float * n2_val = values + n2*value_size;
+ for (int k=0; k<value_size; k++)
+ new_val[k] = old_val[k]+0.5f * (n1_val[k] + n2_val[k]);
+ }
+ float * tmp = values;
+ values = new_values;
+ new_values = tmp;
+ }
+ // Alpha is a magic scaling constant (write Andrew if you really wanna understand this)
+ float alpha = 1.0f / (1.0f + powf (2.0f, - float (d_)));
+
+ // Slicing
+ for (int i=0; i<out_size; i++)
+ {
+ for (int k=0; k<value_size; k++)
+ out[i*value_size+k] = 0;
+ for (int j=0; j<=d_; j++){
+ int o = static_cast<int> (offsetOLD_[(out_offset+i)*(d_+1)+j]) + 1;
+ float w = barycentricOLD_[(out_offset+i)*(d_+1)+j];
+ for (int k=0; k<value_size; k++)
+ out[ i*value_size+k ] += w * values[ o*value_size+k ] * alpha;
+ }
+ }
+
+
+ delete[] values;
+ delete[] new_values;
+}
+
+
+////////////////
+// DEBUG //////
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::Permutohedral::debug ()
+{
+ bool same = true;
+ for (size_t i = 0; i < barycentric_.size (); i++)
+ {
+ if (barycentric_[i] != barycentricOLD_[i])
+ same = false;
+ if (offset_[i] != offsetOLD_[i])
+ {
+ same = false;
+ }
+
+ }
+
+ for (size_t i = 0; i < blur_neighbors_.size (); i++)
+ {
+ if (blur_neighbors_[i].n1 != blur_neighborsOLD_[i].n1)
+ same = false;
+ if (blur_neighbors_[i].n2 != blur_neighborsOLD_[i].n2)
+ same = false;
+ }
+
+
+ if (same)
+ std::cout << "DEBUG - OK" << std::endl;
+ else
+ std::cout << "DEBUG - ERROR" << std::endl;
+}
+
+/*
+std::vector<float>
+pcl::Permuohedral::getBarycentric ()
+{
+ return
+}
+*/
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/ml/point_xy_32f.h>
+
+#include <istream>
+#include <ostream>
+
+///////////////////////////////////////////////////////////////////////////////
+//pcl::PointXY32f::~PointXY32f ()
+//{
+//}
+
+///////////////////////////////////////////////////////////////////////////////
+pcl::PointXY32f
+pcl::PointXY32f::randomPoint (const int min_x, const int max_x, const int min_y, const int max_y)
+{
+ const float width = static_cast<float> (max_x - min_x);
+ const float height = static_cast<float> (max_y - min_y);
+
+ PointXY32f point;
+ point.x = width * static_cast<float> (rand ()) / static_cast<float> (RAND_MAX) + static_cast<float> (min_x);
+ point.y = height * static_cast<float> (rand ()) / static_cast<float> (RAND_MAX) + static_cast<float> (min_y);
+
+ return point;
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/ml/point_xy_32i.h>
+
+#include <istream>
+#include <ostream>
+
+///////////////////////////////////////////////////////////////////////////////
+//pcl::PointXY32i::~PointXY32i ()
+//{
+//}
+
+///////////////////////////////////////////////////////////////////////////////
+pcl::PointXY32i
+pcl::PointXY32i::randomPoint (const int min_x, const int max_x, const int min_y, const int max_y)
+{
+ const float width = static_cast<float> (max_x - min_x);
+ const float height = static_cast<float> (max_y - min_y);
+
+ PointXY32i point;
+ point.x = static_cast<int> ( width * static_cast<float> (rand ()) / static_cast<float> (RAND_MAX) + static_cast<float> (min_x));
+ point.y = static_cast<int> ( height * static_cast<float> (rand ()) / static_cast<float> (RAND_MAX) + static_cast<float> (min_y));
+
+ return point;
+}
--- /dev/null
+ /*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ * Copyright (c) 2000-2012 Chih-Chung Chang and Chih-Jen Lin
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of copyright holders nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef _LIBSVM_HPP_
+#define _LIBSVM_HPP_
+
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <ctype.h>
+#include <float.h>
+#include <string.h>
+#include <stdarg.h>
+#include <limits.h>
+#include <pcl/ml/svm.h>
+#include <iostream>
+int libsvm_version = LIBSVM_VERSION;
+typedef float Qfloat;
+typedef signed char schar;
+#ifndef min
+template <class T> static inline T min (T x, T y)
+{
+ return (x < y) ? x : y;
+}
+
+#endif
+#ifndef max
+template <class T> static inline T max (T x, T y)
+{
+ return (x > y) ? x : y;
+}
+
+#endif
+template <class T> static inline void swap (T& x, T& y)
+{
+ T t = x;
+ x = y;
+ y = t;
+}
+
+template <class S, class T> static inline void
+clone (T*& dst, S* src, int n)
+{
+ dst = new T[n];
+ memcpy (reinterpret_cast<void*> (dst), reinterpret_cast<const void*> (src), sizeof (T) *n);
+}
+
+static inline double powi (double base, int times)
+{
+ double tmp = base, ret = 1.0;
+
+ for (int t = times; t > 0; t /= 2)
+ {
+ if (t % 2 == 1)
+ ret *= tmp;
+
+ tmp = tmp * tmp;
+ }
+
+ return ret;
+}
+
+#define INF HUGE_VAL
+#define TAU 1e-12
+#define Malloc(type,n) static_cast<type *> (malloc((n)*sizeof(type)))
+#define Realloc(var,type,n) static_cast<type *> (realloc(var,(n)*sizeof(type)))
+
+static void print_string_stdout (const char *s)
+{
+ fputs (s, stdout);
+ fflush (stdout);
+}
+
+static void (*svm_print_string) (const char *) = &print_string_stdout;
+#if 1
+static void info (const char *fmt, ...)
+{
+ char buf[BUFSIZ];
+ va_list ap;
+ va_start (ap, fmt);
+ vsprintf (buf, fmt, ap);
+ va_end (ap);
+ (*svm_print_string) (buf);
+}
+
+#else
+static void info (const char *fmt, ...) {}
+
+#endif
+
+//
+// Kernel Cache
+//
+// l is the number of total data items
+// size is the cache size limit in bytes
+//
+
+class Cache
+{
+
+ public:
+ Cache (int l, long int size);
+ ~Cache();
+
+ // request data [0,len)
+ // return some position p where [p,len) need to be filled
+ // (p >= len if nothing needs to be filled)
+ int get_data (const int index, Qfloat **data, int len);
+ void swap_index (int i, int j);
+
+ private:
+ int l;
+ long int size;
+
+ struct head_t
+ {
+ head_t *prev, *next; // a circular list
+ Qfloat *data;
+ int len; // data[0,len) is cached in this entry
+ };
+
+ head_t *head;
+ head_t lru_head;
+ void lru_delete (head_t *h);
+ void lru_insert (head_t *h);
+};
+
+Cache::Cache (int l_, long int size_) : l (l_), size (size_)
+{
+ head = static_cast<head_t *> (calloc (l, sizeof (head_t))); // initialized to 0
+ size /= sizeof (Qfloat);
+ size -= l * sizeof (head_t) / sizeof (Qfloat);
+ size = max (size, 2 * static_cast<long int> (l)); // cache must be large enough for two columns
+ lru_head.next = lru_head.prev = &lru_head;
+}
+
+Cache::~Cache()
+{
+ for (head_t *h = lru_head.next; h != &lru_head; h = h->next)
+ free (h->data);
+
+ free (head);
+}
+
+void Cache::lru_delete (head_t *h)
+{
+ // delete from current location
+ h->prev->next = h->next;
+ h->next->prev = h->prev;
+}
+
+void Cache::lru_insert (head_t *h)
+{
+ // insert to last position
+ h->next = &lru_head;
+ h->prev = lru_head.prev;
+ h->prev->next = h;
+ h->next->prev = h;
+}
+
+int Cache::get_data (const int index, Qfloat **data, int len)
+{
+ head_t *h = &head[index];
+
+ if (h->len)
+ lru_delete (h);
+
+ int more = len - h->len;
+
+ if (more > 0)
+ {
+ // free old space
+ while (size < more)
+ {
+ head_t *old = lru_head.next;
+ lru_delete (old);
+ free (old->data);
+ size += old->len;
+ old->data = 0;
+ old->len = 0;
+ }
+
+ // allocate new space
+ h->data = static_cast<Qfloat *> (realloc (h->data, sizeof (Qfloat) * len));
+
+ size -= more;
+
+ swap (h->len, len);
+ }
+
+ lru_insert (h);
+
+ *data = h->data;
+ return len;
+}
+
+void Cache::swap_index (int i, int j)
+{
+ if (i == j)
+ return;
+
+ if (head[i].len)
+ lru_delete (&head[i]);
+
+ if (head[j].len)
+ lru_delete (&head[j]);
+
+ swap (head[i].data, head[j].data);
+
+ swap (head[i].len, head[j].len);
+
+ if (head[i].len)
+ lru_insert (&head[i]);
+
+ if (head[j].len)
+ lru_insert (&head[j]);
+
+ if (i > j)
+ swap (i, j);
+
+ for (head_t *h = lru_head.next; h != &lru_head; h = h->next)
+ {
+ if (h->len > i)
+ {
+ if (h->len > j)
+ swap (h->data[i], h->data[j]);
+ else
+ {
+ // give up
+ lru_delete (h);
+ free (h->data);
+ size += h->len;
+ h->data = 0;
+ h->len = 0;
+ }
+ }
+ }
+}
+
+//
+// Kernel evaluation
+//
+// the static method k_function is for doing single kernel evaluation
+// the constructor of Kernel prepares to calculate the l*l kernel matrix
+// the member function get_Q is for getting one column from the Q Matrix
+//
+
+class QMatrix
+{
+
+ public:
+ virtual Qfloat *get_Q (int column, int len) const = 0;
+ virtual double *get_QD() const = 0;
+ virtual void swap_index (int i, int j) const = 0;
+ virtual ~QMatrix() {}
+};
+
+class Kernel: public QMatrix
+{
+
+ public:
+ Kernel (int l, svm_node * const * x, const svm_parameter& param);
+ virtual ~Kernel();
+
+ static double k_function (const svm_node *x, const svm_node *y,
+ const svm_parameter& param);
+ virtual Qfloat *get_Q (int column, int len) const = 0;
+ virtual double *get_QD() const = 0;
+ virtual void swap_index (int i, int j) const // no so const...
+ {
+ swap (x[i], x[j]);
+
+ if (x_square)
+ swap (x_square[i], x_square[j]);
+ }
+
+ protected:
+
+ double (Kernel::*kernel_function) (int i, int j) const;
+
+ private:
+ const svm_node **x;
+ double *x_square;
+
+ // svm_parameter
+ const int kernel_type;
+ const int degree;
+ const double gamma;
+ const double coef0;
+
+ static double dot (const svm_node *px, const svm_node *py);
+ double kernel_linear (int i, int j) const
+ {
+ return dot (x[i], x[j]);
+ }
+
+ double kernel_poly (int i, int j) const
+ {
+ return powi (gamma*dot (x[i], x[j]) + coef0, degree);
+ }
+
+ double kernel_rbf (int i, int j) const
+ {
+ return exp (-gamma* (x_square[i] + x_square[j] - 2*dot (x[i], x[j])));
+ }
+
+ double kernel_sigmoid (int i, int j) const
+ {
+ return tanh (gamma*dot (x[i], x[j]) + coef0);
+ }
+
+ double kernel_precomputed (int i, int j) const
+ {
+ return x[i][ int (x[j][0].value) ].value;
+ }
+};
+
+Kernel::Kernel (int l, svm_node * const * x_, const svm_parameter& param)
+ : kernel_type (param.kernel_type), degree (param.degree),
+ gamma (param.gamma), coef0 (param.coef0)
+{
+ switch (kernel_type)
+ {
+
+ case LINEAR:
+ kernel_function = &Kernel::kernel_linear;
+ break;
+
+ case POLY:
+ kernel_function = &Kernel::kernel_poly;
+ break;
+
+ case RBF:
+ kernel_function = &Kernel::kernel_rbf;
+ break;
+
+ case SIGMOID:
+ kernel_function = &Kernel::kernel_sigmoid;
+ break;
+
+ case PRECOMPUTED:
+ kernel_function = &Kernel::kernel_precomputed;
+ break;
+ }
+
+ clone (x, x_, l);
+
+ if (kernel_type == RBF)
+ {
+ x_square = new double[l];
+
+ for (int i = 0;i < l;i++)
+ x_square[i] = dot (x[i], x[i]);
+ }
+ else
+ x_square = 0;
+}
+
+Kernel::~Kernel()
+{
+ delete[] x;
+ delete[] x_square;
+}
+
+double Kernel::dot (const svm_node *px, const svm_node *py)
+{
+ double sum = 0;
+
+ while (px->index != -1 && py->index != -1)
+ {
+ if (px->index == py->index)
+ {
+ sum += px->value * py->value;
+ ++px;
+ ++py;
+ }
+ else
+ {
+ if (px->index > py->index)
+ ++py;
+ else
+ ++px;
+ }
+ }
+
+ return sum;
+}
+
+double Kernel::k_function (const svm_node *x, const svm_node *y,
+ const svm_parameter& param)
+{
+ switch (param.kernel_type)
+ {
+
+ case LINEAR:
+ return dot (x, y);
+
+ case POLY:
+ return powi (param.gamma*dot (x, y) + param.coef0, param.degree);
+
+ case RBF:
+ {
+ double sum = 0;
+
+ while (x->index != -1 && y->index != -1)
+ {
+ if (x->index == y->index)
+ {
+ double d = x->value - y->value;
+ sum += d * d;
+ ++x;
+ ++y;
+ }
+ else
+ {
+ if (x->index > y->index)
+ {
+ sum += y->value * y->value;
+ ++y;
+ }
+ else
+ {
+ sum += x->value * x->value;
+ ++x;
+ }
+ }
+ }
+
+ while (x->index != -1)
+ {
+ sum += x->value * x->value;
+ ++x;
+ }
+
+ while (y->index != -1)
+ {
+ sum += y->value * y->value;
+ ++y;
+ }
+
+ return exp (-param.gamma*sum);
+ }
+
+ case SIGMOID:
+ return tanh (param.gamma*dot (x, y) + param.coef0);
+
+ case PRECOMPUTED: //x: test (validation), y: SV
+ return x[ int (y->value) ].value;
+
+ default:
+ return 0; // Unreachable
+ }
+}
+
+// An SMO algorithm in Fan et al., JMLR 6(2005), p. 1889--1918
+// Solves:
+//
+// min 0.5(\alpha^T Q \alpha) + p^T \alpha
+//
+// y^T \alpha = \delta
+// y_i = +1 or -1
+// 0 <= alpha_i <= Cp for y_i = 1
+// 0 <= alpha_i <= Cn for y_i = -1
+//
+// Given:
+//
+// Q, p, y, Cp, Cn, and an initial feasible point \alpha
+// l is the size of vectors and matrices
+// eps is the stopping tolerance
+//
+// solution will be put in \alpha, objective value will be put in obj
+//
+
+class Solver
+{
+
+ public:
+ Solver() {};
+
+ virtual ~Solver() {};
+
+ struct SolutionInfo
+ {
+ double obj;
+ double rho;
+ double upper_bound_p;
+ double upper_bound_n;
+ double r; // for Solver_NU
+ };
+
+ void Solve (int l, const QMatrix& Q, const double *p_, const schar *y_,
+ double *alpha_, double Cp, double Cn, double eps,
+ SolutionInfo* si, int shrinking);
+
+ protected:
+ int active_size;
+ schar *y;
+ double *G; // gradient of objective function
+ enum { LOWER_BOUND, UPPER_BOUND, FREE };
+ char *alpha_status; // LOWER_BOUND, UPPER_BOUND, FREE
+ double *alpha;
+ const QMatrix *Q;
+ const double *QD;
+ double eps;
+ double Cp, Cn;
+ double *p;
+ int *active_set;
+ double *G_bar; // gradient, if we treat free variables as 0
+ int l;
+ bool unshrink; // XXX
+
+ double get_C (int i)
+ {
+ return (y[i] > 0) ? Cp : Cn;
+ }
+
+ void update_alpha_status (int i)
+ {
+ if (alpha[i] >= get_C (i))
+ alpha_status[i] = UPPER_BOUND;
+ else
+ if (alpha[i] <= 0)
+ alpha_status[i] = LOWER_BOUND;
+ else
+ alpha_status[i] = FREE;
+ }
+
+ bool is_upper_bound (int i)
+ {
+ return alpha_status[i] == UPPER_BOUND;
+ }
+
+ bool is_lower_bound (int i)
+ {
+ return alpha_status[i] == LOWER_BOUND;
+ }
+
+ bool is_free (int i)
+ {
+ return alpha_status[i] == FREE;
+ }
+
+ void swap_index (int i, int j);
+ void reconstruct_gradient();
+ virtual int select_working_set (int &i, int &j);
+ virtual double calculate_rho();
+ virtual void do_shrinking();
+
+ private:
+ bool be_shrunk (int i, double Gmax1, double Gmax2);
+};
+
+void Solver::swap_index (int i, int j)
+{
+ Q->swap_index (i, j);
+ swap (y[i], y[j]);
+ swap (G[i], G[j]);
+ swap (alpha_status[i], alpha_status[j]);
+ swap (alpha[i], alpha[j]);
+ swap (p[i], p[j]);
+ swap (active_set[i], active_set[j]);
+ swap (G_bar[i], G_bar[j]);
+}
+
+void Solver::reconstruct_gradient()
+{
+ // reconstruct inactive elements of G from G_bar and free variables
+
+ if (active_size == l)
+ return;
+
+ int i, j;
+
+ int nr_free = 0;
+
+ for (j = active_size;j < l;j++)
+ G[j] = G_bar[j] + p[j];
+
+ for (j = 0;j < active_size;j++)
+ if (is_free (j))
+ nr_free++;
+
+ if (2*nr_free < active_size)
+ info ("\nWARNING: using -h 0 may be faster\n");
+
+ if (nr_free*l > 2*active_size* (l - active_size))
+ {
+ for (i = active_size;i < l;i++)
+ {
+ const Qfloat *Q_i = Q->get_Q (i, active_size);
+
+ for (j = 0;j < active_size;j++)
+ if (is_free (j))
+ G[i] += alpha[j] * Q_i[j];
+ }
+ }
+ else
+ {
+ for (i = 0;i < active_size;i++)
+ if (is_free (i))
+ {
+ const Qfloat *Q_i = Q->get_Q (i, l);
+ double alpha_i = alpha[i];
+
+ for (j = active_size;j < l;j++)
+ G[j] += alpha_i * Q_i[j];
+ }
+ }
+}
+
+void Solver::Solve (int l, const QMatrix& Q, const double *p_, const schar *y_,
+ double *alpha_, double Cp, double Cn, double eps,
+ SolutionInfo* si, int shrinking)
+{
+ this->l = l;
+ this->Q = &Q;
+ QD = Q.get_QD();
+ clone (p, p_, l);
+ clone (y, y_, l);
+ clone (alpha, alpha_, l);
+ this->Cp = Cp;
+ this->Cn = Cn;
+ this->eps = eps;
+ unshrink = false;
+
+ // initialize alpha_status
+ {
+ alpha_status = new char[l];
+
+ for (int i = 0;i < l;i++)
+ update_alpha_status (i);
+ }
+
+ // initialize active set (for shrinking)
+ {
+ active_set = new int[l];
+
+ for (int i = 0;i < l;i++)
+ active_set[i] = i;
+
+ active_size = l;
+ }
+
+ // initialize gradient
+ {
+ G = new double[l];
+ G_bar = new double[l];
+ int i;
+
+ for (i = 0;i < l;i++)
+ {
+ G[i] = p[i];
+ G_bar[i] = 0;
+ }
+
+ for (i = 0;i < l;i++)
+ if (!is_lower_bound (i))
+ {
+ const Qfloat *Q_i = Q.get_Q (i, l);
+ double alpha_i = alpha[i];
+ int j;
+
+ for (j = 0;j < l;j++)
+ G[j] += alpha_i * Q_i[j];
+
+ if (is_upper_bound (i))
+ for (j = 0;j < l;j++)
+ G_bar[j] += get_C (i) * Q_i[j];
+ }
+ }
+
+ // optimization step
+
+ int iter = 0;
+ int max_iter = max (10000000, l > INT_MAX / 100 ? INT_MAX : 100 * l);
+ int counter = min (l, 1000) + 1;
+
+ while (iter < max_iter)
+ {
+ // show progress and do shrinking
+
+ if (--counter == 0)
+ {
+ counter = min (l, 1000);
+
+ if (shrinking)
+ do_shrinking();
+
+ info (".");
+ }
+
+ int i, j;
+
+ if (select_working_set (i, j) != 0)
+ {
+ // reconstruct the whole gradient
+ reconstruct_gradient();
+ // reset active set size and check
+ active_size = l;
+ info ("*");
+
+ if (select_working_set (i, j) != 0)
+ break;
+ else
+ counter = 1; // do shrinking next iteration
+ }
+
+ ++iter;
+
+ // update alpha[i] and alpha[j], handle bounds carefully
+
+ const Qfloat *Q_i = Q.get_Q (i, active_size);
+ const Qfloat *Q_j = Q.get_Q (j, active_size);
+
+ double C_i = get_C (i);
+ double C_j = get_C (j);
+
+ double old_alpha_i = alpha[i];
+ double old_alpha_j = alpha[j];
+
+ if (y[i] != y[j])
+ {
+ double quad_coef = QD[i] + QD[j] + 2 * Q_i[j];
+
+ if (quad_coef <= 0)
+ quad_coef = TAU;
+
+ double delta = (-G[i] - G[j]) / quad_coef;
+
+ double diff = alpha[i] - alpha[j];
+
+ alpha[i] += delta;
+
+ alpha[j] += delta;
+
+ if (diff > 0)
+ {
+ if (alpha[j] < 0)
+ {
+ alpha[j] = 0;
+ alpha[i] = diff;
+ }
+ }
+ else
+ {
+ if (alpha[i] < 0)
+ {
+ alpha[i] = 0;
+ alpha[j] = -diff;
+ }
+ }
+
+ if (diff > C_i - C_j)
+ {
+ if (alpha[i] > C_i)
+ {
+ alpha[i] = C_i;
+ alpha[j] = C_i - diff;
+ }
+ }
+ else
+ {
+ if (alpha[j] > C_j)
+ {
+ alpha[j] = C_j;
+ alpha[i] = C_j + diff;
+ }
+ }
+ }
+ else
+ {
+ double quad_coef = QD[i] + QD[j] - 2 * Q_i[j];
+
+ if (quad_coef <= 0)
+ quad_coef = TAU;
+
+ double delta = (G[i] - G[j]) / quad_coef;
+
+ double sum = alpha[i] + alpha[j];
+
+ alpha[i] -= delta;
+
+ alpha[j] += delta;
+
+ if (sum > C_i)
+ {
+ if (alpha[i] > C_i)
+ {
+ alpha[i] = C_i;
+ alpha[j] = sum - C_i;
+ }
+ }
+ else
+ {
+ if (alpha[j] < 0)
+ {
+ alpha[j] = 0;
+ alpha[i] = sum;
+ }
+ }
+
+ if (sum > C_j)
+ {
+ if (alpha[j] > C_j)
+ {
+ alpha[j] = C_j;
+ alpha[i] = sum - C_j;
+ }
+ }
+ else
+ {
+ if (alpha[i] < 0)
+ {
+ alpha[i] = 0;
+ alpha[j] = sum;
+ }
+ }
+ }
+
+ // update G
+
+ double delta_alpha_i = alpha[i] - old_alpha_i;
+
+ double delta_alpha_j = alpha[j] - old_alpha_j;
+
+ for (int k = 0;k < active_size;k++)
+ {
+ G[k] += Q_i[k] * delta_alpha_i + Q_j[k] * delta_alpha_j;
+ }
+
+ // update alpha_status and G_bar
+
+ {
+ bool ui = is_upper_bound (i);
+ bool uj = is_upper_bound (j);
+ update_alpha_status (i);
+ update_alpha_status (j);
+ int k;
+
+ if (ui != is_upper_bound (i))
+ {
+ Q_i = Q.get_Q (i, l);
+
+ if (ui)
+ for (k = 0;k < l;k++)
+ G_bar[k] -= C_i * Q_i[k];
+ else
+ for (k = 0;k < l;k++)
+ G_bar[k] += C_i * Q_i[k];
+ }
+
+ if (uj != is_upper_bound (j))
+ {
+ Q_j = Q.get_Q (j, l);
+
+ if (uj)
+ for (k = 0;k < l;k++)
+ G_bar[k] -= C_j * Q_j[k];
+ else
+ for (k = 0;k < l;k++)
+ G_bar[k] += C_j * Q_j[k];
+ }
+ }
+ }
+
+ if (iter >= max_iter)
+ {
+ if (active_size < l)
+ {
+ // reconstruct the whole gradient to calculate objective value
+ reconstruct_gradient();
+ active_size = l;
+ info ("*");
+ }
+
+ info ("\nWARNING: reaching max number of iterations");
+ }
+
+ // calculate rho
+
+ si->rho = calculate_rho();
+
+ // calculate objective value
+ {
+ double v = 0;
+ int i;
+
+ for (i = 0;i < l;i++)
+ v += alpha[i] * (G[i] + p[i]);
+
+ si->obj = v / 2;
+ }
+
+ // put back the solution
+ {
+ for (int i = 0;i < l;i++)
+ alpha_[active_set[i]] = alpha[i];
+ }
+
+ // juggle everything back
+ /*{
+ for(int i=0;i<l;i++)
+ while(active_set[i] != i)
+ swap_index(i,active_set[i]);
+ // or Q.swap_index(i,active_set[i]);
+ }*/
+
+ si->upper_bound_p = Cp;
+
+ si->upper_bound_n = Cn;
+
+ info ("\noptimization finished, #iter = %d\n", iter);
+
+ delete[] p;
+
+ delete[] y;
+
+ delete[] alpha;
+
+ delete[] alpha_status;
+
+ delete[] active_set;
+
+ delete[] G;
+
+ delete[] G_bar;
+}
+
+// return 1 if already optimal, return 0 otherwise
+int Solver::select_working_set (int &out_i, int &out_j)
+{
+ // return i,j such that
+ // i: maximizes -y_i * grad(f)_i, i in I_up(\alpha)
+ // j: minimizes the decrease of obj value
+ // (if quadratic coefficeint <= 0, replace it with tau)
+ // -y_j*grad(f)_j < -y_i*grad(f)_i, j in I_low(\alpha)
+
+ double Gmax = -INF;
+ double Gmax2 = -INF;
+ int Gmax_idx = -1;
+ int Gmin_idx = -1;
+ double obj_diff_min = INF;
+
+ for (int t = 0;t < active_size;t++)
+ if (y[t] == + 1)
+ {
+ if (!is_upper_bound (t))
+ if (-G[t] >= Gmax)
+ {
+ Gmax = -G[t];
+ Gmax_idx = t;
+ }
+ }
+ else
+ {
+ if (!is_lower_bound (t))
+ if (G[t] >= Gmax)
+ {
+ Gmax = G[t];
+ Gmax_idx = t;
+ }
+ }
+
+ int i = Gmax_idx;
+
+ const Qfloat *Q_i = NULL;
+
+ if (i != -1) // NULL Q_i not accessed: Gmax=-INF if i=-1
+ Q_i = Q->get_Q (i, active_size);
+
+ for (int j = 0;j < active_size;j++)
+ {
+ if (y[j] == + 1)
+ {
+ if (!is_lower_bound (j))
+ {
+ double grad_diff = Gmax + G[j];
+
+ if (G[j] >= Gmax2)
+ Gmax2 = G[j];
+
+ if (grad_diff > 0)
+ {
+ double obj_diff;
+ double quad_coef = QD[i] + QD[j] - 2.0 * y[i] * Q_i[j];
+
+ if (quad_coef > 0)
+ obj_diff = - (grad_diff * grad_diff) / quad_coef;
+ else
+ obj_diff = - (grad_diff * grad_diff) / TAU;
+
+ if (obj_diff <= obj_diff_min)
+ {
+ Gmin_idx = j;
+ obj_diff_min = obj_diff;
+ }
+ }
+ }
+ }
+ else
+ {
+ if (!is_upper_bound (j))
+ {
+ double grad_diff = Gmax - G[j];
+
+ if (-G[j] >= Gmax2)
+ Gmax2 = -G[j];
+
+ if (grad_diff > 0)
+ {
+ double obj_diff;
+ double quad_coef = QD[i] + QD[j] + 2.0 * y[i] * Q_i[j];
+
+ if (quad_coef > 0)
+ obj_diff = - (grad_diff * grad_diff) / quad_coef;
+ else
+ obj_diff = - (grad_diff * grad_diff) / TAU;
+
+ if (obj_diff <= obj_diff_min)
+ {
+ Gmin_idx = j;
+ obj_diff_min = obj_diff;
+ }
+ }
+ }
+ }
+ }
+
+ if (Gmax + Gmax2 < eps)
+ return 1;
+
+ out_i = Gmax_idx;
+
+ out_j = Gmin_idx;
+
+ return 0;
+}
+
+bool Solver::be_shrunk (int i, double Gmax1, double Gmax2)
+{
+ if (is_upper_bound (i))
+ {
+ if (y[i] == + 1)
+ return (-G[i] > Gmax1);
+ else
+ return (-G[i] > Gmax2);
+ }
+ else
+ if (is_lower_bound (i))
+ {
+ if (y[i] == + 1)
+ return (G[i] > Gmax2);
+ else
+ return (G[i] > Gmax1);
+ }
+ else
+ return (false);
+}
+
+void Solver::do_shrinking()
+{
+ int i;
+ double Gmax1 = -INF; // max { -y_i * grad(f)_i | i in I_up(\alpha) }
+ double Gmax2 = -INF; // max { y_i * grad(f)_i | i in I_low(\alpha) }
+
+ // find maximal violating pair first
+
+ for (i = 0;i < active_size;i++)
+ {
+ if (y[i] == + 1)
+ {
+ if (!is_upper_bound (i))
+ {
+ if (-G[i] >= Gmax1)
+ Gmax1 = -G[i];
+ }
+
+ if (!is_lower_bound (i))
+ {
+ if (G[i] >= Gmax2)
+ Gmax2 = G[i];
+ }
+ }
+ else
+ {
+ if (!is_upper_bound (i))
+ {
+ if (-G[i] >= Gmax2)
+ Gmax2 = -G[i];
+ }
+
+ if (!is_lower_bound (i))
+ {
+ if (G[i] >= Gmax1)
+ Gmax1 = G[i];
+ }
+ }
+ }
+
+ if (unshrink == false && Gmax1 + Gmax2 <= eps*10)
+ {
+ unshrink = true;
+ reconstruct_gradient();
+ active_size = l;
+ info ("*");
+ }
+
+ for (i = 0;i < active_size;i++)
+ if (be_shrunk (i, Gmax1, Gmax2))
+ {
+ active_size--;
+
+ while (active_size > i)
+ {
+ if (!be_shrunk (active_size, Gmax1, Gmax2))
+ {
+ swap_index (i, active_size);
+ break;
+ }
+
+ active_size--;
+ }
+ }
+}
+
+double Solver::calculate_rho()
+{
+ double r;
+ int nr_free = 0;
+ double ub = INF, lb = -INF, sum_free = 0;
+
+ for (int i = 0;i < active_size;i++)
+ {
+ double yG = y[i] * G[i];
+
+ if (is_upper_bound (i))
+ {
+ if (y[i] == -1)
+ ub = min (ub, yG);
+ else
+ lb = max (lb, yG);
+ }
+ else
+ if (is_lower_bound (i))
+ {
+ if (y[i] == + 1)
+ ub = min (ub, yG);
+ else
+ lb = max (lb, yG);
+ }
+ else
+ {
+ ++nr_free;
+ sum_free += yG;
+ }
+ }
+
+ if (nr_free > 0)
+ r = sum_free / nr_free;
+ else
+ r = (ub + lb) / 2;
+
+ return r;
+}
+
+//
+// Solver for nu-svm classification and regression
+//
+// additional constraint: e^T \alpha = constant
+//
+
+class Solver_NU : public Solver
+{
+
+ public:
+ Solver_NU() {}
+
+ void Solve (int l, const QMatrix& Q, const double *p, const schar *y,
+ double *alpha, double Cp, double Cn, double eps,
+ SolutionInfo* si, int shrinking)
+ {
+ this->si = si;
+ Solver::Solve (l, Q, p, y, alpha, Cp, Cn, eps, si, shrinking);
+ }
+
+ private:
+ SolutionInfo *si;
+ int select_working_set (int &i, int &j);
+ double calculate_rho();
+ bool be_shrunk (int i, double Gmax1, double Gmax2, double Gmax3, double Gmax4);
+ void do_shrinking();
+};
+
+// return 1 if already optimal, return 0 otherwise
+int Solver_NU::select_working_set (int &out_i, int &out_j)
+{
+ // return i,j such that y_i = y_j and
+ // i: maximizes -y_i * grad(f)_i, i in I_up(\alpha)
+ // j: minimizes the decrease of obj value
+ // (if quadratic coefficeint <= 0, replace it with tau)
+ // -y_j*grad(f)_j < -y_i*grad(f)_i, j in I_low(\alpha)
+
+ double Gmaxp = -INF;
+ double Gmaxp2 = -INF;
+ int Gmaxp_idx = -1;
+
+ double Gmaxn = -INF;
+ double Gmaxn2 = -INF;
+ int Gmaxn_idx = -1;
+
+ int Gmin_idx = -1;
+ double obj_diff_min = INF;
+
+ for (int t = 0;t < active_size;t++)
+ if (y[t] == + 1)
+ {
+ if (!is_upper_bound (t))
+ if (-G[t] >= Gmaxp)
+ {
+ Gmaxp = -G[t];
+ Gmaxp_idx = t;
+ }
+ }
+ else
+ {
+ if (!is_lower_bound (t))
+ if (G[t] >= Gmaxn)
+ {
+ Gmaxn = G[t];
+ Gmaxn_idx = t;
+ }
+ }
+
+ int ip = Gmaxp_idx;
+
+ int in = Gmaxn_idx;
+ const Qfloat *Q_ip = NULL;
+ const Qfloat *Q_in = NULL;
+
+ if (ip != -1) // NULL Q_ip not accessed: Gmaxp=-INF if ip=-1
+ Q_ip = Q->get_Q (ip, active_size);
+
+ if (in != -1)
+ Q_in = Q->get_Q (in, active_size);
+
+ for (int j = 0;j < active_size;j++)
+ {
+ if (y[j] == + 1)
+ {
+ if (!is_lower_bound (j))
+ {
+ double grad_diff = Gmaxp + G[j];
+
+ if (G[j] >= Gmaxp2)
+ Gmaxp2 = G[j];
+
+ if (grad_diff > 0)
+ {
+ double obj_diff;
+ double quad_coef = QD[ip] + QD[j] - 2 * Q_ip[j];
+
+ if (quad_coef > 0)
+ obj_diff = - (grad_diff * grad_diff) / quad_coef;
+ else
+ obj_diff = - (grad_diff * grad_diff) / TAU;
+
+ if (obj_diff <= obj_diff_min)
+ {
+ Gmin_idx = j;
+ obj_diff_min = obj_diff;
+ }
+ }
+ }
+ }
+ else
+ {
+ if (!is_upper_bound (j))
+ {
+ double grad_diff = Gmaxn - G[j];
+
+ if (-G[j] >= Gmaxn2)
+ Gmaxn2 = -G[j];
+
+ if (grad_diff > 0)
+ {
+ double obj_diff;
+ double quad_coef = QD[in] + QD[j] - 2 * Q_in[j];
+
+ if (quad_coef > 0)
+ obj_diff = - (grad_diff * grad_diff) / quad_coef;
+ else
+ obj_diff = - (grad_diff * grad_diff) / TAU;
+
+ if (obj_diff <= obj_diff_min)
+ {
+ Gmin_idx = j;
+ obj_diff_min = obj_diff;
+ }
+ }
+ }
+ }
+ }
+
+ if (max (Gmaxp + Gmaxp2, Gmaxn + Gmaxn2) < eps)
+ return 1;
+
+ if (y[Gmin_idx] == + 1)
+ out_i = Gmaxp_idx;
+ else
+ out_i = Gmaxn_idx;
+
+ out_j = Gmin_idx;
+
+ return 0;
+}
+
+bool Solver_NU::be_shrunk (int i, double Gmax1, double Gmax2, double Gmax3, double Gmax4)
+{
+ if (is_upper_bound (i))
+ {
+ if (y[i] == + 1)
+ return (-G[i] > Gmax1);
+ else
+ return (-G[i] > Gmax4);
+ }
+ else
+ if (is_lower_bound (i))
+ {
+ if (y[i] == + 1)
+ return (G[i] > Gmax2);
+ else
+ return (G[i] > Gmax3);
+ }
+ else
+ return (false);
+}
+
+void Solver_NU::do_shrinking()
+{
+ double Gmax1 = -INF; // max { -y_i * grad(f)_i | y_i = +1, i in I_up(\alpha) }
+ double Gmax2 = -INF; // max { y_i * grad(f)_i | y_i = +1, i in I_low(\alpha) }
+ double Gmax3 = -INF; // max { -y_i * grad(f)_i | y_i = -1, i in I_up(\alpha) }
+ double Gmax4 = -INF; // max { y_i * grad(f)_i | y_i = -1, i in I_low(\alpha) }
+
+ // find maximal violating pair first
+ int i;
+
+ for (i = 0;i < active_size;i++)
+ {
+ if (!is_upper_bound (i))
+ {
+ if (y[i] == + 1)
+ {
+ if (-G[i] > Gmax1)
+ Gmax1 = -G[i];
+ }
+ else
+ if (-G[i] > Gmax4)
+ Gmax4 = -G[i];
+ }
+
+ if (!is_lower_bound (i))
+ {
+ if (y[i] == + 1)
+ {
+ if (G[i] > Gmax2)
+ Gmax2 = G[i];
+ }
+ else
+ if (G[i] > Gmax3)
+ Gmax3 = G[i];
+ }
+ }
+
+ if (unshrink == false && max (Gmax1 + Gmax2, Gmax3 + Gmax4) <= eps*10)
+ {
+ unshrink = true;
+ reconstruct_gradient();
+ active_size = l;
+ }
+
+ for (i = 0;i < active_size;i++)
+ if (be_shrunk (i, Gmax1, Gmax2, Gmax3, Gmax4))
+ {
+ active_size--;
+
+ while (active_size > i)
+ {
+ if (!be_shrunk (active_size, Gmax1, Gmax2, Gmax3, Gmax4))
+ {
+ swap_index (i, active_size);
+ break;
+ }
+
+ active_size--;
+ }
+ }
+}
+
+double Solver_NU::calculate_rho()
+{
+ int nr_free1 = 0, nr_free2 = 0;
+ double ub1 = INF, ub2 = INF;
+ double lb1 = -INF, lb2 = -INF;
+ double sum_free1 = 0, sum_free2 = 0;
+
+ for (int i = 0;i < active_size;i++)
+ {
+ if (y[i] == + 1)
+ {
+ if (is_upper_bound (i))
+ lb1 = max (lb1, G[i]);
+ else
+ if (is_lower_bound (i))
+ ub1 = min (ub1, G[i]);
+ else
+ {
+ ++nr_free1;
+ sum_free1 += G[i];
+ }
+ }
+ else
+ {
+ if (is_upper_bound (i))
+ lb2 = max (lb2, G[i]);
+ else
+ if (is_lower_bound (i))
+ ub2 = min (ub2, G[i]);
+ else
+ {
+ ++nr_free2;
+ sum_free2 += G[i];
+ }
+ }
+ }
+
+ double r1, r2;
+
+ if (nr_free1 > 0)
+ r1 = sum_free1 / nr_free1;
+ else
+ r1 = (ub1 + lb1) / 2;
+
+ if (nr_free2 > 0)
+ r2 = sum_free2 / nr_free2;
+ else
+ r2 = (ub2 + lb2) / 2;
+
+ si->r = (r1 + r2) / 2;
+
+ return (r1 -r2) / 2;
+}
+
+//
+// Q matrices for various formulations
+//
+
+class SVC_Q: public Kernel
+{
+
+ public:
+ SVC_Q (const svm_problem& prob, const svm_parameter& param, const schar *y_)
+ : Kernel (prob.l, prob.x, param)
+ {
+ clone (y, y_, prob.l);
+ cache = new Cache (prob.l, static_cast<long int> (param.cache_size* (1 << 20)));
+ QD = new double[prob.l];
+
+ for (int i = 0;i < prob.l;i++)
+ QD[i] = (this->*kernel_function) (i, i);
+ }
+
+ Qfloat *get_Q (int i, int len) const
+ {
+ Qfloat *data;
+ int start, j;
+
+ if ( (start = cache->get_data (i, &data, len)) < len)
+ {
+ for (j = start;j < len;j++)
+ data[j] = Qfloat (y[i] * y[j] * (this->*kernel_function) (i, j));
+ }
+
+ return data;
+ }
+
+ double *get_QD() const
+ {
+ return QD;
+ }
+
+ void swap_index (int i, int j) const
+ {
+ cache->swap_index (i, j);
+ Kernel::swap_index (i, j);
+ swap (y[i], y[j]);
+ swap (QD[i], QD[j]);
+ }
+
+ ~SVC_Q()
+ {
+ delete[] y;
+ delete cache;
+ delete[] QD;
+ }
+
+ private:
+ schar *y;
+ Cache *cache;
+ double *QD;
+};
+
+class ONE_CLASS_Q: public Kernel
+{
+
+ public:
+ ONE_CLASS_Q (const svm_problem& prob, const svm_parameter& param)
+ : Kernel (prob.l, prob.x, param)
+ {
+ cache = new Cache (prob.l, static_cast<long int> (param.cache_size* (1 << 20)));
+ QD = new double[prob.l];
+
+ for (int i = 0;i < prob.l;i++)
+ QD[i] = (this->*kernel_function) (i, i);
+ }
+
+ Qfloat *get_Q (int i, int len) const
+ {
+ Qfloat *data;
+ int start, j;
+
+ if ( (start = cache->get_data (i, &data, len)) < len)
+ {
+ for (j = start;j < len;j++)
+ data[j] = Qfloat ((this->*kernel_function) (i, j));
+ }
+
+ return data;
+ }
+
+ double *get_QD() const
+ {
+ return QD;
+ }
+
+ void swap_index (int i, int j) const
+ {
+ cache->swap_index (i, j);
+ Kernel::swap_index (i, j);
+ swap (QD[i], QD[j]);
+ }
+
+ ~ONE_CLASS_Q()
+ {
+ delete cache;
+ delete[] QD;
+ }
+
+ private:
+ Cache *cache;
+ double *QD;
+};
+
+class SVR_Q: public Kernel
+{
+
+ public:
+ SVR_Q (const svm_problem& prob, const svm_parameter& param)
+ : Kernel (prob.l, prob.x, param)
+ {
+ l = prob.l;
+ cache = new Cache (l, static_cast<long int> (param.cache_size* (1 << 20)));
+ QD = new double[2*l];
+ sign = new schar[2*l];
+ index = new int[2*l];
+
+ for (int k = 0;k < l;k++)
+ {
+ sign[k] = 1;
+ sign[k+l] = -1;
+ index[k] = k;
+ index[k+l] = k;
+ QD[k] = (this->*kernel_function) (k, k);
+ QD[k+l] = QD[k];
+ }
+
+ buffer[0] = new Qfloat[2*l];
+
+ buffer[1] = new Qfloat[2*l];
+ next_buffer = 0;
+ }
+
+ void swap_index (int i, int j) const
+ {
+ swap (sign[i], sign[j]);
+ swap (index[i], index[j]);
+ swap (QD[i], QD[j]);
+ }
+
+ Qfloat *get_Q (int i, int len) const
+ {
+ Qfloat *data;
+ int j, real_i = index[i];
+
+ if (cache->get_data (real_i, &data, l) < l)
+ {
+ for (j = 0;j < l;j++)
+ data[j] = Qfloat ((this->*kernel_function) (real_i, j));
+ }
+
+ // reorder and copy
+ Qfloat *buf = buffer[next_buffer];
+
+ next_buffer = 1 - next_buffer;
+
+ schar si = sign[i];
+
+ for (j = 0;j < len;j++)
+ buf[j] = Qfloat (si) * Qfloat (sign[j]) * data[index[j]];
+
+ return buf;
+ }
+
+ double *get_QD() const
+ {
+ return QD;
+ }
+
+ ~SVR_Q()
+ {
+ delete cache;
+ delete[] sign;
+ delete[] index;
+ delete[] buffer[0];
+ delete[] buffer[1];
+ delete[] QD;
+ }
+
+ private:
+ int l;
+ Cache *cache;
+ schar *sign;
+ int *index;
+ mutable int next_buffer;
+ Qfloat *buffer[2];
+ double *QD;
+};
+
+//
+// construct and solve various formulations
+//
+static void solve_c_svc (
+ const svm_problem *prob, const svm_parameter* param,
+ double *alpha, Solver::SolutionInfo* si, double Cp, double Cn)
+{
+ int l = prob->l;
+ double *minus_ones = new double[l];
+ schar *y = new schar[l];
+
+ int i;
+
+ for (i = 0;i < l;i++)
+ {
+ alpha[i] = 0;
+ minus_ones[i] = -1;
+
+ if (prob->y[i] > 0)
+ y[i] = + 1;
+ else
+ y[i] = -1;
+ }
+
+ Solver s;
+
+ s.Solve (l, SVC_Q (*prob, *param, y), minus_ones, y,
+ alpha, Cp, Cn, param->eps, si, param->shrinking);
+
+ double sum_alpha = 0;
+
+ for (i = 0;i < l;i++)
+ sum_alpha += alpha[i];
+
+ if (Cp == Cn)
+ info ("nu = %f\n", sum_alpha / (Cp*prob->l));
+
+ for (i = 0;i < l;i++)
+ alpha[i] *= y[i];
+
+ delete[] minus_ones;
+
+ delete[] y;
+}
+
+static void solve_nu_svc (
+ const svm_problem *prob, const svm_parameter *param,
+ double *alpha, Solver::SolutionInfo* si)
+{
+ int i;
+ int l = prob->l;
+ double nu = param->nu;
+
+ schar *y = new schar[l];
+
+ for (i = 0;i < l;i++)
+ if (prob->y[i] > 0)
+ y[i] = + 1;
+ else
+ y[i] = -1;
+
+ double sum_pos = nu * l / 2;
+
+ double sum_neg = nu * l / 2;
+
+ for (i = 0;i < l;i++)
+ if (y[i] == + 1)
+ {
+ alpha[i] = min (1.0, sum_pos);
+ sum_pos -= alpha[i];
+ }
+ else
+ {
+ alpha[i] = min (1.0, sum_neg);
+ sum_neg -= alpha[i];
+ }
+
+ double *zeros = new double[l];
+
+ for (i = 0;i < l;i++)
+ zeros[i] = 0;
+
+ Solver_NU s;
+
+ s.Solve (l, SVC_Q (*prob, *param, y), zeros, y,
+ alpha, 1.0, 1.0, param->eps, si, param->shrinking);
+
+ double r = si->r;
+
+ info ("C = %f\n", 1 / r);
+
+ for (i = 0;i < l;i++)
+ alpha[i] *= y[i] / r;
+
+ si->rho /= r;
+
+ si->obj /= (r * r);
+
+ si->upper_bound_p = 1 / r;
+
+ si->upper_bound_n = 1 / r;
+
+ delete[] y;
+
+ delete[] zeros;
+}
+
+static void solve_one_class (
+ const svm_problem *prob, const svm_parameter *param,
+ double *alpha, Solver::SolutionInfo* si)
+{
+ int l = prob->l;
+ double *zeros = new double[l];
+ schar *ones = new schar[l];
+ int i;
+
+ int n = int (param->nu * prob->l); // # of alpha's at upper bound
+
+ for (i = 0;i < n;i++)
+ alpha[i] = 1;
+
+ if (n < prob->l)
+ alpha[n] = param->nu * prob->l - n;
+
+ for (i = n + 1;i < l;i++)
+ alpha[i] = 0;
+
+ for (i = 0;i < l;i++)
+ {
+ zeros[i] = 0;
+ ones[i] = 1;
+ }
+
+ Solver s;
+
+ s.Solve (l, ONE_CLASS_Q (*prob, *param), zeros, ones,
+ alpha, 1.0, 1.0, param->eps, si, param->shrinking);
+
+ delete[] zeros;
+ delete[] ones;
+}
+
+static void solve_epsilon_svr (
+ const svm_problem *prob, const svm_parameter *param,
+ double *alpha, Solver::SolutionInfo* si)
+{
+ int l = prob->l;
+ double *alpha2 = new double[2*l];
+ double *linear_term = new double[2*l];
+ schar *y = new schar[2*l];
+ int i;
+
+ for (i = 0;i < l;i++)
+ {
+ alpha2[i] = 0;
+ linear_term[i] = param->p - prob->y[i];
+ y[i] = 1;
+
+ alpha2[i+l] = 0;
+ linear_term[i+l] = param->p + prob->y[i];
+ y[i+l] = -1;
+ }
+
+ Solver s;
+
+ s.Solve (2*l, SVR_Q (*prob, *param), linear_term, y,
+ alpha2, param->C, param->C, param->eps, si, param->shrinking);
+
+ double sum_alpha = 0;
+
+ for (i = 0;i < l;i++)
+ {
+ alpha[i] = alpha2[i] - alpha2[i+l];
+ sum_alpha += fabs (alpha[i]);
+ }
+
+ info ("nu = %f\n", sum_alpha / (param->C*l));
+
+ delete[] alpha2;
+ delete[] linear_term;
+ delete[] y;
+}
+
+static void solve_nu_svr (
+ const svm_problem *prob, const svm_parameter *param,
+ double *alpha, Solver::SolutionInfo* si)
+{
+ int l = prob->l;
+ double C = param->C;
+ double *alpha2 = new double[2*l];
+ double *linear_term = new double[2*l];
+ schar *y = new schar[2*l];
+ int i;
+
+ double sum = C * param->nu * l / 2;
+
+ for (i = 0;i < l;i++)
+ {
+ alpha2[i] = alpha2[i+l] = min (sum, C);
+ sum -= alpha2[i];
+
+ linear_term[i] = - prob->y[i];
+ y[i] = 1;
+
+ linear_term[i+l] = prob->y[i];
+ y[i+l] = -1;
+ }
+
+ Solver_NU s;
+
+ s.Solve (2*l, SVR_Q (*prob, *param), linear_term, y,
+ alpha2, C, C, param->eps, si, param->shrinking);
+
+ info ("epsilon = %f\n", -si->r);
+
+ for (i = 0;i < l;i++)
+ alpha[i] = alpha2[i] - alpha2[i+l];
+
+ delete[] alpha2;
+
+ delete[] linear_term;
+
+ delete[] y;
+}
+
+//
+// decision_function
+//
+
+struct decision_function
+{
+ double *alpha;
+ double rho;
+};
+
+static decision_function svm_train_one (
+ const svm_problem *prob, const svm_parameter *param,
+ double Cp, double Cn)
+{
+ double *alpha = Malloc (double, prob->l);
+ Solver::SolutionInfo si;
+
+ switch (param->svm_type)
+ {
+
+ case C_SVC:
+ solve_c_svc (prob, param, alpha, &si, Cp, Cn);
+ break;
+
+ case NU_SVC:
+ solve_nu_svc (prob, param, alpha, &si);
+ break;
+
+ case ONE_CLASS:
+ solve_one_class (prob, param, alpha, &si);
+ break;
+
+ case EPSILON_SVR:
+ solve_epsilon_svr (prob, param, alpha, &si);
+ break;
+
+ case NU_SVR:
+ solve_nu_svr (prob, param, alpha, &si);
+ break;
+ }
+
+ info ("obj = %f, rho = %f\n", si.obj, si.rho);
+
+ // output SVs
+
+ int nSV = 0;
+ int nBSV = 0;
+
+ for (int i = 0;i < prob->l;i++)
+ {
+ if (fabs (alpha[i]) > 0)
+ {
+ ++nSV;
+
+ if (prob->y[i] > 0)
+ {
+ if (fabs (alpha[i]) >= si.upper_bound_p)
+ ++nBSV;
+ }
+ else
+ {
+ if (fabs (alpha[i]) >= si.upper_bound_n)
+ ++nBSV;
+ }
+ }
+ }
+
+ info ("nSV = %d, nBSV = %d\n", nSV, nBSV);
+
+ decision_function f;
+ f.alpha = alpha;
+ f.rho = si.rho;
+ return f;
+}
+
+// Platt's binary SVM Probablistic Output: an improvement from Lin et al.
+static void sigmoid_train (
+ int l, const double *dec_values, const double *labels,
+ double& A, double& B)
+{
+ double prior1 = 0, prior0 = 0;
+ int i;
+
+ for (i = 0;i < l;i++)
+ if (labels[i] > 0)
+ prior1 += 1;
+ else
+ prior0 += 1;
+
+ int max_iter = 100; // Maximal number of iterations
+
+ double min_step = 1e-10; // Minimal step taken in line search
+
+ double sigma = 1e-12; // For numerically strict PD of Hessian
+
+ double eps = 1e-5;
+
+ double hiTarget = (prior1 + 1.0) / (prior1 + 2.0);
+
+ double loTarget = 1 / (prior0 + 2.0);
+
+ double *t = Malloc (double, l);
+
+ double fApB, p, q, h11, h22, h21, g1, g2, det, dA, dB, gd, stepsize;
+
+ double newA, newB, newf, d1, d2;
+
+ int iter;
+
+ // Initial Point and Initial Fun Value
+ A = 0.0;
+
+ B = log ( (prior0 + 1.0) / (prior1 + 1.0));
+
+ double fval = 0.0;
+
+ for (i = 0;i < l;i++)
+ {
+ if (labels[i] > 0)
+ t[i] = hiTarget;
+ else
+ t[i] = loTarget;
+
+ fApB = dec_values[i] * A + B;
+
+ if (fApB >= 0)
+ fval += t[i] * fApB + log (1 + exp (-fApB));
+ else
+ fval += (t[i] - 1) * fApB + log (1 + exp (fApB));
+ }
+
+ for (iter = 0;iter < max_iter;iter++)
+ {
+ // Update Gradient and Hessian (use H' = H + sigma I)
+ h11 = sigma; // numerically ensures strict PD
+ h22 = sigma;
+ h21 = 0.0;
+ g1 = 0.0;
+ g2 = 0.0;
+
+ for (i = 0;i < l;i++)
+ {
+ fApB = dec_values[i] * A + B;
+
+ if (fApB >= 0)
+ {
+ p = exp (-fApB) / (1.0 + exp (-fApB));
+ q = 1.0 / (1.0 + exp (-fApB));
+ }
+ else
+ {
+ p = 1.0 / (1.0 + exp (fApB));
+ q = exp (fApB) / (1.0 + exp (fApB));
+ }
+
+ d2 = p * q;
+
+ h11 += dec_values[i] * dec_values[i] * d2;
+ h22 += d2;
+ h21 += dec_values[i] * d2;
+ d1 = t[i] - p;
+ g1 += dec_values[i] * d1;
+ g2 += d1;
+ }
+
+ // Stopping Criteria
+ if (fabs (g1) < eps && fabs (g2) < eps)
+ break;
+
+ // Finding Newton direction: -inv(H') * g
+ det = h11 * h22 - h21 * h21;
+
+ dA = - (h22 * g1 - h21 * g2) / det;
+
+ dB = - (-h21 * g1 + h11 * g2) / det;
+
+ gd = g1 * dA + g2 * dB;
+
+
+ stepsize = 1; // Line Search
+
+ while (stepsize >= min_step)
+ {
+ newA = A + stepsize * dA;
+ newB = B + stepsize * dB;
+
+ // New function value
+ newf = 0.0;
+
+ for (i = 0;i < l;i++)
+ {
+ fApB = dec_values[i] * newA + newB;
+
+ if (fApB >= 0)
+ newf += t[i] * fApB + log (1 + exp (-fApB));
+ else
+ newf += (t[i] - 1) * fApB + log (1 + exp (fApB));
+ }
+
+ // Check sufficient decrease
+ if (newf < fval + 0.0001*stepsize*gd)
+ {
+ A = newA;
+ B = newB;
+ fval = newf;
+ break;
+ }
+ else
+ stepsize = stepsize / 2.0;
+ }
+
+ if (stepsize < min_step)
+ {
+ info ("Line search fails in two-class probability estimates\n");
+ break;
+ }
+ }
+
+ if (iter >= max_iter)
+ info ("Reaching maximal iterations in two-class probability estimates\n");
+
+ free (t);
+}
+
+static double sigmoid_predict (double decision_value, double A, double B)
+{
+ double fApB = decision_value * A + B;
+ // 1-p used later; avoid catastrophic cancellation
+
+ if (fApB >= 0)
+ return exp (-fApB) / (1.0 + exp (-fApB));
+ else
+ return 1.0 / (1 + exp (fApB)) ;
+}
+
+// Method 2 from the multiclass_prob paper by Wu, Lin, and Weng
+static void multiclass_probability (int k, double **r, double *p)
+{
+ int t, j;
+ int iter = 0, max_iter = max (100, k);
+ double **Q = Malloc (double *, k);
+ double *Qp = Malloc (double, k);
+ double pQp, eps = 0.005 / k;
+
+ for (t = 0;t < k;t++)
+ {
+ p[t] = 1.0 / k; // Valid if k = 1
+ Q[t] = Malloc (double, k);
+ Q[t][t] = 0;
+
+ for (j = 0;j < t;j++)
+ {
+ Q[t][t] += r[j][t] * r[j][t];
+ Q[t][j] = Q[j][t];
+ }
+
+ for (j = t + 1;j < k;j++)
+ {
+ Q[t][t] += r[j][t] * r[j][t];
+ Q[t][j] = -r[j][t] * r[t][j];
+ }
+ }
+
+ for (iter = 0;iter < max_iter;iter++)
+ {
+ // stopping condition, recalculate QP,pQP for numerical accuracy
+ pQp = 0;
+
+ for (t = 0;t < k;t++)
+ {
+ Qp[t] = 0;
+
+ for (j = 0;j < k;j++)
+ Qp[t] += Q[t][j] * p[j];
+
+ pQp += p[t] * Qp[t];
+ }
+
+ double max_error = 0;
+
+ for (t = 0;t < k;t++)
+ {
+ double error = fabs (Qp[t] - pQp);
+
+ if (error > max_error)
+ max_error = error;
+ }
+
+ if (max_error < eps)
+ break;
+
+ for (t = 0;t < k;t++)
+ {
+ double diff = (-Qp[t] + pQp) / Q[t][t];
+ p[t] += diff;
+ pQp = (pQp + diff * (diff * Q[t][t] + 2 * Qp[t])) / (1 + diff) / (1 + diff);
+
+ for (j = 0;j < k;j++)
+ {
+ Qp[j] = (Qp[j] + diff * Q[t][j]) / (1 + diff);
+ p[j] /= (1 + diff);
+ }
+ }
+ }
+
+ if (iter >= max_iter)
+ info ("Exceeds max_iter in multiclass_prob\n");
+
+ for (t = 0;t < k;t++)
+ free (Q[t]);
+
+ free (Q);
+
+ free (Qp);
+}
+
+// Cross-validation decision values for probability estimates
+static void svm_binary_svc_probability (
+ const svm_problem *prob, const svm_parameter *param,
+ double Cp, double Cn, double& probA, double& probB)
+{
+ int i;
+ int nr_fold = 5;
+ int *perm = Malloc (int, prob->l);
+ double *dec_values = Malloc (double, prob->l);
+
+ // random shuffle
+
+ for (i = 0;i < prob->l;i++)
+ perm[i] = i;
+
+ for (i = 0;i < prob->l;i++)
+ {
+ int j = i + rand() % (prob->l - i);
+ swap (perm[i], perm[j]);
+ }
+
+ for (i = 0;i < nr_fold;i++)
+ {
+ int begin = i * prob->l / nr_fold;
+ int end = (i + 1) * prob->l / nr_fold;
+ int j, k;
+
+ struct svm_problem subprob;
+
+ subprob.l = prob->l - (end - begin);
+ subprob.x = Malloc (struct svm_node*, subprob.l);
+ subprob.y = Malloc (double, subprob.l);
+
+ k = 0;
+
+ for (j = 0;j < begin;j++)
+ {
+ subprob.x[k] = prob->x[perm[j]];
+ subprob.y[k] = prob->y[perm[j]];
+ ++k;
+ }
+
+ for (j = end;j < prob->l;j++)
+ {
+ subprob.x[k] = prob->x[perm[j]];
+ subprob.y[k] = prob->y[perm[j]];
+ ++k;
+ }
+
+ int p_count = 0, n_count = 0;
+
+ for (j = 0;j < k;j++)
+ if (subprob.y[j] > 0)
+ p_count++;
+ else
+ n_count++;
+
+ if (p_count == 0 && n_count == 0)
+ for (j = begin;j < end;j++)
+ dec_values[perm[j]] = 0;
+ else
+ if (p_count > 0 && n_count == 0)
+ for (j = begin;j < end;j++)
+ dec_values[perm[j]] = 1;
+ else
+ if (p_count == 0 && n_count > 0)
+ for (j = begin;j < end;j++)
+ dec_values[perm[j]] = -1;
+ else
+ {
+ svm_parameter subparam = *param;
+ subparam.probability = 0;
+ subparam.C = 1.0;
+ subparam.nr_weight = 2;
+ subparam.weight_label = Malloc (int, 2);
+ subparam.weight = Malloc (double, 2);
+ subparam.weight_label[0] = + 1;
+ subparam.weight_label[1] = -1;
+ subparam.weight[0] = Cp;
+ subparam.weight[1] = Cn;
+
+ struct svm_model *submodel = svm_train (&subprob, &subparam);
+
+ for (j = begin;j < end;j++)
+ {
+ svm_predict_values (submodel, prob->x[perm[j]], & (dec_values[perm[j]]));
+ // ensure +1 -1 order; reason not using CV subroutine
+ dec_values[perm[j]] *= submodel->label[0];
+ }
+
+ svm_free_and_destroy_model (&submodel);
+
+ svm_destroy_param (&subparam);
+ }
+
+ free (subprob.x);
+
+ free (subprob.y);
+ }
+
+ sigmoid_train (prob->l, dec_values, prob->y, probA, probB);
+
+ free (dec_values);
+ free (perm);
+}
+
+// Return parameter of a Laplace distribution
+static double svm_svr_probability (
+ const svm_problem *prob, const svm_parameter *param)
+{
+ int i;
+ int nr_fold = 5;
+ double *ymv = Malloc (double, prob->l);
+ double mae = 0;
+
+ svm_parameter newparam = *param;
+ newparam.probability = 0;
+ svm_cross_validation (prob, &newparam, nr_fold, ymv);
+
+ for (i = 0;i < prob->l;i++)
+ {
+ ymv[i] = prob->y[i] - ymv[i];
+ mae += fabs (ymv[i]);
+ }
+
+ mae /= prob->l;
+
+ double std = sqrt (2 * mae * mae);
+ int count = 0;
+ mae = 0;
+
+ for (i = 0;i < prob->l;i++)
+ if (fabs (ymv[i]) > 5*std)
+ count = count + 1;
+ else
+ mae += fabs (ymv[i]);
+
+ mae /= (prob->l - count);
+
+ info ("Prob. model for test data: target value = predicted value + z,\nz: Laplace distribution e^(-|z|/sigma)/(2sigma),sigma= %g\n", mae);
+
+ free (ymv);
+
+ return mae;
+}
+
+
+// label: label name, start: begin of each class, count: #data of classes, perm: indices to the original data
+// perm, length l, must be allocated before calling this subroutine
+static void svm_group_classes (const svm_problem *prob, int *nr_class_ret, int **label_ret, int **start_ret, int **count_ret, int *perm)
+{
+ int l = prob->l;
+ int max_nr_class = 16;
+ int nr_class = 0;
+ int *label = Malloc (int, max_nr_class);
+ int *count = Malloc (int, max_nr_class);
+ int *data_label = Malloc (int, l);
+ int i;
+
+ for (i = 0;i < l;i++)
+ {
+ int this_label = int (prob->y[i]);
+ int j;
+
+ for (j = 0;j < nr_class;j++)
+ {
+ if (this_label == label[j])
+ {
+ ++count[j];
+ break;
+ }
+ }
+
+ data_label[i] = j;
+
+ if (j == nr_class)
+ {
+ if (nr_class == max_nr_class)
+ {
+ max_nr_class *= 2;
+ label = static_cast<int *> (realloc (label, max_nr_class * sizeof (int)));
+ count = static_cast<int *> (realloc (count, max_nr_class * sizeof (int)));
+ }
+
+ label[nr_class] = this_label;
+
+ count[nr_class] = 1;
+ ++nr_class;
+ }
+ }
+
+ int *start = Malloc (int, nr_class);
+
+ start[0] = 0;
+
+ for (i = 1;i < nr_class;i++)
+ start[i] = start[i-1] + count[i-1];
+
+ for (i = 0;i < l;i++)
+ {
+ perm[start[data_label[i]]] = i;
+ ++start[data_label[i]];
+ }
+
+ start[0] = 0;
+
+ for (i = 1;i < nr_class;i++)
+ start[i] = start[i-1] + count[i-1];
+
+ *nr_class_ret = nr_class;
+
+ *label_ret = label;
+
+ *start_ret = start;
+
+ *count_ret = count;
+
+ free (data_label);
+}
+
+//
+// Interface functions
+//
+svm_model *svm_train (const svm_problem *prob, const svm_parameter *param)
+{
+ svm_model *model = Malloc (svm_model, 1);
+ model->param = *param;
+ model->free_sv = 0; // XXX
+ model->probA = NULL;
+ model->probB = NULL;
+
+ if (param->svm_type == ONE_CLASS ||
+ param->svm_type == EPSILON_SVR ||
+ param->svm_type == NU_SVR)
+ {
+ // regression or one-class-svm
+ model->nr_class = 2;
+ model->label = NULL;
+ model->nSV = NULL;
+ model->probA = NULL;
+ model->probB = NULL;
+ model->sv_coef = Malloc (double *, 1);
+
+ if (param->probability &&
+ (param->svm_type == EPSILON_SVR ||
+ param->svm_type == NU_SVR))
+ {
+ model->probA = Malloc (double, 1);
+ model->probA[0] = svm_svr_probability (prob, param);
+ }
+
+ decision_function f = svm_train_one (prob, param, 0, 0);
+
+ model->rho = Malloc (double, 1);
+ model->rho[0] = f.rho;
+
+ int nSV = 0;
+ int i;
+
+ for (i = 0;i < prob->l;i++)
+ if (fabs (f.alpha[i]) > 0)
+ ++nSV;
+
+ model->l = nSV;
+
+ model->SV = Malloc (svm_node *, nSV);
+
+ model->sv_coef[0] = Malloc (double, nSV);
+
+ int j = 0;
+
+ for (i = 0;i < prob->l;i++)
+ if (fabs (f.alpha[i]) > 0)
+ {
+ model->SV[j] = prob->x[i];
+ model->sv_coef[0][j] = f.alpha[i];
+ ++j;
+ }
+
+ free (f.alpha);
+ }
+ else
+ {
+ // classification
+ int l = prob->l;
+ int nr_class;
+ int *label = NULL;
+ int *start = NULL;
+ int *count = NULL;
+ int *perm = Malloc (int, l);
+
+ // group training data of the same class
+ svm_group_classes (prob, &nr_class, &label, &start, &count, perm);
+
+ if (nr_class == 1)
+ info ("WARNING: training data in only one class. See README for details.\n");
+
+ svm_node **x = Malloc (svm_node *, l);
+
+ int i;
+
+ for (i = 0;i < l;i++)
+ x[i] = prob->x[perm[i]];
+
+ // calculate weighted C
+
+ double *weighted_C = Malloc (double, nr_class);
+
+ for (i = 0;i < nr_class;i++)
+ weighted_C[i] = param->C;
+
+ for (i = 0;i < param->nr_weight;i++)
+ {
+ int j;
+
+ for (j = 0;j < nr_class;j++)
+ if (param->weight_label[i] == label[j])
+ break;
+
+ if (j == nr_class)
+ fprintf (stderr, "WARNING: class label %d specified in weight is not found\n", param->weight_label[i]);
+ else
+ weighted_C[j] *= param->weight[i];
+ }
+
+ // train k*(k-1)/2 models
+
+ bool *nonzero = Malloc (bool, l);
+
+ for (i = 0;i < l;i++)
+ nonzero[i] = false;
+
+ decision_function *f = Malloc (decision_function, nr_class * (nr_class - 1) / 2);
+
+ double *probA = NULL, *probB = NULL;
+
+ if (param->probability)
+ {
+ probA = Malloc (double, nr_class * (nr_class - 1) / 2);
+ probB = Malloc (double, nr_class * (nr_class - 1) / 2);
+ }
+
+ int p = 0;
+
+ for (i = 0;i < nr_class;i++)
+ for (int j = i + 1;j < nr_class;j++)
+ {
+ svm_problem sub_prob;
+ int si = start[i], sj = start[j];
+ int ci = count[i], cj = count[j];
+ sub_prob.l = ci + cj;
+ sub_prob.x = Malloc (svm_node *, sub_prob.l);
+ sub_prob.y = Malloc (double, sub_prob.l);
+ int k;
+
+ for (k = 0;k < ci;k++)
+ {
+ sub_prob.x[k] = x[si+k];
+ sub_prob.y[k] = + 1;
+ }
+
+ for (k = 0;k < cj;k++)
+ {
+ sub_prob.x[ci+k] = x[sj+k];
+ sub_prob.y[ci+k] = -1;
+ }
+
+ if (param->probability)
+ svm_binary_svc_probability (&sub_prob, param, weighted_C[i], weighted_C[j], probA[p], probB[p]);
+
+ f[p] = svm_train_one (&sub_prob, param, weighted_C[i], weighted_C[j]);
+
+ for (k = 0;k < ci;k++)
+ if (!nonzero[si+k] && fabs (f[p].alpha[k]) > 0)
+ nonzero[si+k] = true;
+
+ for (k = 0;k < cj;k++)
+ if (!nonzero[sj+k] && fabs (f[p].alpha[ci+k]) > 0)
+ nonzero[sj+k] = true;
+
+ free (sub_prob.x);
+
+ free (sub_prob.y);
+
+ ++p;
+ }
+
+ // build output
+
+ model->nr_class = nr_class;
+
+ model->label = Malloc (int, nr_class);
+
+ for (i = 0;i < nr_class;i++)
+ model->label[i] = label[i];
+
+ model->rho = Malloc (double, nr_class * (nr_class - 1) / 2);
+
+ for (i = 0;i < nr_class* (nr_class - 1) / 2;i++)
+ model->rho[i] = f[i].rho;
+
+ if (param->probability)
+ {
+ model->probA = Malloc (double, nr_class * (nr_class - 1) / 2);
+ model->probB = Malloc (double, nr_class * (nr_class - 1) / 2);
+
+ for (i = 0;i < nr_class* (nr_class - 1) / 2;i++)
+ {
+ model->probA[i] = probA[i];
+ model->probB[i] = probB[i];
+ }
+ }
+ else
+ {
+ model->probA = NULL;
+ model->probB = NULL;
+ }
+
+ int total_sv = 0;
+
+ int *nz_count = Malloc (int, nr_class);
+ model->nSV = Malloc (int, nr_class);
+
+ for (i = 0;i < nr_class;i++)
+ {
+ int nSV = 0;
+
+ for (int j = 0;j < count[i];j++)
+ if (nonzero[start[i] + j])
+ {
+ ++nSV;
+ ++total_sv;
+ }
+
+ model->nSV[i] = nSV;
+
+ nz_count[i] = nSV;
+ }
+
+ info ("Total nSV = %d\n", total_sv);
+
+ model->l = total_sv;
+ model->SV = Malloc (svm_node *, total_sv);
+ p = 0;
+
+ for (i = 0;i < l;i++)
+ if (nonzero[i])
+ model->SV[p++] = x[i];
+
+ int *nz_start = Malloc (int, nr_class);
+
+ nz_start[0] = 0;
+
+ for (i = 1;i < nr_class;i++)
+ nz_start[i] = nz_start[i-1] + nz_count[i-1];
+
+ model->sv_coef = Malloc (double *, nr_class - 1);
+
+ for (i = 0;i < nr_class - 1;i++)
+ model->sv_coef[i] = Malloc (double, total_sv);
+
+ p = 0;
+
+ for (i = 0;i < nr_class;i++)
+ for (int j = i + 1;j < nr_class;j++)
+ {
+ // classifier (i,j): coefficients with
+ // i are in sv_coef[j-1][nz_start[i]...],
+ // j are in sv_coef[i][nz_start[j]...]
+
+ int si = start[i];
+ int sj = start[j];
+ int ci = count[i];
+ int cj = count[j];
+
+ int q = nz_start[i];
+ int k;
+
+ for (k = 0;k < ci;k++)
+ if (nonzero[si+k])
+ model->sv_coef[j-1][q++] = f[p].alpha[k];
+
+ q = nz_start[j];
+
+ for (k = 0;k < cj;k++)
+ if (nonzero[sj+k])
+ model->sv_coef[i][q++] = f[p].alpha[ci+k];
+
+ ++p;
+ }
+
+ free (label);
+
+ free (probA);
+ free (probB);
+ free (count);
+ free (perm);
+ free (start);
+ free (x);
+ free (weighted_C);
+ free (nonzero);
+
+ for (i = 0;i < nr_class* (nr_class - 1) / 2;i++)
+ free (f[i].alpha);
+
+ free (f);
+
+ free (nz_count);
+
+ free (nz_start);
+ }
+
+ return model;
+}
+
+// Stratified cross validation
+void svm_cross_validation (const svm_problem *prob, const svm_parameter *param, int nr_fold, double *target)
+{
+ int i;
+ int *fold_start = Malloc (int, nr_fold + 1);
+ int l = prob->l;
+ int *perm = Malloc (int, l);
+ int nr_class;
+
+ // stratified cv may not give leave-one-out rate
+ // Each class to l folds -> some folds may have zero elements
+
+ if ( (param->svm_type == C_SVC ||
+ param->svm_type == NU_SVC) && nr_fold < l)
+ {
+ int *start = NULL;
+ int *label = NULL;
+ int *count = NULL;
+ svm_group_classes (prob, &nr_class, &label, &start, &count, perm);
+
+ // random shuffle and then data grouped by fold using the array perm
+ int *fold_count = Malloc (int, nr_fold);
+ int c;
+ int *index = Malloc (int, l);
+
+ for (i = 0;i < l;i++)
+ index[i] = perm[i];
+
+ for (c = 0; c < nr_class; c++)
+ for (i = 0;i < count[c];i++)
+ {
+ int j = i + rand() % (count[c] - i);
+ swap (index[start[c] + j], index[start[c] + i]);
+ }
+
+ for (i = 0;i < nr_fold;i++)
+ {
+ fold_count[i] = 0;
+
+ for (c = 0; c < nr_class;c++)
+ fold_count[i] += (i + 1) * count[c] / nr_fold - i * count[c] / nr_fold;
+ }
+
+ fold_start[0] = 0;
+
+ for (i = 1;i <= nr_fold;i++)
+ fold_start[i] = fold_start[i-1] + fold_count[i-1];
+
+ for (c = 0; c < nr_class;c++)
+ for (i = 0;i < nr_fold;i++)
+ {
+ int begin = start[c] + i * count[c] / nr_fold;
+ int end = start[c] + (i + 1) * count[c] / nr_fold;
+
+ for (int j = begin;j < end;j++)
+ {
+ perm[fold_start[i]] = index[j];
+ fold_start[i]++;
+ }
+ }
+
+ fold_start[0] = 0;
+
+ for (i = 1;i <= nr_fold;i++)
+ fold_start[i] = fold_start[i-1] + fold_count[i-1];
+
+ free (start);
+
+ free (label);
+
+ free (count);
+
+ free (index);
+
+ free (fold_count);
+ }
+ else
+ {
+ for (i = 0;i < l;i++)
+ perm[i] = i;
+
+ for (i = 0;i < l;i++)
+ {
+ int j = i + rand() % (l - i);
+ swap (perm[i], perm[j]);
+ }
+
+ for (i = 0;i <= nr_fold;i++)
+ fold_start[i] = i * l / nr_fold;
+ }
+
+ for (i = 0;i < nr_fold;i++)
+ {
+ int begin = fold_start[i];
+ int end = fold_start[i+1];
+ int j, k;
+
+ struct svm_problem subprob;
+
+ subprob.l = l - (end - begin);
+ subprob.x = Malloc (struct svm_node*, subprob.l);
+ subprob.y = Malloc (double, subprob.l);
+
+ k = 0;
+
+ for (j = 0;j < begin;j++)
+ {
+ subprob.x[k] = prob->x[perm[j]];
+ subprob.y[k] = prob->y[perm[j]];
+ ++k;
+ }
+
+ for (j = end;j < l;j++)
+ {
+ subprob.x[k] = prob->x[perm[j]];
+ subprob.y[k] = prob->y[perm[j]];
+ ++k;
+ }
+
+ struct svm_model *submodel = svm_train (&subprob, param);
+
+ if (param->probability &&
+ (param->svm_type == C_SVC || param->svm_type == NU_SVC))
+ {
+ double *prob_estimates = Malloc (double, svm_get_nr_class (submodel));
+
+ for (j = begin;j < end;j++)
+ target[perm[j]] = svm_predict_probability (submodel, prob->x[perm[j]], prob_estimates);
+
+ free (prob_estimates);
+ }
+ else
+ for (j = begin;j < end;j++)
+ target[perm[j]] = svm_predict (submodel, prob->x[perm[j]]);
+
+ svm_free_and_destroy_model (&submodel);
+
+ free (subprob.x);
+
+ free (subprob.y);
+ }
+
+ free (fold_start);
+
+ free (perm);
+}
+
+
+int svm_get_svm_type (const svm_model *model)
+{
+ return model->param.svm_type;
+}
+
+int svm_get_nr_class (const svm_model *model)
+{
+ return model->nr_class;
+}
+
+void svm_get_labels (const svm_model *model, int* label)
+{
+ if (model->label != NULL)
+ for (int i = 0;i < model->nr_class;i++)
+ label[i] = model->label[i];
+}
+
+double svm_get_svr_probability (const svm_model *model)
+{
+ if ( (model->param.svm_type == EPSILON_SVR || model->param.svm_type == NU_SVR) &&
+ model->probA != NULL)
+ return model->probA[0];
+ else
+ {
+ fprintf (stderr, "Model doesn't contain information for SVR probability inference\n");
+ return 0;
+ }
+}
+
+double svm_predict_values (const svm_model *model, const svm_node *x, double* dec_values)
+{
+ int i;
+
+ if (model->param.svm_type == ONE_CLASS ||
+ model->param.svm_type == EPSILON_SVR ||
+ model->param.svm_type == NU_SVR)
+ {
+ double *sv_coef = model->sv_coef[0];
+ double sum = 0;
+
+ for (i = 0;i < model->l;i++)
+ sum += sv_coef[i] * Kernel::k_function (x, model->SV[i], model->param);
+
+ sum -= model->rho[0];
+
+ *dec_values = sum;
+
+ if (model->param.svm_type == ONE_CLASS)
+ return (sum > 0) ? 1 : -1;
+ else
+ return sum;
+ }
+ else
+ {
+ int nr_class = model->nr_class;
+ int l = model->l;
+
+ double *kvalue = Malloc (double, l);
+
+ for (i = 0;i < l;i++)
+ kvalue[i] = Kernel::k_function (x, model->SV[i], model->param);
+
+ int *start = Malloc (int, nr_class);
+
+ start[0] = 0;
+
+ for (i = 1;i < nr_class;i++)
+ start[i] = start[i-1] + model->nSV[i-1];
+
+ int *vote = Malloc (int, nr_class);
+
+ for (i = 0;i < nr_class;i++)
+ vote[i] = 0;
+
+ int p = 0;
+
+ for (i = 0;i < nr_class;i++)
+ for (int j = i + 1;j < nr_class;j++)
+ {
+ double sum = 0;
+ int si = start[i];
+ int sj = start[j];
+ int ci = model->nSV[i];
+ int cj = model->nSV[j];
+
+ int k;
+ double *coef1 = model->sv_coef[j-1];
+ double *coef2 = model->sv_coef[i];
+
+ for (k = 0;k < ci;k++)
+ sum += coef1[si+k] * kvalue[si+k];
+
+ for (k = 0;k < cj;k++)
+ sum += coef2[sj+k] * kvalue[sj+k];
+
+ sum -= model->rho[p];
+
+ dec_values[p] = sum;
+
+ if (dec_values[p] > 0)
+ ++vote[i];
+ else
+ ++vote[j];
+
+ p++;
+ }
+
+ int vote_max_idx = 0;
+
+ for (i = 1;i < nr_class;i++)
+ if (vote[i] > vote[vote_max_idx])
+ vote_max_idx = i;
+
+ free (kvalue);
+
+ free (start);
+
+ free (vote);
+
+ return model->label[vote_max_idx];
+ }
+}
+
+double svm_predict (const svm_model *model, const svm_node *x)
+{
+ int nr_class = model->nr_class;
+ double *dec_values;
+
+ if (model->param.svm_type == ONE_CLASS ||
+ model->param.svm_type == EPSILON_SVR ||
+ model->param.svm_type == NU_SVR)
+ dec_values = Malloc (double, 1);
+ else
+ dec_values = Malloc (double, nr_class * (nr_class - 1) / 2);
+
+ double pred_result = svm_predict_values (model, x, dec_values);
+
+ free (dec_values);
+
+ return pred_result;
+}
+
+double svm_predict_probability (
+ const svm_model *model, const svm_node *x, double *prob_estimates)
+{
+ if ( (model->param.svm_type == C_SVC || model->param.svm_type == NU_SVC) &&
+ model->probA != NULL && model->probB != NULL)
+ {
+ int i;
+ int nr_class = model->nr_class;
+ double *dec_values = Malloc (double, nr_class * (nr_class - 1) / 2);
+ svm_predict_values (model, x, dec_values);
+
+ double min_prob = 1e-7;
+ double **pairwise_prob = Malloc (double *, nr_class);
+
+ for (i = 0;i < nr_class;i++)
+ pairwise_prob[i] = Malloc (double, nr_class);
+
+ int k = 0;
+
+ for (i = 0;i < nr_class;i++)
+ for (int j = i + 1;j < nr_class;j++)
+ {
+ pairwise_prob[i][j] = min (max (sigmoid_predict (dec_values[k], model->probA[k], model->probB[k]), min_prob), 1 - min_prob);
+ pairwise_prob[j][i] = 1 - pairwise_prob[i][j];
+ k++;
+ }
+
+ multiclass_probability (nr_class, pairwise_prob, prob_estimates);
+
+ int prob_max_idx = 0;
+
+ for (i = 1;i < nr_class;i++)
+ if (prob_estimates[i] > prob_estimates[prob_max_idx])
+ prob_max_idx = i;
+
+ for (i = 0;i < nr_class;i++)
+ free (pairwise_prob[i]);
+
+ free (dec_values);
+
+ free (pairwise_prob);
+
+ return model->label[prob_max_idx];
+ }
+ else
+ return svm_predict (model, x);
+}
+
+static const char *svm_type_table[] =
+{
+ "c_svc", "nu_svc", "one_class", "epsilon_svr", "nu_svr", NULL
+};
+
+static const char *kernel_type_table[] =
+{
+ "linear", "polynomial", "rbf", "sigmoid", "precomputed", NULL
+};
+
+int svm_save_model (const char *model_file_name, const svm_model *model)
+{
+ FILE *fp = fopen (model_file_name, "w");
+
+ if (fp == NULL)
+ return -1;
+
+ const svm_parameter& param = model->param;
+
+ fprintf (fp, "svm_type %s\n", svm_type_table[param.svm_type]);
+
+ fprintf (fp, "kernel_type %s\n", kernel_type_table[param.kernel_type]);
+
+ if (param.kernel_type == POLY)
+ fprintf (fp, "degree %d\n", param.degree);
+
+ if (param.kernel_type == POLY || param.kernel_type == RBF || param.kernel_type == SIGMOID)
+ fprintf (fp, "gamma %g\n", param.gamma);
+
+ if (param.kernel_type == POLY || param.kernel_type == SIGMOID)
+ fprintf (fp, "coef0 %g\n", param.coef0);
+
+ int nr_class = model->nr_class;
+
+ int l = model->l;
+
+ fprintf (fp, "nr_class %d\n", nr_class);
+
+ fprintf (fp, "total_sv %d\n", l);
+
+ {
+ fprintf (fp, "rho");
+
+ for (int i = 0;i < nr_class* (nr_class - 1) / 2;i++)
+ fprintf (fp, " %g", model->rho[i]);
+
+ fprintf (fp, "\n");
+ }
+
+ if (model->label)
+ {
+ fprintf (fp, "label");
+
+ for (int i = 0;i < nr_class;i++)
+ fprintf (fp, " %d", model->label[i]);
+
+ fprintf (fp, "\n");
+ }
+
+ if (model->probA) // regression has probA only
+ {
+ fprintf (fp, "probA");
+
+ for (int i = 0;i < nr_class* (nr_class - 1) / 2;i++)
+ fprintf (fp, " %g", model->probA[i]);
+
+ fprintf (fp, "\n");
+ }
+
+ if (model->probB)
+ {
+ fprintf (fp, "probB");
+
+ for (int i = 0;i < nr_class* (nr_class - 1) / 2;i++)
+ fprintf (fp, " %g", model->probB[i]);
+
+ fprintf (fp, "\n");
+ }
+
+ if (model->nSV)
+ {
+ fprintf (fp, "nr_sv");
+
+ for (int i = 0;i < nr_class;i++)
+ fprintf (fp, " %d", model->nSV[i]);
+
+ fprintf (fp, "\n");
+ }
+
+ fprintf (fp, "scaling ");
+
+ int ii = 0;
+
+ while (model->scaling[ii].index != -1)
+ {
+ if (model->scaling[ii].index)
+ fprintf (fp, "%d:%.8g ", ii, model->scaling[ii].value);
+
+ ii++;
+ }
+
+ fprintf (fp, "\n");
+
+
+ fprintf (fp, "SV\n");
+ const double * const *sv_coef = model->sv_coef;
+ const svm_node * const *SV = model->SV;
+
+ for (int i = 0;i < l;i++)
+ {
+ for (int j = 0;j < nr_class - 1;j++)
+ fprintf (fp, "%.16g ", sv_coef[j][i]);
+
+ const svm_node *p = SV[i];
+
+ if (param.kernel_type == PRECOMPUTED)
+ fprintf (fp, "0:%d ", int (p->value));
+ else
+ while (p->index != -1)
+ {
+ fprintf (fp, "%d:%.8g ", p->index, p->value);
+ p++;
+ }
+
+ fprintf (fp, "\n");
+ }
+
+ if (ferror (fp) != 0 || fclose (fp) != 0)
+ return -1;
+ else
+ return 0;
+}
+
+static char *line = NULL;
+static int max_line_len;
+
+static char* readline (FILE *input)
+{
+ int len;
+
+ if (fgets (line, max_line_len, input) == NULL)
+ return NULL;
+
+ while (strrchr (line, '\n') == NULL)
+ {
+ max_line_len *= 2;
+ line = static_cast<char *> (realloc (line, max_line_len));
+ len = int (strlen (line));
+
+ if (fgets (line + len, max_line_len - len, input) == NULL)
+ break;
+ }
+
+ return line;
+}
+
+svm_model *svm_load_model (const char *model_file_name)
+{
+ FILE *fp = fopen (model_file_name, "rb");
+
+ if (fp == NULL)
+ return NULL;
+
+ // read parameters
+
+ svm_model *model = Malloc (svm_model, 1);
+
+ svm_parameter& param = model->param;
+
+ model->rho = NULL;
+
+ model->probA = NULL;
+
+ model->probB = NULL;
+
+ model->label = NULL;
+
+ model->nSV = NULL;
+
+ model->scaling = Malloc (struct svm_node, 1);
+
+ model->scaling[0].index = -1;
+
+ char cmd[81];
+
+ while (1)
+ {
+ int res = fscanf (fp, "%80s", cmd);
+
+ if (res > 0 && strcmp (cmd, "svm_type") == 0)
+ {
+ res = fscanf (fp, "%80s", cmd);
+ int i;
+
+ for (i = 0;svm_type_table[i];i++)
+ {
+ if (res > 0 && strcmp (svm_type_table[i], cmd) == 0)
+ {
+ param.svm_type = i;
+ break;
+ }
+ }
+
+ if (svm_type_table[i] == NULL)
+ {
+ fprintf (stderr, "unknown svm type.\n");
+ free (model->rho);
+ free (model->label);
+ free (model->nSV);
+ free (model);
+ return NULL;
+ }
+ }
+ else
+ if (res > 0 && strcmp (cmd, "kernel_type") == 0)
+ {
+ res = fscanf (fp, "%80s", cmd);
+ int i;
+
+ for (i = 0;kernel_type_table[i];i++)
+ {
+ if (res > 0 && strcmp (kernel_type_table[i], cmd) == 0)
+ {
+ param.kernel_type = i;
+ break;
+ }
+ }
+
+ if (kernel_type_table[i] == NULL)
+ {
+ fprintf (stderr, "unknown kernel function.\n");
+ free (model->rho);
+ free (model->label);
+ free (model->nSV);
+ free (model);
+ return NULL;
+ }
+ }
+ else
+ if (res > 0 && strcmp (cmd, "degree") == 0)
+ res = fscanf (fp, "%d", ¶m.degree);
+ else
+ if (res > 0 && strcmp (cmd, "gamma") == 0)
+ res = fscanf (fp, "%lf", ¶m.gamma);
+ else
+ if (res > 0 && strcmp (cmd, "coef0") == 0)
+ res = fscanf (fp, "%lf", ¶m.coef0);
+ else
+ if (res > 0 && strcmp (cmd, "nr_class") == 0)
+ res = fscanf (fp, "%d", &model->nr_class);
+ else
+ if (res > 0 && strcmp (cmd, "total_sv") == 0)
+ res = fscanf (fp, "%d", &model->l);
+ else
+ if (res > 0 && strcmp (cmd, "rho") == 0)
+ {
+ int n = model->nr_class * (model->nr_class - 1) / 2;
+ model->rho = Malloc (double, n);
+
+ for (int i = 0;i < n;i++)
+ res = fscanf (fp, "%lf", &model->rho[i]);
+ }
+ else
+ if (res > 0 && strcmp (cmd, "label") == 0)
+ {
+ int n = model->nr_class;
+ model->label = Malloc (int, n);
+
+ for (int i = 0;i < n;i++)
+ res = fscanf (fp, "%d", &model->label[i]);
+ }
+ else
+ if (res > 0 && strcmp (cmd, "probA") == 0)
+ {
+ int n = model->nr_class * (model->nr_class - 1) / 2;
+ model->probA = Malloc (double, n);
+
+ for (int i = 0;i < n;i++)
+ res = fscanf (fp, "%lf", &model->probA[i]);
+ }
+ else
+ if (res > 0 && strcmp (cmd, "probB") == 0)
+ {
+ int n = model->nr_class * (model->nr_class - 1) / 2;
+ model->probB = Malloc (double, n);
+
+ for (int i = 0;i < n;i++)
+ res = fscanf (fp, "%lf", &model->probB[i]);
+ }
+ else
+ if (res > 0 && strcmp (cmd, "nr_sv") == 0)
+ {
+ int n = model->nr_class;
+ model->nSV = Malloc (int, n);
+
+ for (int i = 0;i < n;i++)
+ res = fscanf (fp, "%d", &model->nSV[i]);
+ }
+ else
+ if (res > 0 && strcmp (cmd, "scaling") == 0)
+ {
+ char *idx, *val, buff[10000];
+ int ii = 0, pre_ii = 0;
+ //char delims[]="\t: ";
+ model->scaling = Malloc (struct svm_node, 1);
+ res = fscanf (fp, "%10000[^\n]", buff);
+ idx = strtok (buff, ":");
+
+ while (idx != NULL)
+ {
+ val = strtok (NULL, " \t");
+ pre_ii = ii;
+ ii = atoi (idx);
+
+ model->scaling = Realloc (model->scaling, struct svm_node, ii + 2);
+
+ //setting to zero the non defined scaling factors
+
+ for (int j = pre_ii + 1; j < ii; j++)
+ {
+ model->scaling[j].index = 0;
+ model->scaling[j].value = 0;
+ }
+
+ model->scaling[ii].index = 1;
+
+ model->scaling[ii].value = atof (val);
+ ++ii;
+ idx = strtok (NULL, ":");
+ //printf("%d e %f\n",model->scaling[ii-1].index,model->scaling[ii-1].value);
+ }
+
+ model->scaling[ii].index = -1;
+ }
+ else
+ if (res > 0 && strcmp (cmd, "SV") == 0)
+ {
+ //std::cout << cmd << std::endl;
+ while (1)
+ {
+ int c = getc (fp);
+
+ if (c == EOF || c == '\n')
+ break;
+ }
+
+ break;
+ }
+ else
+ {
+ fprintf (stderr, "unknown text in model file: [%s]\n", cmd);
+ free (model->rho);
+ free (model->label);
+ free (model->nSV);
+ free (model);
+ return NULL;
+ }
+ }
+
+ // read sv_coef and SV
+
+ int elements = 0;
+
+ long pos = ftell (fp);
+
+ max_line_len = 10000;
+
+ line = Malloc (char, max_line_len);
+
+ char *p, *endptr, *idx, *val;
+
+ while (readline (fp) != NULL)
+ {
+ p = strtok (line, ":");
+
+ while (1)
+ {
+ p = strtok (NULL, ":");
+
+ if (p == NULL)
+ break;
+
+ ++elements;
+ }
+ }
+
+ elements += model->l;
+
+ fseek (fp, pos, SEEK_SET);
+
+ int m = model->nr_class - 1;
+ int l = model->l;
+ model->sv_coef = Malloc (double *, m);
+ int i;
+
+ for (i = 0;i < m;i++)
+ model->sv_coef[i] = Malloc (double, l);
+
+ model->SV = Malloc (svm_node*, l);
+
+ svm_node *x_space = NULL;
+
+ if (l > 0)
+ x_space = Malloc (svm_node, elements);
+
+ long int j = 0;
+
+ for (i = 0;i < l;i++)
+ {
+ readline (fp);
+ model->SV[i] = &x_space[j];
+
+ p = strtok (line, " \t");
+ model->sv_coef[0][i] = strtod (p, &endptr);
+
+ for (int k = 1;k < m;k++)
+ {
+ p = strtok (NULL, " \t");
+ model->sv_coef[k][i] = strtod (p, &endptr);
+ }
+
+ int jj = 0;
+
+ while (1)
+ {
+ idx = strtok (NULL, ":");
+ val = strtok (NULL, " \t");
+
+ if (val == NULL)
+ break;
+
+ x_space[j].index = int (strtol (idx, &endptr, 10));
+
+ x_space[j].value = strtod (val, &endptr);
+
+// printf("i=%d, j=%d, %f ,%d e %f\n",i,j,model->sv_coef[0][i],
+// model->SV[i][jj].index, model->SV[i][jj].value);
+ jj++;
+
+ ++j;
+ }
+
+ x_space[j++].index = -1;
+ }
+
+ free (line);
+
+ //printf("%d e %f\n",model->scaling[j-2].index,model->scaling[j-2].value);
+
+ if (ferror (fp) != 0 || fclose (fp) != 0)
+ return NULL;
+
+ model->free_sv = 1; // XXX
+
+ return model;
+}
+
+void svm_free_model_content (svm_model* model_ptr)
+{
+ if (model_ptr->free_sv && model_ptr->l > 0 && model_ptr->SV != NULL)
+ free (static_cast<void *> (model_ptr->SV[0]));
+
+ if (model_ptr->sv_coef)
+ {
+ for (int i = 0;i < model_ptr->nr_class - 1;i++)
+ free (model_ptr->sv_coef[i]);
+ }
+
+ free (model_ptr->SV);
+
+ model_ptr->SV = NULL;
+
+ free (model_ptr->sv_coef);
+ model_ptr->sv_coef = NULL;
+
+ free (model_ptr->rho);
+ model_ptr->rho = NULL;
+
+ free (model_ptr->label);
+ model_ptr->label = NULL;
+
+ free (model_ptr->probA);
+ model_ptr->probA = NULL;
+
+ free (model_ptr->probB);
+ model_ptr->probB = NULL;
+
+ free (model_ptr->nSV);
+ model_ptr->nSV = NULL;
+}
+
+void svm_free_and_destroy_model (svm_model** model_ptr_ptr)
+{
+ if (model_ptr_ptr != NULL && *model_ptr_ptr != NULL)
+ {
+ svm_free_model_content (*model_ptr_ptr);
+ free (*model_ptr_ptr);
+ *model_ptr_ptr = NULL;
+ }
+}
+
+void svm_destroy_param (svm_parameter* param)
+{
+ free (param->weight_label);
+ free (param->weight);
+}
+
+const char *svm_check_parameter (const svm_problem *prob, const svm_parameter *param)
+{
+ // svm_type
+
+ int svm_type = param->svm_type;
+
+ if (svm_type != C_SVC &&
+ svm_type != NU_SVC &&
+ svm_type != ONE_CLASS &&
+ svm_type != EPSILON_SVR &&
+ svm_type != NU_SVR)
+ return "unknown svm type";
+
+ // kernel_type, degree
+
+ int kernel_type = param->kernel_type;
+
+ if (kernel_type != LINEAR &&
+ kernel_type != POLY &&
+ kernel_type != RBF &&
+ kernel_type != SIGMOID &&
+ kernel_type != PRECOMPUTED)
+ return "unknown kernel type";
+
+ if (param->gamma < 0)
+ return "gamma < 0";
+
+ if (param->degree < 0)
+ return "degree of polynomial kernel < 0";
+
+ // cache_size,eps,C,nu,p,shrinking
+
+ if (param->cache_size <= 0)
+ return "cache_size <= 0";
+
+ if (param->eps <= 0)
+ return "eps <= 0";
+
+ if (svm_type == C_SVC ||
+ svm_type == EPSILON_SVR ||
+ svm_type == NU_SVR)
+ if (param->C <= 0)
+ return "C <= 0";
+
+ if (svm_type == NU_SVC ||
+ svm_type == ONE_CLASS ||
+ svm_type == NU_SVR)
+ if (param->nu <= 0 || param->nu > 1)
+ return "nu <= 0 or nu > 1";
+
+ if (svm_type == EPSILON_SVR)
+ if (param->p < 0)
+ return "p < 0";
+
+ if (param->shrinking != 0 &&
+ param->shrinking != 1)
+ return "shrinking != 0 and shrinking != 1";
+
+ if (param->probability != 0 &&
+ param->probability != 1)
+ return "probability != 0 and probability != 1";
+
+ if (param->probability == 1 &&
+ svm_type == ONE_CLASS)
+ return "one-class SVM probability output not supported yet";
+
+
+ // check whether nu-svc is feasible
+
+ if (svm_type == NU_SVC)
+ {
+ int l = prob->l;
+ int max_nr_class = 16;
+ int nr_class = 0;
+ int *label = Malloc (int, max_nr_class);
+ int *count = Malloc (int, max_nr_class);
+
+ int i;
+
+ for (i = 0;i < l;i++)
+ {
+ int this_label = int (prob->y[i]);
+ int j;
+
+ for (j = 0;j < nr_class;j++)
+ if (this_label == label[j])
+ {
+ ++count[j];
+ break;
+ }
+
+ if (j == nr_class)
+ {
+ if (nr_class == max_nr_class)
+ {
+ max_nr_class *= 2;
+ label = static_cast<int *> (realloc (label, max_nr_class * sizeof (int)));
+ count = static_cast<int *> (realloc (count, max_nr_class * sizeof (int)));
+ }
+
+ label[nr_class] = this_label;
+
+ count[nr_class] = 1;
+ ++nr_class;
+ }
+ }
+
+ for (i = 0;i < nr_class;i++)
+ {
+ int n1 = count[i];
+
+ for (int j = i + 1;j < nr_class;j++)
+ {
+ int n2 = count[j];
+
+ if (param->nu* (n1 + n2) / 2 > min (n1, n2))
+ {
+ free (label);
+ free (count);
+ return "specified nu is infeasible";
+ }
+ }
+ }
+
+ free (label);
+
+ free (count);
+ }
+
+ return NULL;
+}
+
+int svm_check_probability_model (const svm_model *model)
+{
+ return ( (model->param.svm_type == C_SVC || model->param.svm_type == NU_SVC) &&
+ model->probA != NULL && model->probB != NULL) ||
+ ( (model->param.svm_type == EPSILON_SVR || model->param.svm_type == NU_SVR) &&
+ model->probA != NULL);
+}
+
+void svm_set_print_string_function (void (*print_func) (const char *))
+{
+ if (print_func == NULL)
+ svm_print_string = &print_string_stdout;
+ else
+ svm_print_string = print_func;
+}
+
+#endif // _LIBSVM_HPP_
--- /dev/null
+ /*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ * Copyright (c) 2000-2012 Chih-Chung Chang and Chih-Jen Lin
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of copyright holders nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+
+#ifndef PCL_SVM_WRAPPER_HPP_
+#define PCL_SVM_WRAPPER_HPP_
+
+#include <pcl/ml/svm_wrapper.h>
+#include <assert.h>
+#include <fstream>
+
+
+template <typename T>
+inline T module (T a)
+{
+ if (a > 0)
+ return a;
+ else
+ return -a;
+}
+
+char*
+pcl::SVM::readline (FILE *input)
+{
+ int len;
+
+ if (fgets (line_, max_line_len_, input) == NULL)
+ return NULL;
+
+ // Find the endline. If not found extend the max_line_len_
+ while (strrchr (line_ , '\n') == NULL)
+ {
+ max_line_len_ *= 2;
+ line_ = static_cast<char *> (realloc (line_, max_line_len_));
+ len = int (strlen (line_));
+
+ // if the new read part of the string is unavailable, break the while
+ if (fgets (line_ + len, max_line_len_ - len, input) == NULL)
+ break;
+ }
+
+ return line_;
+}
+
+void
+pcl::SVMTrain::doCrossValidation ()
+{
+ int i;
+ int total_correct = 0;
+ double total_error = 0;
+ double sumv = 0, sumy = 0, sumvv = 0, sumyy = 0, sumvy = 0;
+ double *target = Malloc (double, prob_.l);
+
+ // number of fold for the cross validation (n of folds = number of splitting of the input dataset)
+ if (nr_fold_ < 2)
+ {
+ fprintf (stderr, "n-fold cross validation: n must >= 2\n");
+ return;
+ }
+
+ svm_cross_validation (&prob_, ¶m_, nr_fold_, target); // perform cross validation
+
+ if (param_.svm_type == EPSILON_SVR ||
+ param_.svm_type == NU_SVR)
+ {
+ for (i = 0;i < prob_.l;i++)
+ {
+ double y = prob_.y[i];
+ double v = target[i];
+ total_error += (v - y) * (v - y);
+ sumv += v;
+ sumy += y;
+ sumvv += v * v;
+ sumyy += y * y;
+ sumvy += v * y;
+ }
+
+ pcl::console::print_info (" - Cross Validation Mean squared error = ");
+ pcl::console::print_value ("%g\n", total_error / prob_.l);
+
+ pcl::console::print_info (" - Cross Validation Squared correlation coefficient = ");
+ pcl::console::print_value ("%g\n",
+ ( (prob_.l*sumvy - sumv*sumy) * (prob_.l*sumvy - sumv*sumy)) /
+ ( (prob_.l*sumvv - sumv*sumv) * (prob_.l*sumyy - sumy*sumy))
+ );
+ }
+ else
+ {
+ for (i = 0;i < prob_.l;i++)
+ if (target[i] == prob_.y[i])
+ ++total_correct;
+
+ pcl::console::print_info (" - Cross Validation Accuracy = ");
+ pcl::console::print_value ("%g%%\n", 100.0*total_correct / prob_.l);
+ }
+
+ free (target);
+}
+
+void
+pcl::SVMTrain::scaleFactors (std::vector<SVMData> training_set, svm_scaling &scaling)
+{
+ int max = 0;
+
+ for (size_t i = 0; i < training_set.size() ; i++)
+ for (size_t j = 0; j < training_set[i].SV.size() ; j++)
+ if (training_set[i].SV[j].idx > max)
+ max = training_set[i].SV[j].idx; // max number of features
+
+ max += 1;
+
+ scaling.obj = Malloc (struct svm_node, max + 1);
+ scaling.max = max;
+ scaling.obj[max].index = -1; // last index is -1
+
+ for (int i = 0; i < max; i++) // Initialize values
+ {
+ scaling.obj[i].index = 0;
+ scaling.obj[i].value = 0;
+ }
+
+ for (size_t i = 0; i < training_set.size(); i++)
+ for (size_t j = 0; j < training_set[i].SV.size(); j++)
+ // save scaling factor finding the maximum value
+ if (module (training_set[i].SV[j].value) > scaling.obj[ training_set[i].SV[j].idx ].value)
+ {
+ scaling.obj[ training_set[i].SV[j].idx ].index = 1;
+ scaling.obj[ training_set[i].SV[j].idx ].value = module (training_set[i].SV[j].value);
+ }
+};
+
+void
+pcl::SVM::adaptLibSVMToInput (std::vector<SVMData> &training_set, svm_problem prob)
+{
+ training_set.clear (); // Reset input
+
+ for (int i = 0; i < prob.l; i++)
+ {
+ SVMData parent; // buffer data container
+ int j = 0;
+
+ if (labelled_training_set_)
+ parent.label = prob.y[i];
+
+ while (prob.x[i][j].index != -1) // -1 means the end of problem entry list
+ {
+ SVMDataPoint seed; // single feature seed
+
+ if (pcl_isfinite (prob.x[i][j].value))
+ {
+ seed.idx = prob.x[i][j].index;
+ seed.value = float (prob.x[i][j].value);
+ parent.SV.push_back (seed);
+ }
+
+ j++;
+ }
+
+ training_set.push_back (parent);
+ }
+};
+
+void
+pcl::SVM::adaptInputToLibSVM (std::vector<SVMData> training_set, svm_problem &prob)
+{
+ assert (training_set.size() > 0);
+ assert (scaling_.max != 0);
+
+ if (scaling_.max == 0)
+ {
+ // to be sure to have loaded the scaling
+ PCL_ERROR ("[pcl::%s::adaptInputToLibSVM] Classifier model not loaded!\n", getClassName ().c_str ());
+ return;
+ }
+
+ prob.l = int (training_set.size ()); // n of elements/points
+ prob.y = Malloc (double, prob.l);
+ prob.x = Malloc (struct svm_node *, prob.l);
+
+ for (int i = 0; i < prob.l; i++)
+ {
+ if (pcl_isfinite (training_set[i].label) && labelled_training_set_)
+ {
+ prob.y[i] = training_set[i].label;
+ labelled_training_set_ = 1;
+ }
+ else
+ labelled_training_set_ = 0;
+
+ prob.x[i] = Malloc (struct svm_node, training_set[i].SV.size() + 1);
+
+ int k = 0;
+
+ for (size_t j = 0; j < training_set[i].SV.size(); j++)
+ if (training_set[i].SV[j].idx != -1 && pcl_isfinite (training_set[i].SV[j].value))
+ {
+ prob.x[i][k].index = training_set[i].SV[j].idx;
+ if (training_set[i].SV[j].idx < scaling_.max && scaling_.obj[ training_set[i].SV[j].idx ].index == 1)
+ prob.x[i][k].value = training_set[i].SV[j].value / scaling_.obj[ training_set[i].SV[j].idx ].value;
+ else
+ prob.x[i][k].value = training_set[i].SV[j].value;
+ k++;
+ }
+
+ prob.x[i][k].index = -1;
+ }
+};
+
+bool
+pcl::SVMTrain::trainClassifier ()
+{
+ if (training_set_.size() == 0)
+ {
+ // to be sure to have loaded the training set
+ PCL_ERROR ("[pcl::%s::trainClassifier] Training data not set!\n", getClassName ().c_str ());
+ return 0;
+ }
+
+ scaleFactors (training_set_, scaling_);
+ adaptInputToLibSVM (training_set_, prob_);
+
+ const char *error_msg;
+ error_msg = svm_check_parameter (&prob_, ¶m_);
+
+ // initialize gamma parameter
+
+ if (param_.gamma == 0 && scaling_.max > 0)
+ param_.gamma = 1.0 / scaling_.max;
+
+ if (error_msg)
+ {
+ PCL_ERROR ("[pcl::%s::trainClassifier] %s\n", getClassName ().c_str (), error_msg);
+ //fprintf (stderr, "ERROR: %s\n", error_msg);
+ exit (1);
+ }
+
+ if (cross_validation_)
+ {
+ doCrossValidation ();
+ }
+ else
+ {
+ SVMModel* out;
+ out = static_cast<SVMModel*> (svm_train (&prob_, ¶m_));
+ if (out == NULL)
+ {
+ PCL_ERROR ("[pcl::%s::trainClassifier] Error taining the classifier model.\n", getClassName ().c_str ());
+ return 0;
+ }
+ model_ = *out;
+ model_.scaling = scaling_.obj;
+ free (out);
+ }
+
+ return 1;
+}
+
+bool
+pcl::SVM::loadProblem (const char *filename, svm_problem &prob)
+{
+ int elements, max_index, inst_max_index, i, j;
+ FILE *fp = fopen (filename, "r");
+ svm_node *x_space_;
+ char *endptr;
+ char *idx, *val, *label;
+
+ if (fp == NULL)
+ {
+ PCL_ERROR ("[pcl::%s] Can't open input file %s.\n", getClassName ().c_str (), filename);
+ return 0;
+ }
+
+ elements = 0;
+ prob.l = 0;
+
+ line_ = Malloc (char, max_line_len_);
+
+ // readline function writes one line in var. "line_"
+ while (readline (fp) != NULL)
+ {
+ // "\t" cuts the tab or space.
+ // strtok splits the string into tokens
+ char *p = strtok (line_, " \t"); // label
+ ++elements;
+ // features
+
+ while (1)
+ {
+ // split the next element
+ p = strtok (NULL, " \t");
+
+ if (p == NULL || *p == '\n') // check '\n' as ' ' may be after the last feature
+ break;
+
+ ++elements;
+ }
+ ++elements; // contains the number of elements in the string
+ ++prob.l; // number op
+ }
+
+ rewind (fp); // returns to the top pos of fp
+
+ prob.y = Malloc (double, prob.l);
+ prob.x = Malloc (struct svm_node *, prob.l);
+ x_space_ = Malloc (struct svm_node, elements);
+
+ max_index = 0;
+ j = 0;
+ bool isUnlabelled = 0;
+
+ for (i = 0;i < prob.l;i++)
+ {
+ inst_max_index = -1; // strtol gives 0 if wrong format, and precomputed kernel has <index> start from 0
+ // read one line in the file
+ readline (fp);
+ prob.x[i] = &x_space_[j];
+
+ if (!isUnlabelled)
+ {
+ label = strtok (line_, " \t\n"); //save first element as label
+ char * pch;
+ pch = strpbrk (label, ":");
+ // std::cout << label << std::endl;
+
+ // check if the first element is really a label
+
+ if (pch == NULL)
+ {
+ if (label == NULL) // empty line
+ exitInputError (i + 1);
+
+ labelled_training_set_ = 1;
+
+ prob.y[i] = strtod (label, &endptr);
+
+ if (endptr == label || *endptr != '\0')
+ exitInputError (i + 1);
+
+ //idx = strtok(NULL,":"); // indice
+ }
+ else
+ {
+ isUnlabelled = 1;
+ labelled_training_set_ = 0;
+ i = -1;
+ rewind (fp);
+ continue;
+ }
+ } //else
+
+// idx=strtok(line,": \t\n");
+ int k = 0;
+
+ while (1)
+ {
+ if (k++ == 0 && isUnlabelled)
+ idx = strtok (line_, ": \t\n");
+ else
+ idx = strtok (NULL, ":"); // indice
+
+ val = strtok (NULL, " \t"); // valore
+
+ if (val == NULL)
+ break; // exit with the last element
+
+ //std::cout << idx << ":" << val<< " ";
+ errno = 0;
+
+ x_space_[j].index = int (strtol (idx, &endptr, 10));
+
+ if (endptr == idx || errno != 0 || *endptr != '\0' || x_space_[j].index <= inst_max_index)
+ exitInputError (i + 1);
+ else
+ inst_max_index = x_space_[j].index;
+
+ errno = 0;
+
+ x_space_[j].value = strtod (val, &endptr);
+
+ if (endptr == val || errno != 0 || (*endptr != '\0' && !isspace (*endptr)))
+ exitInputError (i + 1);
+
+ ++j;
+
+ }
+
+ //std::cout <<"\n";
+ if (inst_max_index > max_index)
+ max_index = inst_max_index;
+
+ x_space_[j++].index = -1;
+ }
+
+
+
+ if (param_.gamma == 0 && max_index > 0)
+ param_.gamma = 1.0 / max_index;
+
+ if (param_.kernel_type == PRECOMPUTED)
+ for (i = 0;i < prob.l;i++)
+ {
+ if (prob.x[i][0].index != 0)
+ {
+ PCL_ERROR ("[pcl::%s] Wrong input format: first column must be 0:sample_serial_number.\n", getClassName ().c_str () );
+ return 0;
+ }
+
+ if (int (prob.x[i][0].value) <= 0 || int (prob.x[i][0].value) > max_index)
+ {
+ PCL_ERROR ("[pcl::%s] Wrong input format: sample_serial_number out of range.\n", getClassName ().c_str () );
+ return 0;
+ }
+ }
+ fclose (fp);
+
+ return 1;
+}
+
+
+bool
+pcl::SVM::saveProblem (const char *filename, bool labelled = 0)
+{
+ assert (training_set_.size() > 0);
+ std::ofstream myfile;
+ myfile.open (filename);
+
+ if (!myfile.is_open())
+ {
+ PCL_ERROR ("[pcl::%s] Can't open/create file %s.\n", getClassName ().c_str (), filename);
+ return 0;
+ }
+
+
+ for (size_t j = 0; j < training_set_.size() ; j++)
+ {
+
+ if (labelled)
+ {
+ assert (pcl_isfinite (training_set_[j].label));
+ myfile << training_set_[j].label << " ";
+ }
+
+ for (size_t i = 0; i < training_set_[j].SV.size(); i++)
+ if (pcl_isfinite (training_set_[j].SV[i].value))
+ myfile << training_set_[j].SV[i].idx << ":" << training_set_[j].SV[i].value << " ";
+
+ myfile << "\n";
+ }
+
+ myfile.close();
+
+ //std::cout << " * " << filename << " saved" << std::endl;
+ return 1;
+}
+
+bool
+pcl::SVM::saveProblemNorm (const char *filename, svm_problem prob_, bool labelled = 0)
+{
+ if (prob_.l == 0)
+ {
+ PCL_ERROR ("[pcl::%s] Can't save file %s. Input data not set.\n", getClassName ().c_str (), filename);
+ return 0;
+ }
+
+ std::ofstream myfile;
+
+ myfile.open (filename);
+
+ if (!myfile.is_open())
+ {
+ PCL_ERROR ("[pcl::%s] Can't open/create file %s.\n", getClassName ().c_str (), filename);
+ return 0;
+ }
+
+ for (int j = 0; j < prob_.l ; j++)
+ {
+ if (labelled)
+ myfile << prob_.y[j] << " ";
+
+ // for (int i=0; i < nFeatures+2; i++)
+ int i = 0;
+
+ while (prob_.x[j][i].index != -1)
+ {
+ myfile << prob_.x[j][i].index << ":" << prob_.x[j][i].value << " ";
+ i++;
+ }
+
+ myfile << "\n";
+ }
+
+ myfile.close();
+
+ //std::cout << " * " << filename << " saved" << std::endl;
+ return 1;
+}
+
+bool
+pcl::SVMClassify::loadClassifierModel (const char *filename)
+{
+ SVMModel *out;
+ out = static_cast<SVMModel*> (svm_load_model (filename));
+ if (out == NULL)
+ {
+ PCL_ERROR ("[pcl::%s::loadClassifierModel] Can't open classifier model %s.\n", getClassName ().c_str (), filename);
+ return 0;
+ }
+
+ model_ = *out;
+ free (out);
+
+ if (model_.l == 0)
+ {
+ PCL_ERROR ("[pcl::%s::loadClassifierModel] Can't open classifier model %s.\n", getClassName ().c_str (), filename);
+ return 0;
+ }
+
+ scaling_.obj = model_.scaling;
+
+ int i = 0;
+
+ while (model_.scaling[i].index != -1)
+ i++;
+
+ scaling_.max = i;
+
+ return 1;
+}
+
+bool
+pcl::SVMClassify::classificationTest ()
+{
+ if (model_.l == 0)
+ {
+ PCL_ERROR ("[pcl::%s::classificationTest] Classifier model has no data.\n", getClassName ().c_str ());
+ return 0;
+ }
+
+ if (prob_.l == 0)
+ {
+ PCL_ERROR ("[pcl::%s::classificationTest] Input dataset has no data.\n", getClassName ().c_str ());
+ return 0;
+ }
+
+ if (!labelled_training_set_)
+ {
+ PCL_ERROR ("[pcl::%s::classificationTest] Input dataset is not labelled.\n", getClassName ().c_str ());
+ return 0;
+ }
+
+ if (predict_probability_)
+ {
+ if (svm_check_probability_model (&model_) == 0)
+ {
+ PCL_WARN ("[pcl::%s::classificationTest] Classifier model does not support probabiliy estimates. Automatically disabled.\n", getClassName ().c_str ());
+ predict_probability_ = 0;
+ }
+ }
+ else
+ {
+ if (svm_check_probability_model (&model_) != 0)
+ PCL_WARN("[pcl::%s::classificationTest] Classifier model supports probability estimates, but disabled in prediction.\n", getClassName ().c_str ());
+ }
+
+ int correct = 0;
+
+ int total = 0;
+ double error = 0;
+ double sump = 0, sumt = 0, sumpp = 0, sumtt = 0, sumpt = 0;
+
+ int svm_type = svm_get_svm_type (&model_);
+ int nr_class = svm_get_nr_class (&model_);
+ double *prob_estimates = NULL;
+ int j;
+
+ prediction_.clear ();
+
+ if (predict_probability_)
+ {
+ if (svm_type == NU_SVR || svm_type == EPSILON_SVR)
+ PCL_WARN ("[pcl::%s::classificationTest] Prob. model for test data: target value = predicted value + z,\nz: Laplace distribution e^(-|z|/sigma)/(2sigma),sigma=%g\n", getClassName ().c_str (),svm_get_svr_probability (&model_));
+ else
+ {
+ prob_estimates = static_cast<double *> (malloc (nr_class * sizeof (double)));
+ }
+ }
+
+ int ii = 0;
+
+ prediction_.resize (prob_.l);
+
+ while (ii < prob_.l)
+ {
+ //int i = 0;
+ double target_label, predict_label;
+ //char *idx, *val, *endptr;
+ //int inst_max_index = -1; // strtol gives 0 if wrong format, and precomputed kernel has <index> start from 0
+
+ target_label = prob_.y[ii]; //takes the first label
+
+ if (predict_probability_ && (svm_type == C_SVC || svm_type == NU_SVC))
+ {
+ predict_label = svm_predict_probability (&model_, prob_.x[ii], prob_estimates);
+ prediction_[ii].push_back (predict_label);
+
+ for (j = 0;j < nr_class;j++)
+ {
+ prediction_[ii].push_back (prob_estimates[j]);
+ }
+ }
+ else
+ {
+ predict_label = svm_predict (&model_, prob_.x[ii]);
+ prediction_[ii].push_back (predict_label);
+ }
+
+
+ if (predict_label == target_label)
+ ++correct;
+
+ error += (predict_label - target_label) * (predict_label - target_label);
+ sump += predict_label;
+ sumt += target_label;
+ sumpp += predict_label * predict_label;
+ sumtt += target_label * target_label;
+ sumpt += predict_label * target_label;
+
+ ++total;
+ ii++;
+ }
+
+ if (svm_type == NU_SVR || svm_type == EPSILON_SVR)
+ {
+ pcl::console::print_info (" - Mean squared error (regression) = ");
+ pcl::console::print_value ("%g\n", error / total);
+
+ pcl::console::print_info (" - Squared correlation coefficient (regression) = ");
+ pcl::console::print_value ("%g\n",
+ ( (total*sumpt - sump*sumt) * (total*sumpt - sump*sumt)) /
+ ( (total*sumpp - sump*sump) * (total*sumtt - sumt*sumt))
+ );
+ }
+ else
+ {
+ pcl::console::print_info (" - Accuracy (classification) = ");
+ pcl::console::print_value ("%g%% (%d/%d)\n",
+ double (correct) / total*100, correct, total);
+ }
+
+ if (predict_probability_)
+ free (prob_estimates);
+
+ return true;
+}
+
+bool
+pcl::SVMClassify::classification ()
+{
+ if (model_.l == 0)
+ {
+ PCL_ERROR ("[pcl::%s::classification] Classifier model has no data.\n", getClassName ().c_str ());
+ return 0;
+ }
+
+ if (prob_.l == 0)
+ {
+ PCL_ERROR ("[pcl::%s::classification] Input dataset has no data.\n", getClassName ().c_str ());
+ return 0;
+ }
+
+
+ if (predict_probability_)
+ {
+ if (svm_check_probability_model (&model_) == 0)
+ {
+ PCL_WARN ("[pcl::%s::classification] Classifier model does not support probabiliy estimates. Automatically disabled.\n", getClassName ().c_str ());
+ predict_probability_ = 0;
+ }
+ }
+ else
+ {
+ if (svm_check_probability_model (&model_) != 0)
+ PCL_WARN("[pcl::%s::classification] Classifier model supports probability estimates, but disabled in prediction.\n", getClassName ().c_str ());
+ }
+
+ //int correct = 0;
+ int total = 0;
+ int svm_type = svm_get_svm_type (&model_);
+ int nr_class = svm_get_nr_class (&model_);
+
+ double *prob_estimates = NULL;
+
+ int j;
+
+ prediction_.clear();
+
+ if (predict_probability_)
+ {
+ if (svm_type == NU_SVR || svm_type == EPSILON_SVR)
+ PCL_WARN ("[pcl::%s::classificationTest] Prob. model for test data: target value = predicted value + z,\nz: Laplace distribution e^(-|z|/sigma)/(2sigma),sigma=%g\n", getClassName ().c_str (),svm_get_svr_probability (&model_));
+ else
+ {
+ prob_estimates = static_cast<double *> (malloc (nr_class * sizeof (double)));
+ }
+ }
+
+ int ii = 0;
+
+ prediction_.resize (prob_.l);
+
+ while (ii < prob_.l)
+ {
+ double predict_label;
+ //char *idx, *val, *endptr;
+ //int inst_max_index = -1; // strtol gives 0 if wrong format, and precomputed kernel has <index> start from 0
+
+ if (predict_probability_ && (svm_type == C_SVC || svm_type == NU_SVC))
+ {
+ predict_label = svm_predict_probability (&model_, prob_.x[ii], prob_estimates);
+ prediction_[ii].push_back (predict_label);
+
+ for (j = 0;j < nr_class;j++)
+ {
+ prediction_[ii].push_back (prob_estimates[j]);
+ }
+ }
+ else
+ {
+ predict_label = svm_predict (&model_, prob_.x[ii]);
+ prediction_[ii].push_back (predict_label);
+ }
+
+ ++total;
+
+ ii++;
+ }
+
+ if (predict_probability_)
+ free (prob_estimates);
+
+ return (true);
+}
+
+std::vector<double>
+pcl::SVMClassify::classification (pcl::SVMData in)
+{
+ assert (model_.l != 0);
+
+ if (model_.l == 0)
+ {
+ PCL_ERROR ("[pcl::%s::classification] Classifier model has no data.\n", getClassName ().c_str ());
+ exit(0);
+ }
+
+ if (predict_probability_)
+ {
+ if (svm_check_probability_model (&model_) == 0)
+ {
+ PCL_WARN ("[pcl::%s::classification] Classifier model does not support probabiliy estimates. Automatically disabled.\n", getClassName ().c_str ());
+ predict_probability_ = 0;
+ }
+ }
+ else
+ {
+ if (svm_check_probability_model (&model_) != 0)
+ PCL_WARN("[pcl::%s::classification] Classifier model supports probability estimates, but disabled in prediction.\n", getClassName ().c_str ());
+ }
+
+ int svm_type = svm_get_svm_type (&model_);
+ int nr_class = svm_get_nr_class (&model_);
+ double *prob_estimates = NULL;
+
+ int j;
+
+ svm_node *buff;
+ buff = Malloc (struct svm_node, in.SV.size() + 10);
+
+ size_t i = 0;
+
+ for (i = 0; i < in.SV.size (); i++)
+ {
+ buff[i].index = in.SV[i].idx;
+
+ if (in.SV[i].idx < scaling_.max && scaling_.obj[in.SV[i].idx].index == 1)
+ buff[i].value = in.SV[i].value / scaling_.obj[in.SV[i].idx].value;
+ else
+ buff[i].value = in.SV[i].value;
+ }
+
+ buff[i].index = -1;
+
+ // clean the prediction vector
+ prediction_.clear ();
+
+ if (predict_probability_)
+ {
+ if (svm_type == NU_SVR || svm_type == EPSILON_SVR)
+ PCL_WARN ("[pcl::%s::classification] Prob. model for test data: target value = predicted value + z,\nz: Laplace distribution e^(-|z|/sigma)/(2sigma),sigma=%g\n", getClassName ().c_str (),svm_get_svr_probability (&model_));
+ else
+ {
+ prob_estimates = static_cast<double *> (malloc (nr_class * sizeof (double)));
+ }
+ }
+
+ prediction_.resize (1);
+
+ double predict_label;
+
+ if (predict_probability_ && (svm_type == C_SVC || svm_type == NU_SVC))
+ {
+ predict_label = svm_predict_probability (&model_, buff, prob_estimates);
+ prediction_[0].push_back (predict_label);
+
+ for (j = 0;j < nr_class;j++)
+ {
+ prediction_[0].push_back (prob_estimates[j]);
+ }
+ }
+ else
+ {
+ predict_label = svm_predict (&model_, buff);
+ prediction_[0].push_back (predict_label);
+ }
+
+ if (predict_probability_)
+ free (prob_estimates);
+
+ free (buff);
+
+ return prediction_[0];
+};
+
+void
+pcl::SVMClassify::scaleProblem (svm_problem &input, svm_scaling scaling)
+{
+ assert (scaling.max != 0);
+
+ for (int i = 0;i < input.l;i++)
+ {
+ int j = 0;
+
+ while (1)
+ {
+ if (input.x[i][j].index == -1)
+ break;
+
+ if (input.x[i][j].index < scaling.max && scaling.obj[ input.x[i][j].index ].index == 1)
+ input.x[i][j].value = input.x[i][j].value / scaling.obj[ input.x[i][j].index ].value;
+
+ j++;
+ }
+ }
+}
+
+void
+pcl::SVMClassify::saveClassificationResult (const char *filename)
+{
+ assert (prediction_.size() > 0);
+ assert (model_.l > 0);
+
+ std::ofstream output;
+ output.open (filename);
+
+ int nr_class = svm_get_nr_class (&model_);
+ int *labels = static_cast<int *> (malloc (nr_class * sizeof (int)));
+ svm_get_labels (&model_, labels);
+
+ if (predict_probability_)
+ {
+ output << "labels ";
+
+ for (int j = 0 ; j < nr_class; j++)
+ output << labels[j] << " ";
+
+ output << "\n";
+ }
+
+ for (size_t i = 0; i < prediction_.size(); i++)
+ {
+ for (size_t j = 0; j < prediction_[i].size(); j++)
+ output << prediction_[i][j] << " ";
+
+ output << "\n";
+ }
+
+ output.close();
+
+ free (labels);
+}
+
+#define PCL_INSTANTIATE_SVM(T) template class PCL_EXPORTS pcl::SVM<T>;
+#define PCL_INSTANTIATE_SVMTrain(T) template class PCL_EXPORTS pcl::SVMTrain<T>;
+#define PCL_INSTANTIATE_SVMClassify(T) template class PCL_EXPORTS pcl::SVMClassify<T>;
+
+#endif //PCL_SVM_WRAPPER_HPP_
const OctreeKey& key = getCurrentOctreeKey();
// calculate integer id with respect to octree key
unsigned int depth = octree_->getTreeDepth ();
- id = key.x << (depth * 2) | key.y << (depth * 1) | key.z << (depth * 0);
+ id = static_cast<unsigned long> (key.x) << (depth * 2)
+ | static_cast<unsigned long> (key.y) << (depth * 1)
+ | static_cast<unsigned long> (key.z) << (depth * 0);
}
return id;
* \param[out] max_pt upper bound of voxel
*/
inline void
- getVoxelBounds (OctreeIteratorBase<OctreeT>& iterator, Eigen::Vector3f &min_pt, Eigen::Vector3f &max_pt)
+ getVoxelBounds (const OctreeIteratorBase<OctreeT>& iterator, Eigen::Vector3f &min_pt, Eigen::Vector3f &max_pt) const
{
this->genVoxelBoundsFromOctreeKey (iterator.getCurrentOctreeKey (),
iterator.getCurrentOctreeDepth (), min_pt, max_pt);
inline iterator begin () { return (leaf_vector_.begin ()); }
inline iterator end () { return (leaf_vector_.end ()); }
-
+ inline LeafContainerT* at (size_t idx) { return leaf_vector_.at (idx); }
+
// Size of neighbors
inline size_t size () const { return leaf_vector_.size (); }
namespace octree
{
- /** \brief @b Octree adjacency leaf container class- stores set of pointers to neighbors, number of points added, and a DataT value
+ /** \brief @b Octree adjacency leaf container class- stores a list of pointers to neighbors, number of points added, and a DataT value
* \note This class implements a leaf node that stores pointers to neighboring leaves
* \note This class also has a virtual computeData function, which is called by octreePointCloudAdjacency::addPointsFromInputCloud.
* \note You should make explicit instantiations of it for your pointtype/datatype combo (if needed) see supervoxel_clustering.hpp for an example of this
template<typename PointInT, typename DataT = PointInT>
class OctreePointCloudAdjacencyContainer : public OctreeContainerBase
{
+ template<typename T, typename U, typename V>
+ friend class OctreePointCloudAdjacency;
public:
- typedef std::set<OctreePointCloudAdjacencyContainer*> NeighborSetT;
- //iterators to neighbors
- typedef typename NeighborSetT::iterator iterator;
- typedef typename NeighborSetT::const_iterator const_iterator;
- inline iterator begin () { return (neighbors_.begin ()); }
- inline iterator end () { return (neighbors_.end ()); }
- inline const_iterator begin () const { return (neighbors_.begin ()); }
- inline const_iterator end () const { return (neighbors_.end ()); }
+ typedef std::list<OctreePointCloudAdjacencyContainer*> NeighborListT;
+ typedef typename NeighborListT::const_iterator const_iterator;
+ //const iterators to neighbors
+ inline const_iterator cbegin () const { return (neighbors_.begin ()); }
+ inline const_iterator cend () const { return (neighbors_.end ()); }
//size of neighbors
inline size_t size () const { return neighbors_.size (); }
- //insert for neighbors
- inline std::pair<iterator, bool> insert (OctreePointCloudAdjacencyContainer* neighbor) { return neighbors_.insert (neighbor); }
/** \brief Class initialization. */
OctreePointCloudAdjacencyContainer () :
{
}
+ /** \brief Returns the number of neighbors this leaf has
+ * \returns number of neighbors
+ */
+ size_t
+ getNumNeighbors () const
+ {
+ return neighbors_.size ();
+ }
+
+ /** \brief Gets the number of points contributing to this leaf */
+ int
+ getPointCounter () const { return num_points_; }
+
+ /** \brief Returns a reference to the data member to access it without copying */
+ DataT&
+ getData () { return data_; }
+
+ /** \brief Sets the data member
+ * \param[in] data_arg New value for data
+ */
+ void
+ setData (const DataT& data_arg) { data_ = data_arg;}
+
+ /** \brief virtual method to get size of container
+ * \return number of points added to leaf node container.
+ */
+ virtual size_t
+ getSize () const
+ {
+ return num_points_;
+ }
+ protected:
+ //iterators to neighbors
+ typedef typename NeighborListT::iterator iterator;
+ inline iterator begin () { return (neighbors_.begin ()); }
+ inline iterator end () { return (neighbors_.end ()); }
+
/** \brief deep copy function */
virtual OctreePointCloudAdjacencyContainer *
deepCopy () const
{
}
- /** \brief Gets the number of points contributing to this leaf */
- int
- getPointCounter () const { return num_points_; }
-
/** \brief Sets the number of points contributing to this leaf */
void
setPointCounter (int points_arg) { num_points_ = points_arg; }
void
addNeighbor (OctreePointCloudAdjacencyContainer *neighbor)
{
- neighbors_.insert (neighbor);
+ neighbors_.push_back (neighbor);
}
/** \brief Remove neighbor from neighbor set.
void
removeNeighbor (OctreePointCloudAdjacencyContainer *neighbor)
{
- neighbors_.erase (neighbor);
-
- }
-
- /** \brief Returns the number of neighbors this leaf has
- * \returns number of neighbors
- */
- size_t
- getNumNeighbors () const
- {
- return neighbors_.size ();
+ for (iterator neighb_it = neighbors_.begin(); neighb_it != neighbors_.end(); ++neighb_it)
+ {
+ if ( *neighb_it == neighbor)
+ {
+ neighbors_.erase (neighb_it);
+ return;
+ }
+ }
}
/** \brief Sets the whole neighbor set
* \param[in] neighbor_arg the new set
*/
void
- setNeighbors (const NeighborSetT &neighbor_arg)
+ setNeighbors (const NeighborListT &neighbor_arg)
{
neighbors_ = neighbor_arg;
}
- /** \brief Returns a reference to the data member to access it without copying */
- DataT&
- getData () { return data_; }
-
- /** \brief Sets the data member
- * \param[in] data_arg New value for data
- */
- void
- setData (const DataT& data_arg) { data_ = data_arg;}
-
- /** \brief virtual method to get size of container
- * \return number of points added to leaf node container.
- */
- virtual size_t
- getSize () const
- {
- return num_points_;
- }
-
-
private:
int num_points_;
- NeighborSetT neighbors_;
+ NeighborListT neighbors_;
DataT data_;
};
}
* \param point_arg: input point
* */
void setOccupiedVoxelAtPoint( const PointT& point_arg ) {
- OctreeKey key;
+ OctreeKey key;
// make sure bounding box is big enough
- adoptBoundingBoxToPoint (point_arg);
+ this->adoptBoundingBoxToPoint (point_arg);
// generate key
- genOctreeKeyforPoint (point_arg, key);
+ this->genOctreeKeyforPoint (point_arg, key);
// add point to octree at key
this->createLeaf (key);
{
}
- /** \brief Empty class constructor. */
+ /** \brief Empty class destructor. */
virtual
~OctreePointCloudSearch ()
{
if ( indices[i].empty () )
continue;
- if ( children_[i] == false )
+ if (children_[i] == 0)
{
createChild (i);
}
if(indices[i].empty ())
continue;
- if( children_[i] == false )
+ if (children_[i] == 0)
{
assert (i < 8);
createChild (i);
* \param[in] min The minimum corner of the boudning box to query.
* \param[out] max The maximum corner of the bounding box to query.
* \param[in] query_depth The depth in the tree at which to look for the points. Only returns points within the given bounding box at the specified \c query_depth.
+ * \param percent
* \param[out] dst The destination in which to return the points.
*
*/
{
std::set<vtkRenderer*>::iterator renderer_it;
for (renderer_it = associated_renderers_[actor].begin (); renderer_it != associated_renderers_[actor].end ();
- renderer_it++)
+ ++renderer_it)
{
(*renderer_it)->RemoveActor (actor);
}
// PCL - visualziation
#include <pcl/visualization/common/common.h>
+
+#if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
#include <pcl/visualization/vtk/vtkVertexBufferObjectMapper.h>
+#endif
// VTK
#include <vtkVersion.h>
{
vtkSmartPointer<vtkActor> cloud_actor = vtkSmartPointer<vtkActor>::New ();
- vtkSmartPointer<vtkVertexBufferObjectMapper> mapper = vtkSmartPointer<vtkVertexBufferObjectMapper>::New ();
-
CloudDataCacheItem *cloud_data_cache_item = &cloud_data_cache.get(pcd_file);
+#if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
+ vtkSmartPointer<vtkVertexBufferObjectMapper> mapper = vtkSmartPointer<vtkVertexBufferObjectMapper>::New ();
mapper->SetInput (cloud_data_cache_item->item);
+#else
+ vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
+ // Usually we choose between SetInput and SetInputData based on VTK version. But OpenGL ≥ 2 automatically
+ // means VTK version is ≥ 6.3
+ mapper->SetInputData (cloud_data_cache_item->item);
+#endif
+
cloud_actor->SetMapper (mapper);
cloud_actor->GetProperty ()->SetColor (0.0, 0.0, 1.0);
cloud_actor->GetProperty ()->SetPointSize (1);
#cmakedefine HAVE_FZAPI 1
+#cmakedefine HAVE_ENSENSO 1
+
+#cmakedefine HAVE_DAVIDSDK 1
+
// SSE macros
#cmakedefine HAVE_POSIX_MEMALIGN
#cmakedefine HAVE_MM_MALLOC
+#cmakedefine HAVE_SSE4_2_EXTENSIONS
#cmakedefine HAVE_SSE4_1_EXTENSIONS
+#cmakedefine HAVE_SSSE3_EXTENSIONS
#cmakedefine HAVE_SSE3_EXTENSIONS
#cmakedefine HAVE_SSE2_EXTENSIONS
#cmakedefine HAVE_SSE_EXTENSIONS
/* Address the cases where on MacOS and OpenGL and GLUT are not frameworks */
#cmakedefine OPENGL_IS_A_FRAMEWORK
#cmakedefine GLUT_IS_A_FRAMEWORK
+
+/* Version of OpenGL used by VTK as rendering backend */
+#define VTK_RENDERING_BACKEND_OPENGL_VERSION ${VTK_RENDERING_BACKEND_OPENGL_VERSION}
+
#SET_TARGET_PROPERTIES("${LIB_NAME}" PROPERTIES LINKER_LANGUAGE CXX)
- if(OPENNI_FOUND AND BUILD_OPENNI)
+ if(WITH_OPENNI)
PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ground_based_rgbd_people_detector "${SUBSYS_NAME}" apps/main_ground_based_people_detection.cpp)
target_link_libraries(pcl_ground_based_rgbd_people_detector pcl_common pcl_kdtree pcl_search pcl_features pcl_sample_consensus pcl_filters pcl_io pcl_visualization pcl_segmentation pcl_people)
endif()
/**
* \brief Set the transformation matrix, which is used in order to transform the given point cloud, the ground plane and the intrinsics matrix to the internal coordinate frame.
- *
- * \param[in] cloud A pointer to the input cloud.
+ * \param[in] transformation
*/
void
setTransformation (const Eigen::Matrix3f& transformation);
float *M, *O, *G, *H;
M = new float[h_ * w_];
O = new float[h_ * w_];
- H = (float*) calloc((w_ / bin_size_) * (h_ / bin_size_) * n_orients_, sizeof(float));
- G = (float*) calloc((w_ / bin_size_) * (h_ / bin_size_) * n_orients_ *4, sizeof(float));
+ H = new float[(w_ / bin_size_) * (h_ / bin_size_) * n_orients_]();
+ G = new float[(w_ / bin_size_) * (h_ / bin_size_) * n_orients_ *4]();
// Compute gradient magnitude and orientation at each location (uses sse):
gradMag (I, h_, w_, n_channels_, M, O );
}
}
}
- free(M); free(O); free(H); free(G);
+ delete[] M; delete[] O; delete[] H; delete[] G;
}
void
set(SUBSYS_NAME recognition)
set(SUBSYS_DESC "Point cloud recognition library")
-set(SUBSYS_DEPS common io search kdtree octree features filters registration sample_consensus)
+set(SUBSYS_DEPS common io search kdtree octree features filters registration sample_consensus ml)
set(build TRUE)
PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
"include/pcl/${SUBSYS_NAME}/hv/greedy_verification.h"
)
+ set(face_detection_incs
+ "include/pcl/${SUBSYS_NAME}/face_detection/face_common.h"
+ "include/pcl/${SUBSYS_NAME}/face_detection/face_detector_data_provider.h"
+ "include/pcl/${SUBSYS_NAME}/face_detection/rf_face_detector_trainer.h"
+ "include/pcl/${SUBSYS_NAME}/face_detection/rf_face_utils.h"
+ )
+
set(cg_incs
"include/pcl/${SUBSYS_NAME}/cg/correspondence_grouping.h"
"include/pcl/${SUBSYS_NAME}/cg/hough_3d.h"
src/ransac_based/model_library.cpp
src/ransac_based/orr_octree.cpp
src/ransac_based/orr_octree_zprojection.cpp
- src/implicit_shape_model.cpp
+ src/face_detection/face_detector_data_provider.cpp
+ src/face_detection/rf_face_detector_trainer.cpp
+ src/implicit_shape_model.cpp
)
if (HAVE_METSLIB)
set(LIB_NAME "pcl_${SUBSYS_NAME}")
include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
- PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs} ${ransac_based_incs} ${ransac_based_impl_incs} ${hv_incs} ${hv_impl_incs} ${cg_incs} ${cg_impl_incs} ${metslib_incs})
- target_link_libraries("${LIB_NAME}" pcl_common pcl_kdtree pcl_octree pcl_search pcl_features pcl_registration pcl_sample_consensus pcl_filters pcl_io)
+ PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs} ${face_detection_incs} ${ransac_based_incs} ${ransac_based_impl_incs} ${hv_incs} ${hv_impl_incs} ${cg_incs} ${cg_impl_incs} ${metslib_incs})
+ target_link_libraries("${LIB_NAME}" pcl_common pcl_kdtree pcl_octree pcl_search pcl_features pcl_registration pcl_sample_consensus pcl_filters pcl_ml pcl_io)
PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "")
# Install include files
PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs})
PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/ransac_based" ${ransac_based_incs})
PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/hv" ${hv_incs})
PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/cg" ${cg_incs})
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/face_detection" ${face_detection_incs})
PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs})
PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl/ransac_based" ${ransac_based_impl_incs})
PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/linemod" ${LINEMOD_INCLUDES})
PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl/linemod" ${LINEMOD_IMPLS})
+ if (NOT HAVE_METSLIB)
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/3rdparty/metslib" ${metslib_incs})
+ endif(NOT HAVE_METSLIB)
+
endif(build)
#include <pcl/recognition/cg/correspondence_grouping.h>
#include <pcl/recognition/boost.h>
+#include <pcl/point_types.h>
namespace pcl
{
bool needs_training_;
/** \brief The result of the training. The vector between each model point and the centroid of the model adjusted by its local reference frame.*/
- std::vector<Eigen::Vector3f> model_votes_;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > model_votes_;
/** \brief The minimum number of votes in the Hough space needed to infer the presence of a model instance into the scene cloud. */
double hough_threshold_;
--- /dev/null
+#ifndef FACE_DETECTOR_COMMON_H_
+#define FACE_DETECTOR_COMMON_H_
+
+#include <pcl/features/integral_image2D.h>
+
+namespace pcl
+{
+ namespace face_detection
+ {
+ class TrainingExample
+ {
+ public:
+ std::vector<boost::shared_ptr<pcl::IntegralImage2D<float, 1> > > iimages_; //also pointer to the respective integral image
+ int row_, col_;
+ int wsize_;
+ int label_;
+
+ //save pose head information
+ Eigen::Vector3f trans_;
+ Eigen::Vector3f rot_;
+ };
+
+ class FeatureType
+ {
+ public:
+ int row1_, col1_;
+ int row2_, col2_;
+
+ int wsizex1_, wsizey1_;
+ int wsizex2_, wsizey2_;
+
+ float threshold_;
+ int used_ii_;
+
+ FeatureType()
+ {
+ used_ii_ = 0;
+ }
+
+ void serialize(std::ostream & stream) const
+ {
+ stream.write (reinterpret_cast<const char*> (&row1_), sizeof(row1_));
+ stream.write (reinterpret_cast<const char*> (&col1_), sizeof(col1_));
+ stream.write (reinterpret_cast<const char*> (&row2_), sizeof(row2_));
+ stream.write (reinterpret_cast<const char*> (&col2_), sizeof(col2_));
+ stream.write (reinterpret_cast<const char*> (&wsizex1_), sizeof(wsizex1_));
+ stream.write (reinterpret_cast<const char*> (&wsizex2_), sizeof(wsizex2_));
+ stream.write (reinterpret_cast<const char*> (&wsizey1_), sizeof(wsizey1_));
+ stream.write (reinterpret_cast<const char*> (&wsizey2_), sizeof(wsizey2_));
+ stream.write (reinterpret_cast<const char*> (&threshold_), sizeof(threshold_));
+ stream.write (reinterpret_cast<const char*> (&used_ii_), sizeof(used_ii_));
+ }
+
+ inline void deserialize(std::istream & stream)
+ {
+ stream.read (reinterpret_cast<char*> (&row1_), sizeof(row1_));
+ stream.read (reinterpret_cast<char*> (&col1_), sizeof(col1_));
+ stream.read (reinterpret_cast<char*> (&row2_), sizeof(row2_));
+ stream.read (reinterpret_cast<char*> (&col2_), sizeof(col2_));
+ stream.read (reinterpret_cast<char*> (&wsizex1_), sizeof(wsizex1_));
+ stream.read (reinterpret_cast<char*> (&wsizex2_), sizeof(wsizex2_));
+ stream.read (reinterpret_cast<char*> (&wsizey1_), sizeof(wsizey1_));
+ stream.read (reinterpret_cast<char*> (&wsizey2_), sizeof(wsizey2_));
+ stream.read (reinterpret_cast<char*> (&threshold_), sizeof(threshold_));
+ stream.read (reinterpret_cast<char*> (&used_ii_), sizeof(used_ii_));
+ }
+ };
+
+ template<class FeatureType>
+ class RFTreeNode
+ {
+ public:
+ float threshold;
+ FeatureType feature;
+ std::vector<RFTreeNode> sub_nodes;
+ float value;
+ float variance;
+
+ Eigen::Vector3d trans_mean_;
+ Eigen::Vector3d rot_mean_;
+
+ float purity_;
+ Eigen::Matrix3d covariance_trans_;
+ Eigen::Matrix3d covariance_rot_;
+
+ void serialize(::std::ostream & stream) const
+ {
+
+ const int num_of_sub_nodes = static_cast<int> (sub_nodes.size ());
+ stream.write (reinterpret_cast<const char*> (&num_of_sub_nodes), sizeof(num_of_sub_nodes));
+
+ if (sub_nodes.size () > 0)
+ {
+ feature.serialize (stream);
+ stream.write (reinterpret_cast<const char*> (&threshold), sizeof(threshold));
+ }
+
+ stream.write (reinterpret_cast<const char*> (&value), sizeof(value));
+ stream.write (reinterpret_cast<const char*> (&variance), sizeof(variance));
+
+ for (size_t i = 0; i < 3; i++)
+ stream.write (reinterpret_cast<const char*> (&trans_mean_[i]), sizeof(trans_mean_[i]));
+
+ for (size_t i = 0; i < 3; i++)
+ stream.write (reinterpret_cast<const char*> (&rot_mean_[i]), sizeof(rot_mean_[i]));
+
+ for (size_t i = 0; i < 3; i++)
+ for (size_t j = 0; j < 3; j++)
+ stream.write (reinterpret_cast<const char*> (&covariance_trans_ (i, j)), sizeof(covariance_trans_ (i, j)));
+
+ for (size_t i = 0; i < 3; i++)
+ for (size_t j = 0; j < 3; j++)
+ stream.write (reinterpret_cast<const char*> (&covariance_rot_ (i, j)), sizeof(covariance_rot_ (i, j)));
+
+ for (int sub_node_index = 0; sub_node_index < num_of_sub_nodes; ++sub_node_index)
+ {
+ sub_nodes[sub_node_index].serialize (stream);
+ }
+ }
+
+ inline void deserialize(::std::istream & stream)
+ {
+ int num_of_sub_nodes;
+ stream.read (reinterpret_cast<char*> (&num_of_sub_nodes), sizeof(num_of_sub_nodes));
+
+ if (num_of_sub_nodes > 0)
+ {
+ feature.deserialize (stream);
+ stream.read (reinterpret_cast<char*> (&threshold), sizeof(threshold));
+ }
+
+ stream.read (reinterpret_cast<char*> (&value), sizeof(value));
+ stream.read (reinterpret_cast<char*> (&variance), sizeof(variance));
+
+ for (size_t i = 0; i < 3; i++)
+ stream.read (reinterpret_cast<char*> (&trans_mean_[i]), sizeof(trans_mean_[i]));
+
+ for (size_t i = 0; i < 3; i++)
+ stream.read (reinterpret_cast<char*> (&rot_mean_[i]), sizeof(rot_mean_[i]));
+
+ for (size_t i = 0; i < 3; i++)
+ for (size_t j = 0; j < 3; j++)
+ stream.read (reinterpret_cast<char*> (&covariance_trans_ (i, j)), sizeof(covariance_trans_ (i, j)));
+
+ for (size_t i = 0; i < 3; i++)
+ for (size_t j = 0; j < 3; j++)
+ stream.read (reinterpret_cast<char*> (&covariance_rot_ (i, j)), sizeof(covariance_rot_ (i, j)));
+
+ sub_nodes.resize (num_of_sub_nodes);
+
+ if (num_of_sub_nodes > 0)
+ {
+ for (int sub_node_index = 0; sub_node_index < num_of_sub_nodes; ++sub_node_index)
+ {
+ sub_nodes[sub_node_index].deserialize (stream);
+ }
+ }
+ }
+ };
+ }
+}
+#endif /* FACE_DETECTOR_COMMON_H_ */
--- /dev/null
+/*
+ * face_detector_data_provider.h
+ *
+ * Created on: Sep 2, 2012
+ * Author: aitor
+ */
+
+#ifndef FACE_DETECTOR_DATA_PROVIDER_H_
+#define FACE_DETECTOR_DATA_PROVIDER_H_
+
+#include "pcl/common/common.h"
+#include "pcl/recognition/face_detection/face_common.h"
+#include <pcl/ml/dt/decision_tree_data_provider.h>
+#include <boost/filesystem/operations.hpp>
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <boost/algorithm/string.hpp>
+
+namespace bf = boost::filesystem;
+
+namespace pcl
+{
+ namespace face_detection
+ {
+ template<class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+ class FaceDetectorDataProvider: public pcl::DecisionTreeTrainerDataProvider<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>
+ {
+ private:
+ int num_images_;
+ std::vector<std::string> image_files_;
+ bool USE_NORMALS_;
+ int w_size_;
+ int patches_per_image_;
+ int min_images_per_bin_;
+
+ void getFilesInDirectory(bf::path & dir, std::string & rel_path_so_far, std::vector<std::string> & relative_paths, std::string & ext)
+ {
+ bf::directory_iterator end_itr;
+ for (bf::directory_iterator itr (dir); itr != end_itr; ++itr)
+ {
+ //check if its a directory, then get models in it
+ if (bf::is_directory (*itr))
+ {
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string so_far = rel_path_so_far + (itr->path ().filename ()).string () + "/";
+#else
+ std::string so_far = rel_path_so_far + (itr->path ()).filename () + "/";
+#endif
+
+ bf::path curr_path = itr->path ();
+ getFilesInDirectory (curr_path, so_far, relative_paths, ext);
+ } else
+ {
+ //check that it is a ply file and then add, otherwise ignore..
+ std::vector < std::string > strs;
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string file = (itr->path ().filename ()).string ();
+#else
+ std::string file = (itr->path ()).filename ();
+#endif
+
+ boost::split (strs, file, boost::is_any_of ("."));
+ std::string extension = strs[strs.size () - 1];
+
+ if (extension.compare (ext) == 0)
+ {
+#if BOOST_FILESYSTEM_VERSION == 3
+ std::string path = rel_path_so_far + (itr->path ().filename ()).string ();
+#else
+ std::string path = rel_path_so_far + (itr->path ()).filename ();
+#endif
+
+ relative_paths.push_back (path);
+ }
+ }
+ }
+ }
+
+ inline bool readMatrixFromFile(std::string file, Eigen::Matrix4f & matrix)
+ {
+
+ std::ifstream in;
+ in.open (file.c_str (), std::ifstream::in);
+ if (!in.is_open ())
+ {
+ return false;
+ }
+
+ char linebuf[1024];
+ in.getline (linebuf, 1024);
+ std::string line (linebuf);
+ std::vector < std::string > strs_2;
+ boost::split (strs_2, line, boost::is_any_of (" "));
+
+ for (int i = 0; i < 16; i++)
+ {
+ matrix (i / 4, i % 4) = static_cast<float> (atof (strs_2[i].c_str ()));
+ }
+
+ return true;
+ }
+
+ bool check_inside(int col, int row, int min_col, int max_col, int min_row, int max_row)
+ {
+ if (col >= min_col && col <= max_col && row >= min_row && row <= max_row)
+ return true;
+
+ return false;
+ }
+
+ template<class PointInT>
+ void cropCloud(int min_col, int max_col, int min_row, int max_row, pcl::PointCloud<PointInT> & cloud_in, pcl::PointCloud<PointInT> & cloud_out)
+ {
+ cloud_out.width = max_col - min_col + 1;
+ cloud_out.height = max_row - min_row + 1;
+ cloud_out.points.resize (cloud_out.width * cloud_out.height);
+ for (unsigned int u = 0; u < cloud_out.width; u++)
+ {
+ for (unsigned int v = 0; v < cloud_out.height; v++)
+ {
+ cloud_out.at (u, v) = cloud_in.at (min_col + u, min_row + v);
+ }
+ }
+
+ cloud_out.is_dense = cloud_in.is_dense;
+ }
+
+ public:
+
+ FaceDetectorDataProvider()
+ {
+ w_size_ = 80;
+ USE_NORMALS_ = false;
+ num_images_ = 10;
+ patches_per_image_ = 20;
+ min_images_per_bin_ = -1;
+ }
+
+ virtual ~FaceDetectorDataProvider()
+ {
+
+ }
+
+ void setPatchesPerImage(int n)
+ {
+ patches_per_image_ = n;
+ }
+
+ void setMinImagesPerBin(int n)
+ {
+ min_images_per_bin_ = n;
+ }
+
+ void setUseNormals(bool use)
+ {
+ USE_NORMALS_ = use;
+ }
+
+ void setWSize(int size)
+ {
+ w_size_ = size;
+ }
+
+ void setNumImages(int n)
+ {
+ num_images_ = n;
+ }
+
+ void initialize(std::string & data_dir);
+
+ //shuffle file and get the first num_images_ as requested by a tree
+ //extract positive and negative samples
+ //create training examples and labels
+ void getDatasetAndLabels(DataSet & data_set, std::vector<LabelType> & label_data, std::vector<ExampleIndex> & examples);
+ };
+ }
+}
+
+#endif /* FACE_DETECTOR_DATA_PROVIDER_H_ */
--- /dev/null
+/*
+ * rf_face_detector_trainer.h
+ *
+ * Created on: 22 Sep 2012
+ * Author: Aitor Aldoma
+ */
+
+#ifndef PCL_RF_FACE_DETECTOR_TRAINER_H_
+#define PCL_RF_FACE_DETECTOR_TRAINER_H_
+
+#include "pcl/recognition/face_detection/face_detector_data_provider.h"
+#include "pcl/recognition/face_detection/rf_face_utils.h"
+#include "pcl/ml/dt/decision_forest.h"
+#include <pcl/features/integral_image2D.h>
+
+namespace pcl
+{
+ class PCL_EXPORTS RFFaceDetectorTrainer
+ {
+ private:
+ int w_size_;
+ int max_patch_size_;
+ int stride_sw_;
+ int ntrees_;
+ std::string forest_filename_;
+ int nfeatures_;
+ float thres_face_;
+ int num_images_;
+ float trans_max_variance_;
+ size_t min_votes_size_;
+ int used_for_pose_;
+ bool use_normals_;
+ std::string directory_;
+ float HEAD_ST_DIAMETER_;
+ float larger_radius_ratio_;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > head_center_votes_;
+ std::vector<std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > > head_center_votes_clustered_;
+ std::vector<std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > > head_center_original_votes_clustered_;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > angle_votes_;
+ std::vector<float> uncertainties_;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > head_clusters_centers_;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > head_clusters_rotation_;
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr input_;
+ pcl::PointCloud<pcl::PointXYZI>::Ptr face_heat_map_;
+
+ typedef face_detection::RFTreeNode<face_detection::FeatureType> NodeType;
+ pcl::DecisionForest<NodeType> forest_;
+
+ std::string model_path_;
+ bool pose_refinement_;
+ int icp_iterations_;
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr model_original_;
+ float res_;
+
+ public:
+
+ RFFaceDetectorTrainer()
+ {
+ w_size_ = 80;
+ max_patch_size_ = 40;
+ stride_sw_ = 4;
+ ntrees_ = 10;
+ forest_filename_ = std::string ("forest.txt");
+ nfeatures_ = 10000;
+ thres_face_ = 1.f;
+ num_images_ = 1000;
+ trans_max_variance_ = 1600.f;
+ used_for_pose_ = std::numeric_limits<int>::max ();
+ use_normals_ = false;
+ directory_ = std::string ("");
+ HEAD_ST_DIAMETER_ = 0.2364f;
+ larger_radius_ratio_ = 1.5f;
+ face_heat_map_.reset ();
+ model_path_ = std::string ("face_mesh.ply");
+ pose_refinement_ = false;
+ res_ = 0.005f;
+ }
+
+ virtual ~RFFaceDetectorTrainer()
+ {
+
+ }
+
+ /*
+ * Common parameters
+ */
+ void setForestFilename(std::string & ff)
+ {
+ forest_filename_ = ff;
+ }
+
+ void setUseNormals(bool use)
+ {
+ use_normals_ = use;
+ }
+
+ void setWSize(int s)
+ {
+ w_size_ = s;
+ }
+
+ /*
+ * Training parameters
+ */
+
+ void setDirectory(std::string & dir)
+ {
+ directory_ = dir;
+ }
+ void setNumTrainingImages(int num)
+ {
+ num_images_ = num;
+ }
+
+ void setNumTrees(int num)
+ {
+ ntrees_ = num;
+ }
+
+ void setNumFeatures(int num)
+ {
+ nfeatures_ = num;
+ }
+
+ /*
+ * Detection parameters
+ */
+
+ void setModelPath(std::string & model);
+
+ void setPoseRefinement(bool do_it, int iters = 5)
+ {
+ pose_refinement_ = do_it;
+ icp_iterations_ = iters;
+ }
+
+ void setLeavesFaceThreshold(float p)
+ {
+ thres_face_ = p;
+ }
+
+ void setLeavesFaceMaxVariance(float max)
+ {
+ trans_max_variance_ = max;
+ }
+
+ void setWStride(int s)
+ {
+ stride_sw_ = s;
+ }
+
+ void setFaceMinVotes(int mv)
+ {
+ min_votes_size_ = mv;
+ }
+
+ void setNumVotesUsedForPose(int n)
+ {
+ used_for_pose_ = n;
+ }
+
+ void setForest(pcl::DecisionForest<NodeType> & forest)
+ {
+ forest_ = forest;
+ }
+
+ /*
+ * Get functions
+ */
+
+ void getFaceHeatMap(pcl::PointCloud<pcl::PointXYZI>::Ptr & heat_map)
+ {
+ heat_map = face_heat_map_;
+ }
+
+ //get votes
+ void getVotes(pcl::PointCloud<pcl::PointXYZ>::Ptr & votes_cloud)
+ {
+ votes_cloud->points.resize (head_center_votes_.size ());
+ votes_cloud->width = static_cast<int>(head_center_votes_.size ());
+ votes_cloud->height = 1;
+
+ for (size_t i = 0; i < head_center_votes_.size (); i++)
+ {
+ votes_cloud->points[i].getVector3fMap () = head_center_votes_[i];
+ }
+ }
+
+ void getVotes(pcl::PointCloud<pcl::PointXYZI>::Ptr & votes_cloud)
+ {
+ votes_cloud->points.resize (head_center_votes_.size ());
+ votes_cloud->width = static_cast<int>(head_center_votes_.size ());
+ votes_cloud->height = 1;
+
+ int p = 0;
+ for (size_t i = 0; i < head_center_votes_clustered_.size (); i++)
+ {
+ for (size_t j = 0; j < head_center_votes_clustered_[i].size (); j++, p++)
+ {
+ votes_cloud->points[p].getVector3fMap () = head_center_votes_clustered_[i][j];
+ votes_cloud->points[p].intensity = 0.1f * static_cast<float> (i);
+ }
+ }
+
+ votes_cloud->points.resize (p);
+ }
+
+ void getVotes2(pcl::PointCloud<pcl::PointXYZI>::Ptr & votes_cloud)
+ {
+ votes_cloud->points.resize (head_center_votes_.size ());
+ votes_cloud->width = static_cast<int>(head_center_votes_.size ());
+ votes_cloud->height = 1;
+
+ int p = 0;
+ for (size_t i = 0; i < head_center_original_votes_clustered_.size (); i++)
+ {
+ for (size_t j = 0; j < head_center_original_votes_clustered_[i].size (); j++, p++)
+ {
+ votes_cloud->points[p].getVector3fMap () = head_center_original_votes_clustered_[i][j];
+ votes_cloud->points[p].intensity = 0.1f * static_cast<float> (i);
+ }
+ }
+
+ votes_cloud->points.resize (p);
+ }
+
+ //get heads
+ void getDetectedFaces(std::vector<Eigen::VectorXf> & faces)
+ {
+ for (size_t i = 0; i < head_clusters_centers_.size (); i++)
+ {
+ Eigen::VectorXf head (6);
+ head[0] = head_clusters_centers_[i][0];
+ head[1] = head_clusters_centers_[i][1];
+ head[2] = head_clusters_centers_[i][2];
+ head[3] = head_clusters_rotation_[i][0];
+ head[4] = head_clusters_rotation_[i][1];
+ head[5] = head_clusters_rotation_[i][2];
+ faces.push_back (head);
+ }
+ }
+ /*
+ * Other functions
+ */
+ void setInputCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud)
+ {
+ input_ = cloud;
+ }
+
+ void setFaceHeatMapCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & heat_map)
+ {
+ face_heat_map_ = heat_map;
+ }
+
+ void trainWithDataProvider();
+ void faceVotesClustering();
+ void detectFaces();
+ };
+}
+
+#endif /* PCL_RF_FACE_DETECTOR_TRAINER_H_ */
--- /dev/null
+/*
+ * fanellis_face_detector.h
+ *
+ * Created on: 22 Sep 2012
+ * Author: Aitor Aldoma
+ */
+
+#ifndef PCL_RF_FACE_UTILS_H_
+#define PCL_RF_FACE_UTILS_H_
+
+#include "pcl/recognition/face_detection/face_common.h"
+#include <pcl/ml/feature_handler.h>
+#include <pcl/ml/stats_estimator.h>
+#include <pcl/ml/branch_estimator.h>
+
+namespace pcl
+{
+ namespace face_detection
+ {
+ template<class FT, class DataSet, class ExampleIndex>
+ class FeatureHandlerDepthAverage: public pcl::FeatureHandler<FT, DataSet, ExampleIndex>
+ {
+
+ private:
+ int wsize_; //size of the window
+ int max_patch_size_; //max size of the smaller patches
+ int num_channels_; //the number of feature channels
+ float min_valid_small_patch_depth_; //percentage of valid depth in a small patch
+ public:
+
+ FeatureHandlerDepthAverage()
+ {
+ wsize_ = 80;
+ max_patch_size_ = 40;
+ num_channels_ = 1;
+ min_valid_small_patch_depth_ = 0.5f;
+ }
+
+ /** \brief Sets the size of the window to extract features.
+ * \param[in] w Window size.
+ */
+ void setWSize(int w)
+ {
+ wsize_ = w;
+ }
+
+ /** \brief Sets the number of channels a feature has (i.e. 1 - depth, 4 - depth + normals)
+ * \param[in] nf Number of channels.
+ */
+ void setNumChannels(int nf)
+ {
+ num_channels_ = nf;
+ }
+
+ /** \brief Create a set of random tests to evaluate examples.
+ * \param[in] w Number features to generate.
+ */
+ void setMaxPatchSize(int w)
+ {
+ max_patch_size_ = w;
+ }
+
+ /** \brief Create a set of random tests to evaluate examples.
+ * \param[in] num_of_features Number features to generated.
+ * \param[out] features Generated features.
+ */
+ /*void createRandomFeatures(const size_t num_of_features, std::vector<FT> & features)
+ {
+ srand (time(NULL));
+ int min_s = 10;
+ float range_d = 0.03f;
+ for (size_t i = 0; i < num_of_features; i++)
+ {
+ FT f;
+
+ f.row1_ = rand () % (wsize_ - max_patch_size_ - 1);
+ f.col1_ = rand () % (wsize_ / 2 - max_patch_size_ - 1);
+ f.wsizex1_ = min_s + (rand () % (max_patch_size_ - min_s - 1));
+ f.wsizey1_ = min_s + (rand () % (max_patch_size_ - min_s - 1));
+
+ f.row2_ = rand () % (wsize_ - max_patch_size_ - 1);
+ f.col2_ = wsize_ / 2 + rand () % (wsize_ / 2 - max_patch_size_ - 1);
+ f.wsizex2_ = min_s + (rand () % (max_patch_size_ - 1 - min_s));
+ f.wsizey2_ = min_s + (rand () % (max_patch_size_ - 1 - min_s));
+
+ f.used_ii_ = 0;
+ if(num_channels_ > 1)
+ f.used_ii_ = rand() % num_channels_;
+
+ f.threshold_ = -range_d + (rand () / static_cast<float> (RAND_MAX)) * (range_d * 2.f);
+ features.push_back (f);
+ }
+ }*/
+
+ void createRandomFeatures(const size_t num_of_features, std::vector<FT> & features)
+ {
+ srand (static_cast<unsigned int>(time (NULL)));
+ int min_s = 20;
+ float range_d = 0.05f;
+ float incr_d = 0.01f;
+
+ std::vector < FT > windows_and_functions;
+
+ for (size_t i = 0; i < num_of_features; i++)
+ {
+ FT f;
+
+ f.row1_ = rand () % (wsize_ - max_patch_size_ - 1);
+ f.col1_ = rand () % (wsize_ / 2 - max_patch_size_ - 1);
+ f.wsizex1_ = min_s + (rand () % (max_patch_size_ - min_s - 1));
+ f.wsizey1_ = min_s + (rand () % (max_patch_size_ - min_s - 1));
+
+ f.row2_ = rand () % (wsize_ - max_patch_size_ - 1);
+ f.col2_ = wsize_ / 2 + rand () % (wsize_ / 2 - max_patch_size_ - 1);
+ f.wsizex2_ = min_s + (rand () % (max_patch_size_ - 1 - min_s));
+ f.wsizey2_ = min_s + (rand () % (max_patch_size_ - 1 - min_s));
+
+ f.used_ii_ = 0;
+ if (num_channels_ > 1)
+ f.used_ii_ = rand () % num_channels_;
+
+ windows_and_functions.push_back (f);
+ }
+
+ for (size_t i = 0; i < windows_and_functions.size (); i++)
+ {
+ FT f = windows_and_functions[i];
+ for (size_t j = 0; j <= 10; j++)
+ {
+ f.threshold_ = -range_d + static_cast<float> (j) * incr_d;
+ features.push_back (f);
+ }
+ }
+ }
+
+ /** \brief Evaluates a feature on the specified set of examples.
+ * \param[in] feature The feature to evaluate.
+ * \param[in] data_set The data set on which the feature is evaluated.
+ * \param[in] examples The set of examples of the data set the feature is evaluated on.
+ * \param[out] results The destination for the results of the feature evaluation.
+ * \param[out] flags Flags that are supplied together with the results.
+ */
+ void evaluateFeature(const FT & feature, DataSet & data_set, std::vector<ExampleIndex> & examples, std::vector<float> & results,
+ std::vector<unsigned char> & flags) const
+ {
+ results.resize (examples.size ());
+ for (size_t i = 0; i < examples.size (); i++)
+ {
+ evaluateFeature (feature, data_set, examples[i], results[i], flags[i]);
+ }
+ }
+
+ /** \brief Evaluates a feature on the specified example.
+ * \param[in] feature The feature to evaluate.
+ * \param[in] data_set The data set on which the feature is evaluated.
+ * \param[in] example The example of the data set the feature is evaluated on.
+ * \param[out] result The destination for the result of the feature evaluation.
+ * \param[out] flag Flags that are supplied together with the results.
+ */
+ void evaluateFeature(const FT & feature, DataSet & data_set, const ExampleIndex & example, float & result, unsigned char & flag) const
+ {
+ TrainingExample te = data_set[example];
+ int el_f1 = te.iimages_[feature.used_ii_]->getFiniteElementsCount (te.col_ + feature.col1_, te.row_ + feature.row1_, feature.wsizex1_,
+ feature.wsizey1_);
+ int el_f2 = te.iimages_[feature.used_ii_]->getFiniteElementsCount (te.col_ + feature.col2_, te.row_ + feature.row2_, feature.wsizex2_,
+ feature.wsizey2_);
+
+ float sum_f1 = static_cast<float>(te.iimages_[feature.used_ii_]->getFirstOrderSum (te.col_ + feature.col1_, te.row_ + feature.row1_, feature.wsizex1_, feature.wsizey1_));
+ float sum_f2 = static_cast<float>(te.iimages_[feature.used_ii_]->getFirstOrderSum (te.col_ + feature.col2_, te.row_ + feature.row2_, feature.wsizex2_, feature.wsizey2_));
+
+ float f = min_valid_small_patch_depth_;
+ if (el_f1 == 0 || el_f2 == 0 || (el_f1 <= static_cast<int> (f * static_cast<float>(feature.wsizex1_ * feature.wsizey1_)))
+ || (el_f2 <= static_cast<int> (f * static_cast<float>(feature.wsizex2_ * feature.wsizey2_))))
+ {
+ result = static_cast<float> (pcl_round (static_cast<float>(rand ()) / static_cast<float> (RAND_MAX)));
+ flag = 1;
+ } else
+ {
+ result = static_cast<float> ((sum_f1 / static_cast<float>(el_f1) - sum_f2 / static_cast<float>(el_f2)) > feature.threshold_);
+ flag = 0;
+ }
+
+ }
+
+ /** \brief Generates evaluation code for the specified feature and writes it to the specified stream.
+ */
+ // param[in] feature The feature for which code is generated.
+ // param[out] stream The destination for the code.
+ void generateCodeForEvaluation(const FT &/*feature*/, ::std::ostream &/*stream*/) const
+ {
+
+ }
+ };
+
+ /** \brief Statistics estimator for regression trees which optimizes information gain and pose parameters error. */
+ template<class LabelDataType, class NodeType, class DataSet, class ExampleIndex>
+ class PoseClassRegressionVarianceStatsEstimator: public pcl::StatsEstimator<LabelDataType, NodeType, DataSet, ExampleIndex>
+ {
+
+ public:
+ /** \brief Constructor. */
+ PoseClassRegressionVarianceStatsEstimator(BranchEstimator * branch_estimator) :
+ branch_estimator_ (branch_estimator)
+ {
+ }
+
+ /** \brief Destructor. */
+ virtual ~PoseClassRegressionVarianceStatsEstimator()
+ {
+ }
+
+ /** \brief Returns the number of branches the corresponding tree has. */
+ inline size_t getNumOfBranches() const
+ {
+ return branch_estimator_->getNumOfBranches ();
+ }
+
+ /** \brief Returns the label of the specified node.
+ * \param[in] node The node which label is returned.
+ */
+ inline LabelDataType getLabelOfNode(NodeType & node) const
+ {
+ return node.value;
+ }
+
+ /** \brief Computes the covariance matrix for translation offsets.
+ * \param[in] data_set The corresponding data set.
+ * \param[in] examples A set of examples from the dataset.
+ * \param[out] covariance_matrix The covariance matrix.
+ * \param[out] centroid The mean of the data.
+ */
+ inline unsigned int computeMeanAndCovarianceOffset(DataSet & data_set, std::vector<ExampleIndex> & examples, Eigen::Matrix3d & covariance_matrix,
+ Eigen::Vector3d & centroid) const
+ {
+ Eigen::Matrix<double, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 9, Eigen::RowMajor>::Zero ();
+ unsigned int point_count = static_cast<unsigned int> (examples.size ());
+
+ for (size_t i = 0; i < point_count; ++i)
+ {
+ TrainingExample te = data_set[examples[i]];
+ accu[0] += te.trans_[0] * te.trans_[0];
+ accu[1] += te.trans_[0] * te.trans_[1];
+ accu[2] += te.trans_[0] * te.trans_[2];
+ accu[3] += te.trans_[1] * te.trans_[1];
+ accu[4] += te.trans_[1] * te.trans_[2];
+ accu[5] += te.trans_[2] * te.trans_[2];
+ accu[6] += te.trans_[0];
+ accu[7] += te.trans_[1];
+ accu[8] += te.trans_[2];
+ }
+
+ if (point_count != 0)
+ {
+ accu /= static_cast<double> (point_count);
+ centroid.head<3> ().matrix () = accu.tail<3> ();
+ covariance_matrix.coeffRef (0) = accu[0] - accu[6] * accu[6];
+ covariance_matrix.coeffRef (1) = accu[1] - accu[6] * accu[7];
+ covariance_matrix.coeffRef (2) = accu[2] - accu[6] * accu[8];
+ covariance_matrix.coeffRef (4) = accu[3] - accu[7] * accu[7];
+ covariance_matrix.coeffRef (5) = accu[4] - accu[7] * accu[8];
+ covariance_matrix.coeffRef (8) = accu[5] - accu[8] * accu[8];
+ covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
+ covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
+ covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
+ }
+
+ return point_count;
+ }
+
+ /** \brief Computes the covariance matrix for rotation values.
+ * \param[in] data_set The corresponding data set.
+ * \param[in] examples A set of examples from the dataset.
+ * \param[out] covariance_matrix The covariance matrix.
+ * \param[out] centroid The mean of the data.
+ */
+ inline unsigned int computeMeanAndCovarianceAngles(DataSet & data_set, std::vector<ExampleIndex> & examples, Eigen::Matrix3d & covariance_matrix,
+ Eigen::Vector3d & centroid) const
+ {
+ Eigen::Matrix<double, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 9, Eigen::RowMajor>::Zero ();
+ unsigned int point_count = static_cast<unsigned int> (examples.size ());
+
+ for (size_t i = 0; i < point_count; ++i)
+ {
+ TrainingExample te = data_set[examples[i]];
+ accu[0] += te.rot_[0] * te.rot_[0];
+ accu[1] += te.rot_[0] * te.rot_[1];
+ accu[2] += te.rot_[0] * te.rot_[2];
+ accu[3] += te.rot_[1] * te.rot_[1];
+ accu[4] += te.rot_[1] * te.rot_[2];
+ accu[5] += te.rot_[2] * te.rot_[2];
+ accu[6] += te.rot_[0];
+ accu[7] += te.rot_[1];
+ accu[8] += te.rot_[2];
+ }
+
+ if (point_count != 0)
+ {
+ accu /= static_cast<double> (point_count);
+ centroid.head<3> ().matrix () = accu.tail<3> ();
+ covariance_matrix.coeffRef (0) = accu[0] - accu[6] * accu[6];
+ covariance_matrix.coeffRef (1) = accu[1] - accu[6] * accu[7];
+ covariance_matrix.coeffRef (2) = accu[2] - accu[6] * accu[8];
+ covariance_matrix.coeffRef (4) = accu[3] - accu[7] * accu[7];
+ covariance_matrix.coeffRef (5) = accu[4] - accu[7] * accu[8];
+ covariance_matrix.coeffRef (8) = accu[5] - accu[8] * accu[8];
+ covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
+ covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
+ covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
+ }
+
+ return point_count;
+ }
+
+ /** \brief Computes the information gain obtained by the specified threshold.
+ * \param[in] data_set The data set corresponding to the supplied result data.
+ * \param[in] examples The examples used for extracting the supplied result data.
+ * \param[in] label_data The label data corresponding to the specified examples.
+ * \param[in] results The results computed using the specified examples.
+ * \param[in] flags The flags corresponding to the results.
+ * \param[in] threshold The threshold for which the information gain is computed.
+ */
+ float computeInformationGain(DataSet & data_set, std::vector<ExampleIndex> & examples, std::vector<LabelDataType> & label_data,
+ std::vector<float> & results, std::vector<unsigned char> & flags, const float threshold) const
+ {
+ const size_t num_of_examples = examples.size ();
+ const size_t num_of_branches = getNumOfBranches ();
+
+ // compute variance
+ std::vector < LabelDataType > sums (num_of_branches + 1, 0.f);
+ std::vector < LabelDataType > sqr_sums (num_of_branches + 1, 0.f);
+ std::vector < size_t > branch_element_count (num_of_branches + 1, 0.f);
+
+ for (size_t branch_index = 0; branch_index < num_of_branches; ++branch_index)
+ {
+ branch_element_count[branch_index] = 1;
+ ++branch_element_count[num_of_branches];
+ }
+
+ for (size_t example_index = 0; example_index < num_of_examples; ++example_index)
+ {
+ unsigned char branch_index;
+ computeBranchIndex (results[example_index], flags[example_index], threshold, branch_index);
+
+ LabelDataType label = label_data[example_index];
+
+ ++branch_element_count[branch_index];
+ ++branch_element_count[num_of_branches];
+
+ sums[branch_index] += label;
+ sums[num_of_branches] += label;
+ }
+
+ std::vector<float> hp (num_of_branches + 1, 0.f);
+ for (size_t branch_index = 0; branch_index < (num_of_branches + 1); ++branch_index)
+ {
+ float pf = sums[branch_index] / static_cast<float> (branch_element_count[branch_index]);
+ float pnf = (static_cast<LabelDataType>(branch_element_count[branch_index]) - sums[branch_index] + 1.f)
+ / static_cast<LabelDataType> (branch_element_count[branch_index]);
+ hp[branch_index] -= static_cast<float>(pf * log (pf) + pnf * log (pnf));
+ }
+
+ //use mean of the examples as purity
+ float purity = sums[num_of_branches] / static_cast<LabelDataType>(branch_element_count[num_of_branches]);
+ float tp = 0.8f;
+
+ if (purity >= tp)
+ {
+ //compute covariance matrices from translation offsets and angles for the whole set and children
+ //consider only positive examples...
+ std::vector < size_t > branch_element_count (num_of_branches + 1, 0);
+ std::vector < std::vector<ExampleIndex> > positive_examples;
+ positive_examples.resize (num_of_branches + 1);
+
+ size_t pos = 0;
+ for (size_t example_index = 0; example_index < num_of_examples; ++example_index)
+ {
+ unsigned char branch_index;
+ computeBranchIndex (results[example_index], flags[example_index], threshold, branch_index);
+
+ LabelDataType label = label_data[example_index];
+
+ if (label == 1 /*&& !flags[example_index]*/)
+ {
+ ++branch_element_count[branch_index];
+ ++branch_element_count[num_of_branches];
+
+ positive_examples[branch_index].push_back (examples[example_index]);
+ positive_examples[num_of_branches].push_back (examples[example_index]);
+ pos++;
+ }
+ }
+
+ //compute covariance from offsets and angles for all branchs
+ std::vector < Eigen::Matrix3d > offset_covariances;
+ std::vector < Eigen::Matrix3d > angle_covariances;
+
+ std::vector < Eigen::Vector3d > offset_centroids;
+ std::vector < Eigen::Vector3d > angle_centroids;
+
+ offset_covariances.resize (num_of_branches + 1);
+ angle_covariances.resize (num_of_branches + 1);
+ offset_centroids.resize (num_of_branches + 1);
+ angle_centroids.resize (num_of_branches + 1);
+
+ for (size_t branch_index = 0; branch_index < (num_of_branches + 1); ++branch_index)
+ {
+ computeMeanAndCovarianceOffset (data_set, positive_examples[branch_index], offset_covariances[branch_index],
+ offset_centroids[branch_index]);
+ computeMeanAndCovarianceAngles (data_set, positive_examples[branch_index], angle_covariances[branch_index],
+ angle_centroids[branch_index]);
+ }
+
+ //update information_gain
+ std::vector<float> hr (num_of_branches + 1, 0.f);
+ for (size_t branch_index = 0; branch_index < (num_of_branches + 1); ++branch_index)
+ {
+ hr[branch_index] = static_cast<float>(0.5f * log (std::pow (2 * M_PI, 3)
+ * offset_covariances[branch_index].determinant ())
+ + 0.5f * log (std::pow (2 * M_PI, 3)
+ * angle_covariances[branch_index].determinant ()));
+ }
+
+ for (size_t branch_index = 0; branch_index < (num_of_branches + 1); ++branch_index)
+ {
+ hp[branch_index] += std::max (sums[branch_index] / static_cast<float> (branch_element_count[branch_index]) - tp, 0.f) * hr[branch_index];
+ }
+ }
+
+ float information_gain = hp[num_of_branches + 1];
+ for (size_t branch_index = 0; branch_index < (num_of_branches); ++branch_index)
+ {
+ information_gain -= static_cast<float> (branch_element_count[branch_index]) / static_cast<float> (branch_element_count[num_of_branches])
+ * hp[branch_index];
+ }
+
+ return information_gain;
+ }
+
+ /** \brief Computes the branch indices for all supplied results.
+ * \param[in] results The results the branch indices will be computed for.
+ * \param[in] flags The flags corresponding to the specified results.
+ * \param[in] threshold The threshold used to compute the branch indices.
+ * \param[out] branch_indices The destination for the computed branch indices.
+ */
+ void computeBranchIndices(std::vector<float> & results, std::vector<unsigned char> & flags, const float threshold,
+ std::vector<unsigned char> & branch_indices) const
+ {
+ const size_t num_of_results = results.size ();
+
+ branch_indices.resize (num_of_results);
+ for (size_t result_index = 0; result_index < num_of_results; ++result_index)
+ {
+ unsigned char branch_index;
+ computeBranchIndex (results[result_index], flags[result_index], threshold, branch_index);
+ branch_indices[result_index] = branch_index;
+ }
+ }
+
+ /** \brief Computes the branch index for the specified result.
+ * \param[in] result The result the branch index will be computed for.
+ * \param[in] flag The flag corresponding to the specified result.
+ * \param[in] threshold The threshold used to compute the branch index.
+ * \param[out] branch_index The destination for the computed branch index.
+ */
+ inline void computeBranchIndex(const float result, const unsigned char flag, const float threshold, unsigned char & branch_index) const
+ {
+ branch_estimator_->computeBranchIndex (result, flag, threshold, branch_index);
+ }
+
+ /** \brief Computes and sets the statistics for a node.
+ * \param[in] data_set The data set which is evaluated.
+ * \param[in] examples The examples which define which parts of the data set are used for evaluation.
+ * \param[in] label_data The label_data corresponding to the examples.
+ * \param[out] node The destination node for the statistics.
+ */
+ void computeAndSetNodeStats(DataSet & data_set, std::vector<ExampleIndex> & examples, std::vector<LabelDataType> & label_data, NodeType & node) const
+ {
+ const size_t num_of_examples = examples.size ();
+
+ LabelDataType sum = 0.0f;
+ LabelDataType sqr_sum = 0.0f;
+ for (size_t example_index = 0; example_index < num_of_examples; ++example_index)
+ {
+ const LabelDataType label = label_data[example_index];
+
+ sum += label;
+ sqr_sum += label * label;
+ }
+
+ sum /= static_cast<float>(num_of_examples);
+ sqr_sum /= static_cast<float>(num_of_examples);
+
+ const float variance = sqr_sum - sum * sum;
+
+ node.value = sum;
+ node.variance = variance;
+
+ //set node stats regarding pose regression
+ std::vector < ExampleIndex > positive_examples;
+
+ for (size_t example_index = 0; example_index < num_of_examples; ++example_index)
+ {
+ LabelDataType label = label_data[example_index];
+
+ if (label == 1)
+ positive_examples.push_back (examples[example_index]);
+
+ }
+
+ //compute covariance from offsets and angles
+ computeMeanAndCovarianceOffset (data_set, positive_examples, node.covariance_trans_, node.trans_mean_);
+ computeMeanAndCovarianceAngles (data_set, positive_examples, node.covariance_rot_, node.rot_mean_);
+ }
+
+ /** \brief Generates code for branch index computation.
+ * \param[out] stream The destination for the generated code.
+ */
+ // param[in] node The node for which code is generated.
+ void generateCodeForBranchIndexComputation(NodeType & /*node*/, std::ostream & stream) const
+ {
+ stream << "ERROR: RegressionVarianceStatsEstimator does not implement generateCodeForBranchIndex(...)";
+ }
+
+ /** \brief Generates code for label output.
+ * \param[out] stream The destination for the generated code.
+ */
+ // param[in] node The node for which code is generated.
+ void generateCodeForOutput(NodeType & /*node*/, std::ostream & stream) const
+ {
+ stream << "ERROR: RegressionVarianceStatsEstimator does not implement generateCodeForBranchIndex(...)";
+ }
+
+ private:
+ /** \brief The branch estimator. */
+ pcl::BranchEstimator * branch_estimator_;
+ };
+ }
+}
+
+#endif /* PCL_RF_FACE_UTILS_H_ */
#include <pcl/pcl_macros.h>
#include <pcl/recognition/hv/hypotheses_verification.h>
#include <pcl/common/common.h>
-#include "metslib/mets.hh"
+#include <pcl/recognition/3rdparty/metslib/mets.hh>
#include <pcl/features/normal_3d.h>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
return (false);
}
- std::vector<Eigen::Vector3d> scene_votes (n_matches);
+ std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > scene_votes (n_matches);
Eigen::Vector3d d_min, d_max, bin_size;
d_min.setConstant (std::numeric_limits<double>::max ());
double SIGMA_DIST = in_sigma;// rule of thumb: 10% of the object radius
const double FINAL_EPS = SIGMA_DIST / 100;// another heuristic
- std::vector<Eigen::Vector3f> peaks (NUM_INIT_PTS);
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > peaks (NUM_INIT_PTS);
std::vector<double> peak_densities (NUM_INIT_PTS);
double max_density = -1.0;
for (int i = 0; i < NUM_INIT_PTS; i++)
Eigen::MatrixXf centers (number_of_clusters, feature_dimension);
Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension);
std::vector<int> counters (number_of_clusters);
- std::vector<Eigen::Vector2f> boxes (feature_dimension);
+ std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > boxes (feature_dimension);
Eigen::Vector2f* box = &boxes[0];
double best_compactness = std::numeric_limits<double>::max ();
//////////////////////////////////////////////////////////////////////////////////////////////
template <int FeatureSize, typename PointT, typename NormalT> void
-pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::generateRandomCenter (const std::vector<Eigen::Vector2f>& boxes,
+pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::generateRandomCenter (const std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >& boxes,
Eigen::VectorXf& center)
{
size_t dimension = boxes.size ();
typename LineRGBD<PointXYZT, PointRGBT>::Detection & detection = detections_[detection_index];
// find depth with most valid points
- const size_t start_x = detection.region.x;
- const size_t start_y = detection.region.y;
- const size_t end_x = start_x + detection.region.width;
- const size_t end_y = start_y + detection.region.height;
+ const size_t start_x = std::max (detection.region.x, 0);
+ const size_t start_y = std::max (detection.region.y, 0);
+ const size_t end_x = std::min (static_cast<size_t> (detection.region.x + detection.region.width),
+ static_cast<size_t> (cloud_xyz_->width));
+ const size_t end_y = std::min (static_cast<size_t> (detection.region.y + detection.region.height),
+ static_cast<size_t> (cloud_xyz_->height));
float min_depth = std::numeric_limits<float>::max ();
* \param[out] center it will the contain generated center
*/
void
- generateRandomCenter (const std::vector<Eigen::Vector2f>& boxes, Eigen::VectorXf& center);
+ generateRandomCenter (const std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >& boxes, Eigen::VectorXf& center);
/** \brief Computes the square distance beetween two vectors.
* \param[in] vec_1 first vector
--- /dev/null
+#include "pcl/recognition/face_detection/face_detector_data_provider.h"
+#include "pcl/recognition/face_detection/face_common.h"
+#include <pcl/common/time.h>
+#include <pcl/features/integral_image_normal.h>
+#include <pcl/io/pcd_io.h>
+
+//Uncomment the following lines and set PCL_FACE_DETECTION_VIS_TRAINING_FDDP to 1
+//to visualize the training process and change the CMakeLists.txt accordingly.
+//#include <pcl/visualization/pcl_visualizer.h>
+//#define PCL_FACE_DETECTION_VIS_TRAINING_FDDP 1
+//TODO: This is not very good as it forces recognition to depend on pcl_visualization
+//much better solution would be pass a visualization function from the user code to make it transparent
+//to the training process
+
+namespace pcl
+{
+ namespace face_detection
+ {
+ inline
+ void showBining(int num_pitch, float res_pitch, int min_pitch, int num_yaw, float res_yaw, int min_yaw, std::vector<std::vector<int> > & yaw_pitch_bins)
+ {
+ std::cout << "\t\t";
+ for (int j = 0; j < num_pitch; j++)
+ {
+ std::cout << pcl_round (static_cast<float>(min_pitch) + res_pitch * static_cast<float>(j)) << "\t";
+ }
+
+ std::cout << std::endl;
+
+ for (int i = 0; i < num_yaw; i++)
+ {
+ std::cout << pcl_round (static_cast<float>(min_yaw) + res_yaw * static_cast<float>(i)) << " => \t\t";
+ for (int j = 0; j < num_pitch; j++)
+ {
+ //std::cout << std::setprecision(3) << yaw_pitch_bins[i][j] / static_cast<float>(max_elems) << "\t";
+ std::cout << yaw_pitch_bins[i][j] << "\t";
+ }
+
+ std::cout << std::endl;
+ }
+ }
+ }
+}
+
+template<class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+void pcl::face_detection::FaceDetectorDataProvider<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::initialize(std::string & data_dir)
+{
+ std::string start = "";
+ std::string ext = std::string ("pcd");
+ bf::path dir = data_dir;
+
+ std::vector < std::string > files;
+ getFilesInDirectory (dir, start, files, ext);
+
+ //apart from loading the file names, we will do some bining regarding pitch and yaw
+ std::vector < std::vector<int> > yaw_pitch_bins;
+ std::vector < std::vector<std::vector<std::string> > > image_files_per_bin;
+
+ float res_yaw = 15.f;
+ float res_pitch = res_yaw;
+ int min_yaw = -75;
+ int min_pitch = -60;
+
+ int num_yaw = static_cast<int>((std::abs (min_yaw) * 2) / static_cast<int>(res_yaw + 1.f));
+ int num_pitch = static_cast<int>((std::abs (min_pitch) * 2) / static_cast<int>(res_pitch + 1.f));
+
+ yaw_pitch_bins.resize (num_yaw);
+ image_files_per_bin.resize (num_yaw);
+ for (int i = 0; i < num_yaw; i++)
+ {
+ yaw_pitch_bins[i].resize (num_pitch);
+ image_files_per_bin[i].resize (num_pitch);
+ for (int j = 0; j < num_pitch; j++)
+ {
+ yaw_pitch_bins[i][j] = 0;
+ }
+ }
+
+ for (size_t i = 0; i < files.size (); i++)
+ {
+ std::stringstream filestream;
+ filestream << data_dir << "/" << files[i];
+ std::string file = filestream.str ();
+
+ std::string pose_file (files[i]);
+ boost::replace_all (pose_file, ".pcd", "_pose.txt");
+
+ Eigen::Matrix4f pose_mat;
+ pose_mat.setIdentity (4, 4);
+
+ std::stringstream filestream_pose;
+ filestream_pose << data_dir << "/" << pose_file;
+ pose_file = filestream_pose.str ();
+
+ bool result = readMatrixFromFile (pose_file, pose_mat);
+ if (result)
+ {
+ Eigen::Vector3f ea = pose_mat.block<3, 3> (0, 0).eulerAngles (0, 1, 2);
+ ea *= 57.2957795f; //transform it to degrees to do the binning
+ int y = static_cast<int>(pcl_round ((ea[0] + static_cast<float>(std::abs (min_yaw))) / res_yaw));
+ int p = static_cast<int>(pcl_round ((ea[1] + static_cast<float>(std::abs (min_pitch))) / res_pitch));
+
+ if (y < 0)
+ y = 0;
+ if (p < 0)
+ p = 0;
+ if (p >= num_pitch)
+ p = num_pitch - 1;
+ if (y >= num_yaw)
+ y = num_yaw - 1;
+
+ assert (y >= 0 && y < num_yaw);
+ assert (p >= 0 && p < num_pitch);
+
+ yaw_pitch_bins[y][p]++;
+
+ image_files_per_bin[y][p].push_back (file);
+ }
+ }
+
+ pcl::face_detection::showBining (num_pitch, res_pitch, min_pitch, num_yaw, res_yaw, min_yaw, yaw_pitch_bins);
+
+ int max_elems = 0;
+ int total_elems = 0;
+
+ for (int i = 0; i < num_yaw; i++)
+ {
+ for (int j = 0; j < num_pitch; j++)
+ {
+ total_elems += yaw_pitch_bins[i][j];
+ if (yaw_pitch_bins[i][j] > max_elems)
+ max_elems = yaw_pitch_bins[i][j];
+ }
+ }
+
+ float average = static_cast<float> (total_elems) / (static_cast<float> (num_pitch + num_yaw));
+ std::cout << "The average number of image per bin is:" << average << std::endl;
+
+ std::cout << "Total number of images in the dataset:" << total_elems << std::endl;
+ //reduce unbalance from dataset by capping the number of images per bin, keeping at least a certain min
+ if (min_images_per_bin_ != -1)
+ {
+ std::cout << "Reducing unbalance of the dataset." << std::endl;
+ for (int i = 0; i < num_yaw; i++)
+ {
+ for (int j = 0; j < num_pitch; j++)
+ {
+ if (yaw_pitch_bins[i][j] >= min_images_per_bin_)
+ {
+ std::random_shuffle (image_files_per_bin[i][j].begin (), image_files_per_bin[i][j].end ());
+ image_files_per_bin[i][j].resize (min_images_per_bin_);
+ yaw_pitch_bins[i][j] = min_images_per_bin_;
+ }
+
+ for (size_t ii = 0; ii < image_files_per_bin[i][j].size (); ii++)
+ {
+ image_files_.push_back (image_files_per_bin[i][j][ii]);
+ }
+ }
+ }
+ }
+
+ pcl::face_detection::showBining (num_pitch, res_pitch, min_pitch, num_yaw, res_yaw, min_yaw, yaw_pitch_bins);
+ std::cout << "Total number of images in the dataset:" << image_files_.size () << std::endl;
+}
+
+//shuffle file and get the first num_images_ as requested by a tree
+//extract positive and negative samples
+//create training examples and labels
+
+template<class FeatureType, class DataSet, class LabelType, class ExampleIndex, class NodeType>
+void pcl::face_detection::FaceDetectorDataProvider<FeatureType, DataSet, LabelType, ExampleIndex, NodeType>::getDatasetAndLabels(DataSet & data_set,
+ std::vector<LabelType> & label_data, std::vector<ExampleIndex> & examples)
+{
+ srand (static_cast<unsigned int>(time (NULL)));
+ std::random_shuffle (image_files_.begin (), image_files_.end ());
+ std::vector < std::string > files;
+ files = image_files_;
+ files.resize (std::min (num_images_, static_cast<int> (files.size ())));
+
+ std::vector < TrainingExample > training_examples;
+ std::vector<float> labels;
+
+ int total_neg, total_pos;
+ total_neg = total_pos = 0;
+
+#if PCL_FACE_DETECTION_VIS_TRAINING_FDDP == 1
+ pcl::visualization::PCLVisualizer vis("training");
+#endif
+
+ for (size_t j = 0; j < files.size (); j++)
+ {
+
+#if PCL_FACE_DETECTION_VIS_TRAINING_FDDP == 1
+ vis.removeAllPointClouds();
+ vis.removeAllShapes();
+#endif
+
+ if ((j % 50) == 0)
+ {
+ std::cout << "Loading image..." << j << std::endl;
+ }
+ //1. Load clouds with and without labels
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr loaded_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+
+ pcl::io::loadPCDFile (files[j], *loaded_cloud);
+
+ pcl::PointCloud<pcl::PointXYZL>::Ptr cloud_labels (new pcl::PointCloud<pcl::PointXYZL>);
+ pcl::PointCloud<pcl::PointXYZL>::Ptr loaded_cloud_labels (new pcl::PointCloud<pcl::PointXYZL>);
+ pcl::io::loadPCDFile (files[j], *loaded_cloud_labels);
+
+ //crop images to remove as many NaNs as possible and reduce the memory footprint
+ {
+ size_t min_col, min_row;
+ size_t max_col, max_row;
+ min_col = min_row = std::numeric_limits<size_t>::max ();
+ max_col = max_row = 0;
+
+ for (size_t col = 0; col < loaded_cloud->width; col++)
+ {
+ for (size_t row = 0; row < loaded_cloud->height; row++)
+ {
+ if (pcl::isFinite (loaded_cloud->at (col, row)))
+ {
+ if (row < min_row)
+ min_row = row;
+
+ if (row > max_row)
+ max_row = row;
+
+ if (col < min_col)
+ min_col = col;
+
+ if (col > max_col)
+ max_col = col;
+ }
+ }
+ }
+
+ //std::cout << min_col << " - " << max_col << std::endl;
+ //std::cout << min_row << " - " << max_row << std::endl;
+
+ cropCloud<pcl::PointXYZ> (min_col, max_col, min_row, max_row, *loaded_cloud, *cloud);
+ cropCloud<pcl::PointXYZL> (min_col, max_col, min_row, max_row, *loaded_cloud_labels, *cloud_labels);
+
+ /*pcl::visualization::PCLVisualizer vis ("training");
+ vis.addPointCloud(loaded_cloud);
+ vis.spin();*/
+ }
+
+ //Compute integral image over depth
+ boost::shared_ptr < pcl::IntegralImage2D<float, 1> > integral_image_depth;
+ integral_image_depth.reset (new pcl::IntegralImage2D<float, 1> (false));
+
+ int element_stride = sizeof(pcl::PointXYZ) / sizeof(float);
+ int row_stride = element_stride * cloud->width;
+ const float *data = reinterpret_cast<const float*> (&cloud->points[0]);
+ integral_image_depth->setInput (data + 2, cloud->width, cloud->height, element_stride, row_stride);
+
+ //Compute normals and normal integral images
+ pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+
+ if (USE_NORMALS_)
+ {
+ typedef typename pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> NormalEstimator_;
+ NormalEstimator_ n3d;
+ n3d.setNormalEstimationMethod (n3d.COVARIANCE_MATRIX);
+ n3d.setInputCloud (cloud);
+ n3d.setRadiusSearch (0.02);
+ n3d.setKSearch (0);
+ {
+ pcl::ScopeTime t ("compute normals...");
+ n3d.compute (*normals);
+ }
+ }
+
+ int element_stride_normal = sizeof(pcl::Normal) / sizeof(float);
+ int row_stride_normal = element_stride_normal * normals->width;
+ boost::shared_ptr < pcl::IntegralImage2D<float, 1> > integral_image_normal_x;
+ boost::shared_ptr < pcl::IntegralImage2D<float, 1> > integral_image_normal_y;
+ boost::shared_ptr < pcl::IntegralImage2D<float, 1> > integral_image_normal_z;
+
+ if (USE_NORMALS_)
+ {
+ integral_image_normal_x.reset (new pcl::IntegralImage2D<float, 1> (false));
+ const float *data_nx = reinterpret_cast<const float*> (&normals->points[0]);
+ integral_image_normal_x->setInput (data_nx, normals->width, normals->height, element_stride_normal, row_stride_normal);
+
+ integral_image_normal_y.reset (new pcl::IntegralImage2D<float, 1> (false));
+ const float *data_ny = reinterpret_cast<const float*> (&normals->points[0]);
+ integral_image_normal_y->setInput (data_ny + 1, normals->width, normals->height, element_stride_normal, row_stride_normal);
+
+ integral_image_normal_z.reset (new pcl::IntegralImage2D<float, 1> (false));
+ const float *data_nz = reinterpret_cast<const float*> (&normals->points[0]);
+ integral_image_normal_z->setInput (data_nz + 2, normals->width, normals->height, element_stride_normal, row_stride_normal);
+ }
+
+ //Using cloud labels estimate a 2D window from where to extract positive samples
+ //Rest can be used to extract negative samples
+ size_t min_col, min_row;
+ size_t max_col, max_row;
+ min_col = min_row = std::numeric_limits<size_t>::max ();
+ max_col = max_row = 0;
+
+ //std::cout << cloud_labels->width << " " << cloud_labels->height << std::endl;
+
+ for (size_t col = 0; col < cloud_labels->width; col++)
+ {
+ for (size_t row = 0; row < cloud_labels->height; row++)
+ {
+ if (cloud_labels->at (col, row).label == 1)
+ {
+ if (row < min_row)
+ min_row = row;
+
+ if (row > max_row)
+ max_row = row;
+
+ if (col < min_col)
+ min_col = col;
+
+ if (col > max_col)
+ max_col = col;
+ }
+ }
+ }
+
+#if PCL_FACE_DETECTION_VIS_TRAINING_FDDP == 1
+ pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_intensity(new pcl::PointCloud<pcl::PointXYZI>);
+ cloud_intensity->width = cloud->width;
+ cloud_intensity->height = cloud->height;
+ cloud_intensity->points.resize(cloud->points.size());
+ cloud_intensity->is_dense = cloud->is_dense;
+
+ for (int jjj = 0; jjj < static_cast<int>(cloud->points.size()); jjj++)
+ {
+ cloud_intensity->points[jjj].getVector4fMap() = cloud->points[jjj].getVector4fMap();
+ cloud_intensity->points[jjj].intensity = 0.f;
+ int row, col;
+ col = jjj % cloud->width;
+ row = jjj / cloud->width;
+ //std::cout << row << " " << col << std::endl;
+ if (check_inside(col, row, min_col, max_col, min_row, max_row))
+ {
+ cloud_intensity->points[jjj].intensity = 1.f;
+ }
+ }
+
+ pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler(cloud_intensity, "intensity");
+ vis.addPointCloud(cloud_intensity, handler, "intensity_cloud");
+#endif
+
+ std::string pose_file (files[j]);
+ boost::replace_all (pose_file, ".pcd", "_pose.txt");
+
+ Eigen::Matrix4f pose_mat;
+ pose_mat.setIdentity (4, 4);
+ readMatrixFromFile (pose_file, pose_mat);
+
+ Eigen::Vector3f ea = pose_mat.block<3, 3> (0, 0).eulerAngles (0, 1, 2);
+ Eigen::Vector3f trans_vector = Eigen::Vector3f (pose_mat (0, 3), pose_mat (1, 3), pose_mat (2, 3));
+
+ pcl::PointXYZ center_point;
+ center_point.x = trans_vector[0];
+ center_point.y = trans_vector[1];
+ center_point.z = trans_vector[2];
+
+ int N_patches = patches_per_image_;
+ int pos_extracted = 0;
+ int neg_extracted = 0;
+ int w_size_2 = static_cast<int> (w_size_ / 2);
+
+ //************************************************
+ //2nd training style, fanelli's journal description
+ //************************************************
+ {
+
+ typedef std::pair<int, int> pixelpair;
+ std::vector < pixelpair > negative_p, positive_p;
+ //get negative and positive indices to sample from
+ for (int col = 0; col < (static_cast<int> (cloud_labels->width) - w_size_); col++)
+ {
+ for (int row = 0; row < (static_cast<int> (cloud_labels->height) - w_size_); row++)
+ {
+ if (!pcl::isFinite (cloud->at (col + w_size_2, row + w_size_2)))
+ continue;
+
+ //reject patches with more than percent invalid values
+ float percent = 0.5f;
+ if (static_cast<float>(integral_image_depth->getFiniteElementsCount (col, row, w_size_, w_size_)) < (percent * static_cast<float>(w_size_ * w_size_)))
+ continue;
+
+ pixelpair pp = std::make_pair (col, row);
+ if (cloud_labels->at (col + w_size_2, row + w_size_2).label == 1)
+ positive_p.push_back (pp);
+ else
+ negative_p.push_back (pp);
+ }
+ }
+
+ //shuffle and resize
+ std::random_shuffle (positive_p.begin (), positive_p.end ());
+ std::random_shuffle (negative_p.begin (), negative_p.end ());
+ positive_p.resize (N_patches);
+ negative_p.resize (N_patches);
+
+ //extract positive patch
+ for (size_t p = 0; p < positive_p.size (); p++)
+ {
+ int col, row;
+ col = positive_p[p].first;
+ row = positive_p[p].second;
+
+ pcl::PointXYZ patch_center_point;
+ patch_center_point.x = cloud->at (col + w_size_2, row + w_size_2).x;
+ patch_center_point.y = cloud->at (col + w_size_2, row + w_size_2).y;
+ patch_center_point.z = cloud->at (col + w_size_2, row + w_size_2).z;
+
+ TrainingExample te;
+ te.iimages_.push_back (integral_image_depth);
+ if (USE_NORMALS_)
+ {
+ te.iimages_.push_back (integral_image_normal_x);
+ te.iimages_.push_back (integral_image_normal_y);
+ te.iimages_.push_back (integral_image_normal_z);
+ }
+
+ te.row_ = row;
+ te.col_ = col;
+ te.wsize_ = w_size_;
+
+ te.trans_ = center_point.getVector3fMap () - patch_center_point.getVector3fMap ();
+ te.trans_ *= 1000.f; //transform it to milimiters
+ te.rot_ = ea;
+ te.rot_ *= 57.2957795f; //transform it to degrees
+
+ labels.push_back (1);
+ pos_extracted++;
+ total_pos++;
+
+ training_examples.push_back (te);
+#if PCL_FACE_DETECTION_VIS_TRAINING_FDDP == 1
+ pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_intensity2(new pcl::PointCloud<pcl::PointXYZI>(*cloud_intensity));
+ for (int jjj = col; jjj < (col + w_size_); jjj++)
+ {
+ for (int iii = row; iii < (row + w_size_); iii++)
+ {
+ cloud_intensity2->at(jjj, iii).intensity = 2.f;
+ }
+ }
+
+ vis.removeAllPointClouds();
+
+ pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler(cloud_intensity2, "intensity");
+ vis.addPointCloud(cloud_intensity2, handler, "cloud");
+ vis.spinOnce();
+ vis.spin();
+ sleep(1);
+#endif
+
+ }
+
+#if PCL_FACE_DETECTION_VIS_TRAINING_FDDP == 1
+ std::cout << "Going to extract negative patches..." << std::endl;
+ sleep(2);
+#endif
+
+ for (size_t p = 0; p < negative_p.size (); p++)
+ {
+ int col, row;
+ col = negative_p[p].first;
+ row = negative_p[p].second;
+
+ TrainingExample te;
+ te.iimages_.push_back (integral_image_depth);
+ if (USE_NORMALS_)
+ {
+ te.iimages_.push_back (integral_image_normal_x);
+ te.iimages_.push_back (integral_image_normal_y);
+ te.iimages_.push_back (integral_image_normal_z);
+ }
+
+ te.row_ = row;
+ te.col_ = col;
+ te.wsize_ = w_size_;
+ labels.push_back (0);
+ neg_extracted++;
+ total_neg++;
+
+ training_examples.push_back (te);
+#if PCL_FACE_DETECTION_VIS_TRAINING_FDDP == 1
+ pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_intensity2(new pcl::PointCloud<pcl::PointXYZI>(*cloud_intensity));
+ for (int jjj = col; jjj < (col + w_size_); jjj++)
+ {
+ for (int iii = row; iii < (row + w_size_); iii++)
+ {
+ cloud_intensity2->at(jjj, iii).intensity = 2.f;
+ }
+ }
+
+ vis.removeAllPointClouds();
+
+ pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler(cloud_intensity2, "intensity");
+ vis.addPointCloud(cloud_intensity2, handler, "cloud");
+ vis.spinOnce();
+ vis.spin();
+ sleep(1);
+#endif
+ }
+
+ if (neg_extracted != N_patches)
+ {
+ std::cout << "Extracted " << neg_extracted << " negative patches" << std::endl;
+ std::cout << files[j] << std::endl;
+ }
+
+ if (pos_extracted != N_patches)
+ {
+ std::cout << "Extracted " << pos_extracted << " positive patches" << std::endl;
+ std::cout << files[j] << std::endl;
+ }
+ }
+ }
+
+ std::cout << training_examples.size () << " " << labels.size () << " " << total_neg << " " << total_pos << std::endl;
+
+ //train random forest and make persistent
+ std::vector<int> example_indices;
+ for (size_t i = 0; i < labels.size (); i++)
+ example_indices.push_back (static_cast<int> (i));
+
+ label_data = labels;
+ data_set = training_examples;
+ examples = example_indices;
+}
+
+template class pcl::face_detection::FaceDetectorDataProvider<pcl::face_detection::FeatureType, std::vector<pcl::face_detection::TrainingExample>, float, int,
+ pcl::face_detection::RFTreeNode<pcl::face_detection::FeatureType> >;
--- /dev/null
+/*
+ * rf_face_detector_trainer.cpp
+ *
+ * Created on: 22 Sep 2012
+ * Author: ari
+ */
+
+#include "pcl/recognition/face_detection/rf_face_detector_trainer.h"
+#include "pcl/recognition/face_detection/face_common.h"
+#include "pcl/io/pcd_io.h"
+#include "pcl/ml/dt/decision_tree_trainer.h"
+#include "pcl/ml/dt/decision_tree_evaluator.h"
+#include "pcl/ml/dt/decision_forest_trainer.h"
+#include "pcl/ml/dt/decision_forest_evaluator.h"
+#include "pcl/filters/passthrough.h"
+#include <pcl/features/integral_image_normal.h>
+#include <pcl/registration/icp.h>
+#include <pcl/registration/transformation_estimation_point_to_plane_lls.h>
+#include <pcl/registration/correspondence_estimation_normal_shooting.h>
+#include <pcl/registration/correspondence_rejection_sample_consensus.h>
+#include "pcl/filters/voxel_grid.h"
+#include <pcl/recognition/hv/hv_papazov.h>
+#include <pcl/features/normal_3d.h>
+
+void pcl::RFFaceDetectorTrainer::trainWithDataProvider()
+{
+
+ face_detection::FeatureHandlerDepthAverage<face_detection::FeatureType, std::vector<face_detection::TrainingExample>, int> fhda;
+ fhda.setWSize (w_size_);
+ fhda.setMaxPatchSize (max_patch_size_);
+ fhda.setNumChannels (1);
+ if (use_normals_)
+ fhda.setNumChannels (4);
+
+ pcl::TernaryTreeMissingDataBranchEstimator * btt = new pcl::TernaryTreeMissingDataBranchEstimator ();
+ pcl::face_detection::PoseClassRegressionVarianceStatsEstimator<float, NodeType, std::vector<face_detection::TrainingExample>, int> rse (btt);
+
+ std::vector<float> thresholds_;
+ thresholds_.push_back (0.5f);
+
+ pcl::DecisionForestTrainer<face_detection::FeatureType, std::vector<face_detection::TrainingExample>, float, int, NodeType> dft;
+ dft.setMaxTreeDepth (15);
+ dft.setNumOfFeatures (nfeatures_);
+ dft.setNumOfThresholds (1);
+ dft.setNumberOfTreesToTrain (ntrees_);
+ dft.setMinExamplesForSplit (20);
+ dft.setFeatureHandler (fhda);
+ dft.setStatsEstimator (rse);
+ dft.setRandomFeaturesAtSplitNode (true);
+ dft.setThresholds (thresholds_);
+
+ boost::shared_ptr < face_detection::FaceDetectorDataProvider<face_detection::FeatureType, std::vector<face_detection::TrainingExample>, float, int, NodeType>
+ > dtdp;
+ dtdp.reset (new face_detection::FaceDetectorDataProvider<face_detection::FeatureType, std::vector<face_detection::TrainingExample>, float, int, NodeType>);
+ dtdp->setUseNormals (use_normals_);
+ dtdp->setWSize (w_size_);
+ dtdp->setNumImages (num_images_);
+ dtdp->setMinImagesPerBin (300);
+
+ dtdp->initialize (directory_);
+
+ boost::shared_ptr < pcl::DecisionTreeTrainerDataProvider<face_detection::FeatureType, std::vector<face_detection::TrainingExample>, float, int, NodeType>
+ > cast_dtdp;
+ cast_dtdp = boost::dynamic_pointer_cast
+ < pcl::DecisionTreeTrainerDataProvider<face_detection::FeatureType, std::vector<face_detection::TrainingExample>, float, int, NodeType> > (dtdp);
+ dft.setDecisionTreeDataProvider (cast_dtdp);
+
+ pcl::DecisionForest<NodeType> forest;
+ dft.train (forest);
+
+ PCL_INFO("Finished training forest...\n");
+
+ std::filebuf fb;
+ fb.open (forest_filename_.c_str (), std::ios::out);
+ std::ostream os (&fb);
+ forest.serialize (os);
+ fb.close ();
+}
+
+void pcl::RFFaceDetectorTrainer::faceVotesClustering()
+{
+ float HEAD_DIAMETER_SQ = HEAD_ST_DIAMETER_ * HEAD_ST_DIAMETER_;
+ float large_radius = HEAD_DIAMETER_SQ / (larger_radius_ratio_ * larger_radius_ratio_);
+
+ std::vector < Eigen::Vector3f > clusters_mean;
+ std::vector < std::vector<int> > votes_indices;
+
+ for (size_t i = 0; i < head_center_votes_.size (); i++)
+ {
+ Eigen::Vector3f center_vote = head_center_votes_[i];
+ std::vector<bool> valid_in_cluster (clusters_mean.size (), false);
+ bool found = false;
+ for (size_t j = 0; j < clusters_mean.size () /*&& !found*/; j++)
+ {
+ float sq_norm = (clusters_mean[j] - center_vote).squaredNorm ();
+ if (sq_norm < large_radius)
+ {
+ //found one cluster, update cluster mean and append index
+ valid_in_cluster[j] = true;
+ found = true;
+ }
+ }
+
+ //no cluster found, create new cluster
+ if (!found)
+ {
+ std::vector < int > ind;
+ ind.push_back (static_cast<int>(i));
+ votes_indices.push_back (ind);
+
+ std::vector < Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > votes_in_cluster;
+ votes_in_cluster.push_back (center_vote);
+ head_center_original_votes_clustered_.push_back (votes_in_cluster);
+
+ clusters_mean.push_back (center_vote);
+ continue;
+ }
+
+ //get the largest biggest cluster and put if there
+ int idx = -1;
+ size_t biggest_num = 0;
+ for (size_t j = 0; j < clusters_mean.size () /*&& !found*/; j++)
+ {
+ if ((votes_indices[j].size () > biggest_num) && (valid_in_cluster[j]))
+ {
+ idx = static_cast<int>(j);
+ biggest_num = votes_indices[j].size ();
+ }
+ }
+
+ clusters_mean[idx] = (clusters_mean[idx] * (static_cast<float> (votes_indices[idx].size ())) + center_vote)
+ / (static_cast<float> (votes_indices[idx].size ()) + 1.f);
+ votes_indices[idx].push_back (static_cast<int>(i));
+ head_center_original_votes_clustered_[idx].push_back (center_vote);
+ }
+
+ //mean shift
+ //float SMALL_HEAD_RADIUS = HEAD_ST_DIAMETER_ / 6.f;
+ float SMALL_HEAD_RADIUS_SQ = HEAD_ST_DIAMETER_ * HEAD_ST_DIAMETER_ / 36.f;
+ int msi = 10;
+ std::cout << "Number of clusters:" << clusters_mean.size () << " votes:" << head_center_votes_.size () << std::endl;
+
+ int valid = 0;
+ for (size_t i = 0; i < clusters_mean.size (); i++)
+ {
+ //ignore this cluster
+ if (votes_indices[i].size () < min_votes_size_)
+ continue;
+
+ std::vector < int > new_cluster;
+
+ for (int it = 0; it < msi; it++)
+ {
+ Eigen::Vector3f mean;
+ mean.setZero ();
+ int good_votes = 0;
+ new_cluster.clear ();
+ for (size_t j = 0; j < votes_indices[i].size (); j++)
+ {
+ Eigen::Vector3f center_vote = head_center_votes_[votes_indices[i][j]];
+ float sq_norm = (clusters_mean[i] - center_vote).squaredNorm ();
+ if (sq_norm < SMALL_HEAD_RADIUS_SQ)
+ {
+ mean += center_vote;
+ new_cluster.push_back (votes_indices[i][j]);
+ good_votes++;
+ }
+ }
+
+ mean = mean / static_cast<float> (good_votes);
+ clusters_mean[i] = mean;
+ }
+
+ clusters_mean[valid] = clusters_mean[i];
+ votes_indices[valid] = new_cluster;
+ valid++;
+ }
+
+ clusters_mean.resize (valid);
+ votes_indices.resize (valid);
+
+ std::cout << "Valid:" << valid << std::endl;
+
+ head_clusters_centers_.clear ();
+ head_clusters_rotation_.clear ();
+ head_center_votes_clustered_.resize (clusters_mean.size ());
+
+ for (size_t i = 0; i < clusters_mean.size (); i++)
+ {
+ if (votes_indices[i].size () > min_votes_size_)
+ {
+ //compute rotation using the first less uncertain votes
+ std::vector < std::pair<int, float> > uncertainty;
+ for (size_t j = 0; j < votes_indices[i].size (); j++)
+ {
+ uncertainty.push_back (std::make_pair (votes_indices[i][j], uncertainties_[votes_indices[i][j]]));
+ }
+
+ std::sort (uncertainty.begin (), uncertainty.end (), boost::bind (&std::pair<int, float>::second, _1) < boost::bind (&std::pair<int, float>::second, _2));
+
+ Eigen::Vector3f rot;
+ rot.setZero ();
+ int num = std::min (used_for_pose_, static_cast<int> (uncertainty.size ()));
+ for (int j = 0; j < num; j++)
+ {
+ rot += angle_votes_[uncertainty[j].first];
+ }
+
+ rot = rot / static_cast<float> (num);
+
+ Eigen::Vector3f pos;
+ pos.setZero ();
+ for (int j = 0; j < num; j++)
+ pos += head_center_votes_[uncertainty[j].first];
+
+ pos = pos / static_cast<float> (num);
+
+ head_clusters_centers_.push_back (pos); //clusters_mean[i]
+ head_clusters_rotation_.push_back (rot);
+
+ for (size_t j = 0; j < votes_indices[i].size (); j++)
+ {
+ head_center_votes_clustered_[i].push_back (head_center_votes_[votes_indices[i][j]]);
+ }
+ }
+ }
+
+ std::cout << "Number of heads:" << head_clusters_centers_.size () << std::endl;
+}
+
+void pcl::RFFaceDetectorTrainer::setModelPath(std::string & model)
+{
+ model_path_ = model;
+ pcl::PointCloud<pcl::PointXYZ>::Ptr model_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
+ pcl::io::loadPCDFile (model_path_, *model_cloud);
+
+ model_original_.reset (new pcl::PointCloud<pcl::PointXYZ> ());
+
+ {
+ pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_icp;
+ voxel_grid_icp.setInputCloud (model_cloud);
+ voxel_grid_icp.setLeafSize (res_, res_, res_);
+ voxel_grid_icp.filter (*model_original_);
+
+ pcl::PassThrough<pcl::PointXYZ> pass_;
+ pass_.setFilterLimits (-1.f, 0.03f);
+ pass_.setFilterFieldName ("z");
+ pass_.setInputCloud (model_original_);
+ pass_.filter (*model_original_);
+
+ pass_.setFilterLimits (-0.1f, 0.07f);
+ pass_.setFilterFieldName ("y");
+ pass_.setInputCloud (model_original_);
+ pass_.filter (*model_original_);
+ }
+}
+
+void pcl::RFFaceDetectorTrainer::detectFaces()
+{
+ //clear stuff from last round
+ head_center_votes_.clear ();
+ head_center_original_votes_clustered_.clear ();
+ head_center_votes_clustered_.clear ();
+ angle_votes_.clear ();
+ uncertainties_.clear ();
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PassThrough<pcl::PointXYZ> pass_;
+ pass_.setFilterLimits (0.f, 1.25f);
+ pass_.setFilterFieldName ("z");
+ pass_.setInputCloud (input_);
+ pass_.setKeepOrganized (true);
+ pass_.filter (*cloud);
+
+ //compute depth integral image
+ boost::shared_ptr<pcl::IntegralImage2D<float, 1> > integral_image_depth;
+ integral_image_depth.reset (new pcl::IntegralImage2D<float, 1> (false));
+
+ int element_stride = sizeof(pcl::PointXYZ) / sizeof(float);
+ int row_stride = element_stride * cloud->width;
+ const float *data = reinterpret_cast<const float*> (&cloud->points[0]);
+ integral_image_depth->setInput (data + 2, cloud->width, cloud->height, element_stride, row_stride);
+
+ //Compute normals and normal integral images
+ pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+
+ if (use_normals_)
+ {
+ typedef pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> NormalEstimator_;
+ NormalEstimator_ n3d;
+ n3d.setNormalEstimationMethod (n3d.COVARIANCE_MATRIX);
+ n3d.setInputCloud (cloud);
+ n3d.setRadiusSearch (0.02);
+ n3d.setKSearch (0);
+ n3d.compute (*normals);
+ }
+
+ int element_stride_normal = sizeof(pcl::Normal) / sizeof(float);
+ int row_stride_normal = element_stride_normal * normals->width;
+ boost::shared_ptr<pcl::IntegralImage2D<float, 1> > integral_image_normal_x;
+ boost::shared_ptr<pcl::IntegralImage2D<float, 1> > integral_image_normal_y;
+ boost::shared_ptr<pcl::IntegralImage2D<float, 1> > integral_image_normal_z;
+
+ if (use_normals_)
+ {
+ integral_image_normal_x.reset (new pcl::IntegralImage2D<float, 1> (false));
+ const float *data_nx = reinterpret_cast<const float*> (&normals->points[0]);
+ integral_image_normal_x->setInput (data_nx, normals->width, normals->height, element_stride_normal, row_stride_normal);
+
+ integral_image_normal_y.reset (new pcl::IntegralImage2D<float, 1> (false));
+ const float *data_ny = reinterpret_cast<const float*> (&normals->points[0]);
+ integral_image_normal_y->setInput (data_ny + 1, normals->width, normals->height, element_stride_normal, row_stride_normal);
+
+ integral_image_normal_z.reset (new pcl::IntegralImage2D<float, 1> (false));
+ const float *data_nz = reinterpret_cast<const float*> (&normals->points[0]);
+ integral_image_normal_z->setInput (data_nz + 2, normals->width, normals->height, element_stride_normal, row_stride_normal);
+ }
+
+ {
+ //instantiate evaluator
+ pcl::DecisionForestEvaluator<face_detection::FeatureType, std::vector<face_detection::TrainingExample>, float, int, NodeType> dfe;
+ face_detection::FeatureHandlerDepthAverage<face_detection::FeatureType, std::vector<face_detection::TrainingExample>, int> fhda;
+ fhda.setWSize (w_size_);
+ fhda.setNumChannels (1);
+ if (use_normals_)
+ fhda.setNumChannels (4);
+
+ //pcl::BinaryTreeThresholdBasedBranchEstimator * btt = new pcl::BinaryTreeThresholdBasedBranchEstimator ();
+ pcl::TernaryTreeMissingDataBranchEstimator * btt = new pcl::TernaryTreeMissingDataBranchEstimator ();
+ face_detection::PoseClassRegressionVarianceStatsEstimator<float, NodeType, std::vector<face_detection::TrainingExample>, int> rse (btt);
+
+ std::vector<float> weights;
+ weights.resize (cloud->points.size ());
+ for (size_t i = 0; i < cloud->points.size (); i++)
+ weights[i] = 0;
+
+ int w_size_2 = static_cast<int> (w_size_ / 2);
+
+ //do sliding window
+ for (int col = 0; col < (static_cast<int> (cloud->width) - w_size_); col += stride_sw_)
+ {
+ for (int row = 0; row < (static_cast<int> (cloud->height) - w_size_); row += stride_sw_)
+ {
+
+ if (!pcl::isFinite (cloud->at (col + w_size_2, row + w_size_2))) //reject patches with invalid center point
+ continue;
+
+ if (integral_image_depth->getFiniteElementsCount (col, row, w_size_, w_size_) > (0.1 * w_size_ * w_size_))
+ {
+ face_detection::TrainingExample te;
+ //te.iimage_ = integral_image_depth;
+ te.iimages_.push_back (integral_image_depth);
+ if (use_normals_)
+ {
+ te.iimages_.push_back (integral_image_normal_x);
+ te.iimages_.push_back (integral_image_normal_y);
+ te.iimages_.push_back (integral_image_normal_z);
+ }
+ te.row_ = row;
+ te.col_ = col;
+ te.wsize_ = w_size_;
+
+ std::vector<face_detection::TrainingExample> eval_examples;
+ eval_examples.push_back (te);
+ /*std::vector<int> example_indices;
+ example_indices.push_back(0);*/
+
+ //evaluate this patch through the trees
+ std::vector<NodeType> leaves;
+ dfe.evaluate (forest_, fhda, rse, eval_examples, 0, leaves);
+
+ for (size_t l = 0; l < leaves.size (); l++)
+ {
+ if (leaves[l].value >= thres_face_)
+ {
+ if ((leaves[l].covariance_trans_.trace () + leaves[l].covariance_rot_.trace ()) > trans_max_variance_)
+ continue;
+
+ Eigen::Vector3f head_center = Eigen::Vector3f (static_cast<float>(leaves[l].trans_mean_[0]),
+ static_cast<float>(leaves[l].trans_mean_[1]),
+ static_cast<float>(leaves[l].trans_mean_[2]));
+ head_center *= 0.001f;
+
+ pcl::PointXYZ patch_center_point;
+ patch_center_point.x = cloud->at (col + w_size_2, row + w_size_2).x;
+ patch_center_point.y = cloud->at (col + w_size_2, row + w_size_2).y;
+ patch_center_point.z = cloud->at (col + w_size_2, row + w_size_2).z;
+
+ head_center = patch_center_point.getVector3fMap () + head_center;
+
+ pcl::PointXYZ ppp;
+ ppp.getVector3fMap () = head_center;
+ if (!pcl::isFinite (ppp))
+ continue;
+
+ //this is a good leave
+ for (int j = te.col_; j < (te.col_ + w_size_); j++)
+ {
+ for (int k = te.row_; k < (te.row_ + w_size_); k++)
+ weights[k * cloud->width + j]++;
+ }
+
+ head_center_votes_.push_back (head_center);
+ float mult_fact = 0.0174532925f;
+ angle_votes_.push_back (
+ Eigen::Vector3f (static_cast<float>(leaves[l].rot_mean_[0]) * mult_fact,
+ static_cast<float>(leaves[l].rot_mean_[1]) * mult_fact,
+ static_cast<float>(leaves[l].rot_mean_[2]) * mult_fact));
+ uncertainties_.push_back (static_cast<float>(leaves[l].covariance_trans_.trace () + leaves[l].covariance_rot_.trace ()));
+ }
+ }
+ }
+ }
+ }
+
+ if (face_heat_map_)
+ {
+ face_heat_map_.reset (new pcl::PointCloud<pcl::PointXYZI>);
+ face_heat_map_->resize (cloud->points.size ());
+ face_heat_map_->height = 1;
+ face_heat_map_->width = static_cast<unsigned int>(cloud->points.size ());
+ face_heat_map_->is_dense = false;
+
+ for (size_t i = 0; i < cloud->points.size (); i++)
+ {
+ face_heat_map_->points[i].getVector4fMap () = cloud->points[i].getVector4fMap ();
+ face_heat_map_->points[i].intensity = weights[i];
+ }
+ }
+ }
+
+ faceVotesClustering ();
+
+ if (pose_refinement_ && (head_clusters_centers_.size () > 0))
+ {
+ Eigen::Matrix4f icp_trans;
+ float max_distance = 0.015f;
+ int iter = icp_iterations_;
+
+ pcl::PointCloud<pcl::PointNormal>::Ptr cloud_voxelized (new pcl::PointCloud<pcl::PointNormal> ());
+ pcl::PointCloud<pcl::Normal>::Ptr scene_normals (new pcl::PointCloud<pcl::Normal> ());
+
+ {
+ typedef pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> NormalEstimator_;
+ NormalEstimator_ n3d;
+ n3d.setNormalEstimationMethod (n3d.COVARIANCE_MATRIX);
+ n3d.setInputCloud (input_);
+ n3d.setRadiusSearch (0.f);
+ n3d.setKSearch (10);
+ n3d.compute (*scene_normals);
+ }
+
+ pcl::copyPointCloud (*input_, *cloud_voxelized);
+ pcl::copyPointCloud (*scene_normals, *cloud_voxelized);
+
+ pcl::PointCloud<pcl::PointNormal>::Ptr cloud_voxelized_icp_normals (new pcl::PointCloud<pcl::PointNormal> ());
+ pcl::VoxelGrid<pcl::PointNormal> voxel_grid_icp;
+ voxel_grid_icp.setInputCloud (cloud_voxelized);
+ voxel_grid_icp.setDownsampleAllData (true);
+ voxel_grid_icp.setLeafSize (res_, res_, res_);
+ voxel_grid_icp.filter (*cloud_voxelized_icp_normals);
+
+ //compute normals
+ pcl::PointCloud<pcl::PointNormal>::Ptr model_aligned_normals (new pcl::PointCloud<pcl::PointNormal> ());
+ pcl::copyPointCloud (*model_original_, *model_aligned_normals);
+
+ pcl::NormalEstimation<pcl::PointNormal, pcl::PointNormal> normal_est_;
+ normal_est_.setKSearch (10);
+
+ {
+ normal_est_.setInputCloud (model_aligned_normals);
+ normal_est_.compute (*model_aligned_normals);
+ }
+
+ //do pose refinement for the detected heads
+ //std::vector<pcl::PointCloud<pcl::PointNormal>::ConstPtr> aligned_models_;
+ pcl::PointCloud<pcl::PointNormal>::Ptr output (new pcl::PointCloud<pcl::PointNormal> ());
+
+ pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> reg;
+ for (size_t i = 0; i < head_clusters_centers_.size (); i++)
+ {
+ Eigen::Matrix3f matrixxx;
+
+ matrixxx = Eigen::AngleAxisf (head_clusters_rotation_[i][0], Eigen::Vector3f::UnitX ())
+ * Eigen::AngleAxisf (head_clusters_rotation_[i][1], Eigen::Vector3f::UnitY ())
+ * Eigen::AngleAxisf (head_clusters_rotation_[i][2], Eigen::Vector3f::UnitZ ());
+
+ Eigen::Matrix4f guess;
+ guess.setIdentity ();
+ guess.block<3, 3> (0, 0) = matrixxx;
+ guess (0, 3) = head_clusters_centers_[i][0];
+ guess (1, 3) = head_clusters_centers_[i][1];
+ guess (2, 3) = head_clusters_centers_[i][2];
+
+ pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>::Ptr trans_lls (
+ new pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>);
+
+ pcl::registration::CorrespondenceEstimationNormalShooting<pcl::PointNormal, pcl::PointNormal, pcl::PointNormal>::Ptr cens (
+ new pcl::registration::CorrespondenceEstimationNormalShooting<pcl::PointNormal, pcl::PointNormal, pcl::PointNormal>);
+
+ cens->setInputSource (model_aligned_normals);
+ cens->setInputTarget (cloud_voxelized_icp_normals);
+ cens->setSourceNormals (model_aligned_normals);
+
+ pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointNormal>::Ptr rej (
+ new pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointNormal> ());
+
+ rej->setInputSource (model_aligned_normals);
+ rej->setInputTarget (cloud_voxelized_icp_normals);
+ rej->setMaximumIterations (1000);
+ rej->setInlierThreshold (0.01f);
+
+ reg.addCorrespondenceRejector (rej);
+ reg.setCorrespondenceEstimation (cens);
+ reg.setTransformationEstimation (trans_lls);
+
+ reg.setInputSource (model_aligned_normals); //model
+ reg.setInputTarget (cloud_voxelized_icp_normals); //scene
+ reg.setMaximumIterations (iter);
+ reg.setMaxCorrespondenceDistance (max_distance);
+ reg.setTransformationEpsilon (1e-12);
+ reg.align (*output, guess);
+ icp_trans = reg.getFinalTransformation ();
+
+ //update values
+ head_clusters_centers_[i][0] = icp_trans (0, 3);
+ head_clusters_centers_[i][1] = icp_trans (1, 3);
+ head_clusters_centers_[i][2] = icp_trans (2, 3);
+
+ Eigen::Vector3f ea = icp_trans.block<3, 3> (0, 0).eulerAngles (0, 1, 2);
+ head_clusters_rotation_[i][0] = ea[0];
+ head_clusters_rotation_[i][1] = ea[1];
+ head_clusters_rotation_[i][2] = ea[2];
+
+ }
+
+ //do HV
+ /*pcl::PapazovHV<pcl::PointXYZ, pcl::PointXYZ> papazov;
+ papazov.setResolution (0.005f);
+ papazov.setInlierThreshold (0.01f);
+ papazov.setSupportThreshold (0.1f);
+ papazov.setPenaltyThreshold (0.2f);
+ papazov.setConflictThreshold (0.01f);
+
+ std::vector<bool> mask_hv;
+ papazov.setOcclusionCloud (input_);
+ papazov.setSceneCloud (cloud_voxelized_icp);
+ papazov.addModels (aligned_models_, true);
+ papazov.verify ();
+ papazov.getMask (mask_hv);
+
+ size_t valid=0;
+ for(size_t i=0; i < mask_hv.size(); i++) {
+ if (!mask_hv[i])
+ continue;
+
+ if(valid < i) {
+ head_clusters_centers_[valid] = head_clusters_centers_[i];
+ head_clusters_rotation_[valid] = head_clusters_rotation_[i];
+ }
+ }
+
+ std::cout << "Valid heads after HV:" << valid << " before was:" << mask_hv.size() << std::endl;
+ head_clusters_centers_.resize(valid);
+ head_clusters_rotation_.resize(valid);*/
+ }
+}
+
}
}
+#ifdef __SSE2__
+ aligned_free (score_sums);
+ aligned_free (tmp_score_sums);
+#else
delete[] score_sums;
+#endif
+
#ifdef LINEMOD_USE_SEPARATE_ENERGY_MAPS
delete[] score_sums_1;
delete[] score_sums_2;
"include/pcl/${SUBSYS_NAME}/ia_ransac.h"
"include/pcl/${SUBSYS_NAME}/icp.h"
"include/pcl/${SUBSYS_NAME}/joint_icp.h"
+ "include/pcl/${SUBSYS_NAME}/incremental_registration.h"
"include/pcl/${SUBSYS_NAME}/icp_nl.h"
"include/pcl/${SUBSYS_NAME}/lum.h"
"include/pcl/${SUBSYS_NAME}/elch.h"
+ "include/pcl/${SUBSYS_NAME}/meta_registration.h"
"include/pcl/${SUBSYS_NAME}/ndt.h"
"include/pcl/${SUBSYS_NAME}/ndt_2d.h"
"include/pcl/${SUBSYS_NAME}/ppf_registration.h"
"include/pcl/${SUBSYS_NAME}/distances.h"
"include/pcl/${SUBSYS_NAME}/exceptions.h"
"include/pcl/${SUBSYS_NAME}/sample_consensus_prerejective.h"
+ "include/pcl/${SUBSYS_NAME}/ia_fpcs.h"
+ "include/pcl/${SUBSYS_NAME}/ia_kfpcs.h"
+ "include/pcl/${SUBSYS_NAME}/matching_candidate.h"
+ "include/pcl/${SUBSYS_NAME}/transformation_estimation_3point.h"
)
set(impl_incs
"include/pcl/${SUBSYS_NAME}/impl/ia_ransac.hpp"
"include/pcl/${SUBSYS_NAME}/impl/icp.hpp"
"include/pcl/${SUBSYS_NAME}/impl/joint_icp.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/incremental_registration.hpp"
"include/pcl/${SUBSYS_NAME}/impl/icp_nl.hpp"
"include/pcl/${SUBSYS_NAME}/impl/elch.hpp"
"include/pcl/${SUBSYS_NAME}/impl/lum.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/meta_registration.hpp"
"include/pcl/${SUBSYS_NAME}/impl/ndt.hpp"
"include/pcl/${SUBSYS_NAME}/impl/ndt_2d.hpp"
"include/pcl/${SUBSYS_NAME}/impl/ppf_registration.hpp"
"include/pcl/${SUBSYS_NAME}/impl/transformation_validation_euclidean.hpp"
"include/pcl/${SUBSYS_NAME}/impl/gicp.hpp"
"include/pcl/${SUBSYS_NAME}/impl/sample_consensus_prerejective.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/ia_fpcs.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/ia_kfpcs.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_3point.hpp"
)
set(srcs
* cloud for computing correspondences. By default we use k = 10 nearest
* neighbors.
*/
- inline void
+ inline unsigned int
getKSearch () const { return (k_); }
/** \brief Clone and cast to CorrespondenceEstimationBase */
* cloud for computing correspondences. By default we use k = 10 nearest
* neighbors.
*/
- inline void
+ inline unsigned int
getKSearch () const { return (k_); }
/** \brief Clone and cast to CorrespondenceEstimationBase */
virtual ~DataContainerInterface () {}
virtual double getCorrespondenceScore (int index) = 0;
virtual double getCorrespondenceScore (const pcl::Correspondence &) = 0;
+ virtual double getCorrespondenceScoreFromNormals (const pcl::Correspondence &) = 0;
};
/** @b DataContainer is a container for the input and target point clouds and implements the interface
public:
SolverDidntConvergeException (const std::string& error_description,
- const std::string& file_name = "",
- const std::string& function_name = "" ,
- unsigned line_number = 0) throw ()
+ const char* file_name = NULL,
+ const char* function_name = NULL,
+ unsigned line_number = 0)
: pcl::PCLException (error_description, file_name, function_name, line_number) { }
} ;
public:
NotEnoughPointsException (const std::string& error_description,
- const std::string& file_name = "",
- const std::string& function_name = "" ,
- unsigned line_number = 0) throw ()
+ const char* file_name = NULL,
+ const char* function_name = NULL,
+ unsigned line_number = 0)
: pcl::PCLException (error_description, file_name, function_name, line_number) { }
} ;
}
{
/** \brief GeneralizedIterativeClosestPoint is an ICP variant that implements the
* generalized iterative closest point algorithm as described by Alex Segal et al. in
- * http://www.stanford.edu/~avsegal/resources/papers/Generalized_ICP.pdf
+ * http://www.robots.ox.ac.uk/~avsegal/resources/papers/Generalized_ICP.pdf
* The approach is based on using anistropic cost functions to optimize the alignment
* after closest point assignments have been made.
* The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and
typedef PointIndices::Ptr PointIndicesPtr;
typedef PointIndices::ConstPtr PointIndicesConstPtr;
+ typedef std::vector< Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> > MatricesVector;
+ typedef boost::shared_ptr< MatricesVector > MatricesVectorPtr;
+ typedef boost::shared_ptr< const MatricesVector > MatricesVectorConstPtr;
+
typedef typename Registration<PointSource, PointTarget>::KdTree InputKdTree;
typedef typename Registration<PointSource, PointTarget>::KdTreePtr InputKdTreePtr;
: k_correspondences_(20)
, gicp_epsilon_(0.001)
, rotation_epsilon_(2e-3)
- , input_covariances_(0)
- , target_covariances_(0)
, mahalanobis_(0)
, max_inner_iterations_(20)
{
input[i].data[3] = 1.0;
pcl::IterativeClosestPoint<PointSource, PointTarget>::setInputSource (cloud);
- input_covariances_.clear ();
- input_covariances_.reserve (input_->size ());
+ input_covariances_.reset ();
}
+ /** \brief Provide a pointer to the covariances of the input source (if computed externally!).
+ * If not set, GeneralizedIterativeClosestPoint will compute the covariances itself.
+ * Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances).
+ * \param[in] target the input point cloud target
+ */
+ inline void
+ setSourceCovariances (const MatricesVectorPtr& covariances)
+ {
+ input_covariances_ = covariances;
+ }
+
/** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to)
* \param[in] target the input point cloud target
*/
setInputTarget (const PointCloudTargetConstPtr &target)
{
pcl::IterativeClosestPoint<PointSource, PointTarget>::setInputTarget(target);
- target_covariances_.clear ();
- target_covariances_.reserve (target_->size ());
+ target_covariances_.reset ();
}
+ /** \brief Provide a pointer to the covariances of the input target (if computed externally!).
+ * If not set, GeneralizedIterativeClosestPoint will compute the covariances itself.
+ * Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances).
+ * \param[in] target the input point cloud target
+ */
+ inline void
+ setTargetCovariances (const MatricesVectorPtr& covariances)
+ {
+ target_covariances_ = covariances;
+ }
+
/** \brief Estimate a rigid rotation transformation between a source and a target point cloud using an iterative
* non-linear Levenberg-Marquardt approach.
* \param[in] cloud_src the source point cloud dataset
/** \brief Input cloud points covariances. */
- std::vector<Eigen::Matrix3d> input_covariances_;
+ MatricesVectorPtr input_covariances_;
/** \brief Target cloud points covariances. */
- std::vector<Eigen::Matrix3d> target_covariances_;
+ MatricesVectorPtr target_covariances_;
/** \brief Mahalanobis matrices holder. */
std::vector<Eigen::Matrix3d> mahalanobis_;
template<typename PointT>
void computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
const typename pcl::search::KdTree<PointT>::Ptr tree,
- std::vector<Eigen::Matrix3d>& cloud_covariances);
+ MatricesVector& cloud_covariances);
/** \return trace of mat1^t . mat2
* \param mat1 matrix of dimension nxm
* \author Martin Holzkothen, Michael Korn
* \ingroup registration
*/
- class GeneralizedIterativeClosestPoint6D : public GeneralizedIterativeClosestPoint<PointXYZRGBA, PointXYZRGBA>
+ class
PCL_EXPORTS GeneralizedIterativeClosestPoint6D : public GeneralizedIterativeClosestPoint<PointXYZRGBA, PointXYZRGBA>
{
typedef PointXYZRGBA PointSource;
typedef PointXYZRGBA PointTarget;
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ * Copyright (C) 2008 Ben Gurion University of the Negev, Beer Sheva, Israel.
+ *
+ * All rights reserved
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met
+ *
+ * * The use for research only (no for any commercial application).
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_REGISTRATION_IA_FPCS_H_
+#define PCL_REGISTRATION_IA_FPCS_H_
+
+#include <pcl/common/common.h>
+#include <pcl/registration/registration.h>
+#include <pcl/registration/matching_candidate.h>
+
+namespace pcl
+{
+ /** \brief Compute the mean point density of a given point cloud.
+ * \param[in] cloud pointer to the input point cloud
+ * \param[in] max_dist maximum distance of a point to be considered as a neighbor
+ * \param[in] nr_threads number of threads to use (default = 1, only used if OpenMP flag is set)
+ * \return the mean point density of a given point cloud
+ */
+ template <typename PointT> inline float
+ getMeanPointDensity (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, float max_dist, int nr_threads = 1);
+
+ /** \brief Compute the mean point density of a given point cloud.
+ * \param[in] cloud pointer to the input point cloud
+ * \param[in] indices the vector of point indices to use from \a cloud
+ * \param[in] max_dist maximum distance of a point to be considered as a neighbor
+ * \param[in] nr_threads number of threads to use (default = 1, only used if OpenMP flag is set)
+ * \return the mean point density of a given point cloud
+ */
+ template <typename PointT> inline float
+ getMeanPointDensity (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, const std::vector <int> &indices,
+ float max_dist, int nr_threads = 1);
+
+
+ namespace registration
+ {
+ /** \brief FPCSInitialAlignment computes corresponding four point congruent sets as described in:
+ * "4-points congruent sets for robust pairwise surface registration", Dror Aiger, Niloy Mitra, Daniel Cohen-Or.
+ * ACM Transactions on Graphics, vol. 27(3), 2008
+ * \author P.W.Theiler
+ * \ingroup registration
+ */
+ template <typename PointSource, typename PointTarget, typename NormalT = pcl::Normal, typename Scalar = float>
+ class FPCSInitialAlignment : public Registration <PointSource, PointTarget, Scalar>
+ {
+ public:
+ /** \cond */
+ typedef boost::shared_ptr <FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar> > Ptr;
+ typedef boost::shared_ptr <const FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar> > ConstPtr;
+
+ typedef pcl::search::KdTree<PointSource> KdTreeReciprocal;
+ typedef typename KdTreeReciprocal::Ptr KdTreeReciprocalPtr;
+
+ typedef pcl::PointCloud <PointTarget> PointCloudTarget;
+ typedef pcl::PointCloud <PointSource> PointCloudSource;
+ typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
+ typedef typename PointCloudSource::iterator PointCloudSourceIterator;
+
+ typedef pcl::PointCloud <NormalT> Normals;
+ typedef typename Normals::ConstPtr NormalsConstPtr;
+
+ typedef pcl::registration::MatchingCandidate MatchingCandidate;
+ typedef pcl::registration::MatchingCandidates MatchingCandidates;
+ /** \endcond */
+
+
+ /** \brief Constructor.
+ * Resets the maximum number of iterations to 0 thus forcing an internal computation if not set by the user.
+ * Sets the number of RANSAC iterations to 1000 and the standard transformation estimation to TransformationEstimation3Point.
+ */
+ FPCSInitialAlignment ();
+
+ /** \brief Destructor. */
+ virtual ~FPCSInitialAlignment ()
+ {};
+
+
+ /** \brief Provide a pointer to the vector of target indices.
+ * \param[in] target_indices a pointer to the target indices
+ */
+ inline void
+ setTargetIndices (const IndicesPtr &target_indices)
+ {
+ target_indices_ = target_indices;
+ };
+
+ /** \return a pointer to the vector of target indices. */
+ inline IndicesPtr
+ getTargetIndices () const
+ {
+ return (target_indices_);
+ };
+
+
+ /** \brief Provide a pointer to the normals of the source point cloud.
+ * \param[in] source_normals pointer to the normals of the source pointer cloud.
+ */
+ inline void
+ setSourceNormals (const NormalsConstPtr &source_normals)
+ {
+ source_normals_ = source_normals;
+ };
+
+ /** \return the normals of the source point cloud. */
+ inline NormalsConstPtr
+ getSourceNormals () const
+ {
+ return (source_normals_);
+ };
+
+
+ /** \brief Provide a pointer to the normals of the target point cloud.
+ * \param[in] target_normals point to the normals of the target point cloud.
+ */
+ inline void
+ setTargetNormals (const NormalsConstPtr &target_normals)
+ {
+ target_normals_ = target_normals;
+ };
+
+ /** \return the normals of the target point cloud. */
+ inline NormalsConstPtr
+ getTargetNormals () const
+ {
+ return (target_normals_);
+ };
+
+
+ /** \brief Set the number of used threads if OpenMP is activated.
+ * \param[in] nr_threads the number of used threads
+ */
+ inline void
+ setNumberOfThreads (int nr_threads)
+ {
+ nr_threads_ = nr_threads;
+ };
+
+ /** \return the number of threads used if OpenMP is activated. */
+ inline int
+ getNumberOfThreads () const
+ {
+ return (nr_threads_);
+ };
+
+
+ /** \brief Set the constant factor delta which weights the internally calculated parameters.
+ * \param[in] delta the weight factor delta
+ * \param[in] normalize flag if delta should be normalized according to point cloud density
+ */
+ inline void
+ setDelta (float delta, bool normalize = false)
+ {
+ delta_ = delta;
+ normalize_delta_ = normalize;
+ };
+
+ /** \return the constant factor delta which weights the internally calculated parameters. */
+ inline float
+ getDelta () const
+ {
+ return (delta_);
+ };
+
+
+ /** \brief Set the approximate overlap between source and target.
+ * \param[in] approx_overlap the estimated overlap
+ */
+ inline void
+ setApproxOverlap (float approx_overlap)
+ {
+ approx_overlap_ = approx_overlap;
+ };
+
+ /** \return the approximated overlap between source and target. */
+ inline float
+ getApproxOverlap () const
+ {
+ return (approx_overlap_);
+ };
+
+
+ /** \brief Set the scoring threshold used for early finishing the method.
+ * \param[in] score_threshold early terminating score criteria
+ */
+ inline void
+ setScoreThreshold (float score_threshold)
+ {
+ score_threshold_ = score_threshold;
+ };
+
+ /** \return the scoring threshold used for early finishing the method. */
+ inline float
+ getScoreThreshold () const
+ {
+ return (score_threshold_);
+ };
+
+
+ /** \brief Set the number of source samples to use during alignment.
+ * \param[in] nr_samples the number of source samples
+ */
+ inline void
+ setNumberOfSamples (int nr_samples)
+ {
+ nr_samples_ = nr_samples;
+ };
+
+ /** \return the number of source samples to use during alignment. */
+ inline int
+ getNumberOfSamples () const
+ {
+ return (nr_samples_);
+ };
+
+
+ /** \brief Set the maximum normal difference between valid point correspondences in degree.
+ * \param[in] max_norm_diff the maximum difference in degree
+ */
+ inline void
+ setMaxNormalDifference (float max_norm_diff)
+ {
+ max_norm_diff_ = max_norm_diff;
+ };
+
+ /** \return the maximum normal difference between valid point correspondences in degree. */
+ inline float
+ getMaxNormalDifference () const
+ {
+ return (max_norm_diff_);
+ };
+
+
+ /** \brief Set the maximum computation time in seconds.
+ * \param[in] max_runtime the maximum runtime of the method in seconds
+ */
+ inline void
+ setMaxComputationTime (int max_runtime)
+ {
+ max_runtime_ = max_runtime;
+ };
+
+ /** \return the maximum computation time in seconds. */
+ inline int
+ getMaxComputationTime () const
+ {
+ return (max_runtime_);
+ };
+
+
+ /** \return the fitness score of the best scored four-point match. */
+ inline float
+ getFitnessScore () const
+ {
+ return (fitness_score_);
+ };
+
+ protected:
+
+ using PCLBase <PointSource>::deinitCompute;
+ using PCLBase <PointSource>::input_;
+ using PCLBase <PointSource>::indices_;
+
+ using Registration <PointSource, PointTarget, Scalar>::reg_name_;
+ using Registration <PointSource, PointTarget, Scalar>::target_;
+ using Registration <PointSource, PointTarget, Scalar>::tree_;
+ using Registration <PointSource, PointTarget, Scalar>::correspondences_;
+ using Registration <PointSource, PointTarget, Scalar>::target_cloud_updated_;
+ using Registration <PointSource, PointTarget, Scalar>::final_transformation_;
+ using Registration <PointSource, PointTarget, Scalar>::max_iterations_;
+ using Registration <PointSource, PointTarget, Scalar>::ransac_iterations_;
+ using Registration <PointSource, PointTarget, Scalar>::transformation_estimation_;
+ using Registration <PointSource, PointTarget, Scalar>::converged_;
+
+
+ /** \brief Rigid transformation computation method.
+ * \param output the transformed input point cloud dataset using the rigid transformation found
+ * \param guess The computed transforamtion
+ */
+ virtual void
+ computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess);
+
+
+ /** \brief Internal computation initialization. */
+ virtual bool
+ initCompute ();
+
+ /** \brief Select an approximately coplanar set of four points from the source cloud.
+ * \param[out] base_indices selected source cloud indices, further used as base (B)
+ * \param[out] ratio the two diagonal intersection ratios (r1,r2) of the base points
+ * \return
+ * * < 0 no coplanar four point sets with large enough sampling distance was found
+ * * = 0 a set of four congruent points was selected
+ */
+ int
+ selectBase (std::vector <int> &base_indices, float (&ratio)[2]);
+
+ /** \brief Select randomly a triplet of points with large point-to-point distances. The minimum point
+ * sampling distance is calculated based on the estimated point cloud overlap during initialization.
+ *
+ * \param[out] base_indices indices of base B
+ * \return
+ * * < 0 no triangle with large enough base lines could be selected
+ * * = 0 base triangle succesully selected
+ */
+ int
+ selectBaseTriangle (std::vector <int> &base_indices);
+
+ /** \brief Setup the base (four coplanar points) by ordering the points and computing intersection
+ * ratios and segment to segment distances of base diagonal.
+ *
+ * \param[in,out] base_indices indices of base B (will be reordered)
+ * \param[out] ratio diagonal intersection ratios of base points
+ */
+ void
+ setupBase (std::vector <int> &base_indices, float (&ratio)[2]);
+
+ /** \brief Calculate intersection ratios and segment to segment distances of base diagonals.
+ * \param[in] base_indices indices of base B
+ * \param[out] ratio diagonal intersection ratios of base points
+ * \return quality value of diagonal intersection
+ */
+ float
+ segmentToSegmentDist (const std::vector <int> &base_indices, float (&ratio)[2]);
+
+ /** \brief Search for corresponding point pairs given the distance between two base points.
+ *
+ * \param[in] idx1 first index of current base segment (in source cloud)
+ * \param[in] idx2 second index of current base segment (in source cloud)
+ * \param[out] pairs resulting point pairs with point-to-point distance close to ref_dist
+ * \return
+ * * < 0 no corresponding point pair was found
+ * * = 0 at least one point pair candidate was found
+ */
+ virtual int
+ bruteForceCorrespondences (int idx1, int idx2, pcl::Correspondences &pairs);
+
+ /** \brief Determine base matches by combining the point pair candidate and search for coinciding
+ * intersection points using the diagonal segment ratios of base B. The coincidation threshold is
+ * calculated during initialization (coincidation_limit_).
+ *
+ * \param[in] base_indices indices of base B
+ * \param[out] matches vector of candidate matches w.r.t the base B
+ * \param[in] pairs_a point pairs corresponding to points of 1st diagonal of base B
+ * \param[in] pairs_b point pairs corresponding to points of 2nd diagonal of base B
+ * \param[in] ratio diagonal intersection ratios of base points
+ * \return
+ * * < 0 no base match could be found
+ * * = 0 at least one base match was found
+ */
+ virtual int
+ determineBaseMatches (
+ const std::vector <int> &base_indices,
+ std::vector <std::vector <int> > &matches,
+ const pcl::Correspondences &pairs_a,
+ const pcl::Correspondences &pairs_b,
+ const float (&ratio)[2]);
+
+ /** \brief Check if outer rectangle distance of matched points fit with the base rectangle.
+ *
+ * \param[in] match_indices indices of match M
+ * \param[in] ds edge lengths of base B
+ * \return
+ * * < 0 at least one edge of the match M has no corresponding one in the base B
+ * * = 0 edges of match M fits to the ones of base B
+ */
+ int
+ checkBaseMatch (const std::vector <int> &match_indices, const float (&ds)[4]);
+
+ /** \brief Method to handle current candidate matches. Here we validate and evaluate the matches w.r.t the
+ * base and store the best fitting match (together with its score and estimated transformation).
+ * \note For forwards compatibility the results are stored in 'vectors of size 1'.
+ *
+ * \param[in] base_indices indices of base B
+ * \param[in,out] matches vector of candidate matches w.r.t the base B. The candidate matches are
+ * reordered during this step.
+ * \param[out] candidates vector which contains the candidates matches M
+ */
+ virtual void
+ handleMatches (
+ const std::vector <int> &base_indices,
+ std::vector <std::vector <int> > &matches,
+ MatchingCandidates &candidates);
+
+ /** \brief Sets the correspondences between the base B and the match M by using the distance of each point
+ * to the centroid of the rectangle.
+ *
+ * \param[in] base_indices indices of base B
+ * \param[in] match_indices indices of match M
+ * \param[out] correspondences resulting correspondences
+ */
+ virtual void
+ linkMatchWithBase (
+ const std::vector <int> &base_indices,
+ std::vector <int> &match_indices,
+ pcl::Correspondences &correspondences);
+
+ /** \brief Validate the matching by computing the transformation between the source and target based on the
+ * four matched points and by comparing the mean square error (MSE) to a threshold. The MSE limit was
+ * calculated during initialization (max_mse_).
+ *
+ * \param[in] base_indices indices of base B
+ * \param[in] match_indices indices of match M
+ * \param[in] correspondences corresondences between source and target
+ * \param[out] transformation resulting transformation matrix
+ * \return
+ * * < 0 MSE bigger than max_mse_
+ * * = 0 MSE smaller than max_mse_
+ */
+ virtual int
+ validateMatch (
+ const std::vector <int> &base_indices,
+ const std::vector <int> &match_indices,
+ const pcl::Correspondences &correspondences,
+ Eigen::Matrix4f &transformation);
+
+ /** \brief Validate the transformation by calculating the number of inliers after transforming the source cloud.
+ * The resulting fitness score is later used as the decision criteria of the best fitting match.
+ *
+ * \param[out] transformation updated orientation matrix using all inliers
+ * \param[out] fitness_score current best fitness_score
+ * \note fitness score is only updated if the score of the current transformation exceeds the input one.
+ * \return
+ * * < 0 if previous result is better than the current one (score remains)
+ * * = 0 current result is better than the previous one (score updated)
+ */
+ virtual int
+ validateTransformation (Eigen::Matrix4f &transformation, float &fitness_score);
+
+ /** \brief Final computation of best match out of vector of best matches. To avoid cross thread dependencies
+ * during parallel running, a best match for each try was calculated.
+ * \note For forwards compatibility the candidates are stored in vectors of 'vectors of size 1'.
+ * \param[in] candidates vector of candidate matches
+ */
+ virtual void
+ finalCompute (const std::vector <MatchingCandidates > &candidates);
+
+
+ /** \brief Normals of source point cloud. */
+ NormalsConstPtr source_normals_;
+
+ /** \brief Normals of target point cloud. */
+ NormalsConstPtr target_normals_;
+
+
+ /** \brief Number of threads for parallelization (standard = 1).
+ * \note Only used if run compiled with OpenMP.
+ */
+ int nr_threads_;
+
+ /** \brief Estimated overlap between source and target (standard = 0.5). */
+ float approx_overlap_;
+
+ /** \brief Delta value of 4pcs algorithm (standard = 1.0).
+ * It can be used as:
+ * * absolute value (normalization = false), value should represent the point accuracy to ensure finding neighbors between source <-> target
+ * * relative value (normalization = true), to adjust the internally calculated point accuracy (= point density)
+ */
+ float delta_;
+
+ /** \brief Score threshold to stop calculation with success.
+ * If not set by the user it is equal to the approximated overlap
+ */
+ float score_threshold_;
+
+ /** \brief The number of points to uniformly sample the source point cloud. (standard = 0 => full cloud). */
+ int nr_samples_;
+
+ /** \brief Maximum normal difference of corresponding point pairs in degrees (standard = 90). */
+ float max_norm_diff_;
+
+ /** \brief Maximum allowed computation time in seconds (standard = 0 => ~unlimited). */
+ int max_runtime_;
+
+
+ /** \brief Resulting fitness score of the best match. */
+ float fitness_score_;
+
+
+ /** \brief Estimated diamter of the target point cloud. */
+ float diameter_;
+
+ /** \brief Estimated squared metric overlap between source and target.
+ * \note Internally calculated using the estimated overlap and the extent of the source cloud.
+ * It is used to derive the minimum sampling distance of the base points as well as to calculated
+ * the number of trys to reliable find a correct mach.
+ */
+ float max_base_diameter_sqr_;
+
+ /** \brief Use normals flag. */
+ bool use_normals_;
+
+ /** \brief Normalize delta flag. */
+ bool normalize_delta_;
+
+
+ /** \brief A pointer to the vector of source point indices to use after sampling. */
+ pcl::IndicesPtr source_indices_;
+
+ /** \brief A pointer to the vector of target point indices to use after sampling. */
+ pcl::IndicesPtr target_indices_;
+
+ /** \brief Maximal difference between corresponding point pairs in source and target.
+ * \note Internally calculated using an estimation of the point density.
+ */
+ float max_pair_diff_;
+
+ /** \brief Maximal difference between the length of the base edges and valid match edges.
+ * \note Internally calculated using an estimation of the point density.
+ */
+ float max_edge_diff_;
+
+ /** \brief Maximal distance between coinciding intersection points to find valid matches.
+ * \note Internally calculated using an estimation of the point density.
+ */
+ float coincidation_limit_;
+
+ /** \brief Maximal mean squared errors of a transformation calculated from a candidate match.
+ * \note Internally calculated using an estimation of the point density.
+ */
+ float max_mse_;
+
+ /** \brief Maximal squared point distance between source and target points to count as inlier.
+ * \note Internally calculated using an estimation of the point density.
+ */
+ float max_inlier_dist_sqr_;
+
+
+ /** \brief Definition of a small error. */
+ const float small_error_;
+
+ };
+ }; // namespace registration
+}; // namespace pcl
+
+#include <pcl/registration/impl/ia_fpcs.hpp>
+
+#endif // PCL_REGISTRATION_IA_FPCS_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met
+ *
+ * * The use for research only (no for any commercial application).
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_REGISTRATION_IA_KFPCS_H_
+#define PCL_REGISTRATION_IA_KFPCS_H_
+
+#include <pcl/registration/ia_fpcs.h>
+
+namespace pcl
+{
+ namespace registration
+ {
+ /** \brief KFPCSInitialAlignment computes corresponding four point congruent sets based on keypoints
+ * as described in: "Markerless point cloud registration with keypoint-based 4-points congruent sets",
+ * Pascal Theiler, Jan Dirk Wegner, Konrad Schindler. ISPRS Annals II-5/W2, 2013. Presented at ISPRS Workshop
+ * Laser Scanning, Antalya, Turkey, 2013.
+ * \note Method has since been improved and some variations to the paper exist.
+ * \author P.W.Theiler
+ * \ingroup registration
+ */
+ template <typename PointSource, typename PointTarget, typename NormalT = pcl::Normal, typename Scalar = float>
+ class KFPCSInitialAlignment : public virtual FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>
+ {
+ public:
+ /** \cond */
+ typedef boost::shared_ptr <KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar> > Ptr;
+ typedef boost::shared_ptr <const KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar> > ConstPtr;
+
+ typedef pcl::PointCloud <PointSource> PointCloudSource;
+ typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
+ typedef typename PointCloudSource::iterator PointCloudSourceIterator;
+
+ typedef pcl::PointCloud <PointTarget> PointCloudTarget;
+ typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
+ typedef typename PointCloudTarget::iterator PointCloudTargetIterator;
+
+ typedef pcl::registration::MatchingCandidate MatchingCandidate;
+ typedef pcl::registration::MatchingCandidates MatchingCandidates;
+ /** \endcond */
+
+
+ /** \brief Constructor. */
+ KFPCSInitialAlignment ();
+
+ /** \brief Destructor. */
+ virtual ~KFPCSInitialAlignment ()
+ {};
+
+
+ /** \brief Set the upper translation threshold used for score evaluation.
+ * \param[in] upper_trl_boundary upper translation threshold
+ */
+ inline void
+ setUpperTranslationThreshold (float upper_trl_boundary)
+ {
+ upper_trl_boundary_ = upper_trl_boundary;
+ };
+
+ /** \return the upper translation threshold used for score evaluation. */
+ inline float
+ getUpperTranslationThreshold () const
+ {
+ return (upper_trl_boundary_);
+ };
+
+
+ /** \brief Set the lower translation threshold used for score evaluation.
+ * \param[in] lower_trl_boundary lower translation threshold
+ */
+ inline void
+ setLowerTranslationThreshold (float lower_trl_boundary)
+ {
+ lower_trl_boundary_ = lower_trl_boundary;
+ };
+
+ /** \return the lower translation threshold used for score evaluation. */
+ inline float
+ getLowerTranslationThreshold () const
+ {
+ return (lower_trl_boundary_);
+ };
+
+
+ /** \brief Set the weighting factor of the translation cost term.
+ * \param[in] lambda the weighting factor of the translation cost term
+ */
+ inline void
+ setLambda (float lambda)
+ {
+ lambda_ = lambda;
+ };
+
+ /** \return the weighting factor of the translation cost term. */
+ inline float
+ getLambda () const
+ {
+ return (lambda_);
+ };
+
+
+ /** \brief Get the N best unique candidate matches according to their fitness score.
+ * The method only returns unique transformations comparing the translation
+ * and the 3D rotation to already returned transformations.
+ *
+ * \note The method may return less than N candidates, if the number of unique candidates
+ * is smaller than N
+ *
+ * \param[in] n number of best candidates to return
+ * \param[in] min_angle3d minimum 3D angle difference in radian
+ * \param[in] min_translation3d minimum 3D translation difference
+ * \param[out] candidates vector of unique candidates
+ */
+ void
+ getNBestCandidates (int n, float min_angle3d, float min_translation3d, MatchingCandidates &candidates);
+
+ /** \brief Get all unique candidate matches with fitness scores above a threshold t.
+ * The method only returns unique transformations comparing the translation
+ * and the 3D rotation to already returned transformations.
+ *
+ * \param[in] t fitness score threshold
+ * \param[in] min_angle3d minimum 3D angle difference in radian
+ * \param[in] min_translation3d minimum 3D translation difference
+ * \param[out] candidates vector of unique candidates
+ */
+ void
+ getTBestCandidates (float t, float min_angle3d, float min_translation3d, MatchingCandidates &candidates);
+
+
+ protected:
+
+ using PCLBase <PointSource>::deinitCompute;
+ using PCLBase <PointSource>::input_;
+ using PCLBase <PointSource>::indices_;
+
+ using Registration <PointSource, PointTarget, Scalar>::reg_name_;
+ using Registration <PointSource, PointTarget, Scalar>::tree_;
+ using Registration <PointSource, PointTarget, Scalar>::final_transformation_;
+ using Registration <PointSource, PointTarget, Scalar>::ransac_iterations_;
+ using Registration <PointSource, PointTarget, Scalar>::correspondences_;
+ using Registration <PointSource, PointTarget, Scalar>::converged_;
+
+ using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::delta_;
+ using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::approx_overlap_;
+ using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::max_pair_diff_;
+ using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::max_edge_diff_;
+ using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::coincidation_limit_;
+ using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::max_mse_;
+ using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::max_inlier_dist_sqr_;
+ using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::diameter_;
+ using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::normalize_delta_;
+ using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::fitness_score_;
+ using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::score_threshold_;
+ using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::linkMatchWithBase;
+ using FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::validateMatch;
+
+
+ /** \brief Internal computation initialization. */
+ virtual bool
+ initCompute ();
+
+ /** \brief Method to handle current candidate matches. Here we validate and evaluate the matches w.r.t the
+ * base and store the sorted matches (together with score values and estimated transformations).
+ *
+ * \param[in] base_indices indices of base B
+ * \param[in,out] matches vector of candidate matches w.r.t the base B. The candidate matches are
+ * reordered during this step.
+ * \param[out] candidates vector which contains the candidates matches M
+ */
+ virtual void
+ handleMatches (
+ const std::vector <int> &base_indices,
+ std::vector <std::vector <int> > &matches,
+ MatchingCandidates &candidates);
+
+ /** \brief Validate the transformation by calculating the score value after transforming the input source cloud.
+ * The resulting score is later used as the decision criteria of the best fitting match.
+ *
+ * \param[out] transformation updated orientation matrix using all inliers
+ * \param[out] fitness_score current best score
+ * \note fitness score is only updated if the score of the current transformation exceeds the input one.
+ * \return
+ * * < 0 if previous result is better than the current one (score remains)
+ * * = 0 current result is better than the previous one (score updated)
+ */
+ virtual int
+ validateTransformation (Eigen::Matrix4f &transformation, float &fitness_score);
+
+ /** \brief Final computation of best match out of vector of matches. To avoid cross thread dependencies
+ * during parallel running, a best match for each try was calculated.
+ * \note For forwards compatibility the candidates are stored in vectors of 'vectors of size 1'.
+ * \param[in] candidates vector of candidate matches
+ */
+ virtual void
+ finalCompute (const std::vector <MatchingCandidates > &candidates);
+
+
+ /** \brief Lower boundary for translation costs calculation.
+ * \note If not set by the user, the translation costs are not used during evaluation.
+ */
+ float lower_trl_boundary_;
+
+ /** \brief Upper boundary for translation costs calculation.
+ * \note If not set by the user, it is calculated from the estimated overlap and the diameter
+ * of the point cloud.
+ */
+ float upper_trl_boundary_;
+
+ /** \brief Weighting factor for translation costs (standard = 0.5). */
+ float lambda_;
+
+
+ /** \brief Container for resulting vector of registration candidates. */
+ MatchingCandidates candidates_;
+
+ /** \brief Flag if translation score should be used in validation (internal calculation). */
+ bool use_trl_score_;
+
+ /** \brief Subset of input indices on which we evaluate candidates.
+ * To speed up the evaluation, we only use a fix number of indices defined during initialization.
+ */
+ pcl::IndicesPtr indices_validation_;
+
+ };
+ }; // namespace registration
+}; // namespace pcl
+
+#include <pcl/registration/impl/ia_kfpcs.hpp>
+
+#endif // PCL_REGISTRATION_IA_KFPCS_H_
using Registration<PointSource, PointTarget, Scalar>::reg_name_;
using Registration<PointSource, PointTarget, Scalar>::getClassName;
- using Registration<PointSource, PointTarget, Scalar>::setInputSource;
using Registration<PointSource, PointTarget, Scalar>::input_;
using Registration<PointSource, PointTarget, Scalar>::indices_;
using Registration<PointSource, PointTarget, Scalar>::target_;
template<typename PointT> void
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
const typename pcl::search::KdTree<PointT>::Ptr kdtree,
- std::vector<Eigen::Matrix3d>& cloud_covariances)
+ MatricesVector& cloud_covariances)
{
if (k_correspondences_ > int (cloud->size ()))
{
cloud_covariances.resize (cloud->size ());
typename pcl::PointCloud<PointT>::const_iterator points_iterator = cloud->begin ();
- std::vector<Eigen::Matrix3d>::iterator matrices_iterator = cloud_covariances.begin ();
+ MatricesVector::iterator matrices_iterator = cloud_covariances.begin ();
for(;
points_iterator != cloud->end ();
++points_iterator, ++matrices_iterator)
// Set the mahalanobis matrices to identity
mahalanobis_.resize (N, Eigen::Matrix3d::Identity ());
// Compute target cloud covariance matrices
- if (target_covariances_.empty ())
- computeCovariances<PointTarget> (target_, tree_, target_covariances_);
+ if ((!target_covariances_) || (target_covariances_->empty ()))
+ {
+ target_covariances_.reset (new MatricesVector);
+ computeCovariances<PointTarget> (target_, tree_, *target_covariances_);
+ }
// Compute input cloud covariance matrices
- if (input_covariances_.empty ())
- computeCovariances<PointSource> (input_, tree_reciprocal_, input_covariances_);
+ if ((!input_covariances_) || (input_covariances_->empty ()))
+ {
+ input_covariances_.reset (new MatricesVector);
+ computeCovariances<PointSource> (input_, tree_reciprocal_, *input_covariances_);
+ }
base_transformation_ = guess;
nr_iterations_ = 0;
// Check if the distance to the nearest neighbor is smaller than the user imposed threshold
if (nn_dists[0] < dist_threshold)
{
- Eigen::Matrix3d &C1 = input_covariances_[i];
- Eigen::Matrix3d &C2 = target_covariances_[nn_indices[0]];
+ Eigen::Matrix3d &C1 = (*input_covariances_)[i];
+ Eigen::Matrix3d &C2 = (*target_covariances_)[nn_indices[0]];
Eigen::Matrix3d &M = mahalanobis_[i];
// M = R*C1
M = R * C1;
final_transformation_(0,3) = previous_transformation_(0,3) + guess(0,3);
final_transformation_(1,3) = previous_transformation_(1,3) + guess(1,3);
final_transformation_(2,3) = previous_transformation_(2,3) + guess(2,3);
+
+ // Transform the point cloud
+ pcl::transformPointCloud (*input_, output, final_transformation_);
}
template <typename PointSource, typename PointTarget> void
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ * Copyright (C) 2008 Ben Gurion University of the Negev, Beer Sheva, Israel.
+ *
+ * All rights reserved
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met
+ *
+ * * The use for research only (no for any commercial application).
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_REGISTRATION_IMPL_IA_FPCS_H_
+#define PCL_REGISTRATION_IMPL_IA_FPCS_H_
+
+#include <pcl/registration/ia_fpcs.h>
+#include <pcl/common/time.h>
+#include <pcl/common/distances.h>
+#include <pcl/sample_consensus/sac_model_plane.h>
+#include <pcl/registration/transformation_estimation_3point.h>
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> inline float
+pcl::getMeanPointDensity (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, float max_dist, int nr_threads)
+{
+ const float max_dist_sqr = max_dist * max_dist;
+ const std::size_t s = cloud.size ();
+
+ pcl::search::KdTree <PointT> tree;
+ tree.setInputCloud (cloud);
+
+ float mean_dist = 0.f;
+ int num = 0;
+ std::vector <int> ids (2);
+ std::vector <float> dists_sqr (2);
+
+#ifdef _OPENMP
+#pragma omp parallel for \
+ reduction (+:mean_dist, num) \
+ private (ids, dists_sqr) shared (tree, cloud) \
+ default (none)num_threads (nr_threads)
+#endif
+
+ for (int i = 0; i < 1000; i++)
+ {
+ tree.nearestKSearch (cloud->points[rand () % s], 2, ids, dists_sqr);
+ if (dists_sqr[1] < max_dist_sqr)
+ {
+ mean_dist += std::sqrt (dists_sqr[1]);
+ num++;
+ }
+ }
+
+ return (mean_dist / num);
+};
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> inline float
+pcl::getMeanPointDensity (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, const std::vector <int> &indices,
+ float max_dist, int nr_threads)
+{
+ const float max_dist_sqr = max_dist * max_dist;
+ const std::size_t s = indices.size ();
+
+ pcl::search::KdTree <PointT> tree;
+ tree.setInputCloud (cloud);
+
+ float mean_dist = 0.f;
+ int num = 0;
+ std::vector <int> ids (2);
+ std::vector <float> dists_sqr (2);
+
+#ifdef _OPENMP
+#pragma omp parallel for \
+ reduction (+:mean_dist, num) \
+ private (ids, dists_sqr) shared (tree, cloud, indices) \
+ default (none)num_threads (nr_threads)
+#endif
+
+ for (int i = 0; i < 1000; i++)
+ {
+ tree.nearestKSearch (cloud->points[indices[rand () % s]], 2, ids, dists_sqr);
+ if (dists_sqr[1] < max_dist_sqr)
+ {
+ mean_dist += std::sqrt (dists_sqr[1]);
+ num++;
+ }
+ }
+
+ return (mean_dist / num);
+};
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::FPCSInitialAlignment () :
+ source_normals_ (),
+ target_normals_ (),
+ nr_threads_ (1),
+ approx_overlap_ (0.5f),
+ delta_ (1.f),
+ score_threshold_ (FLT_MAX),
+ nr_samples_ (0),
+ max_norm_diff_ (90.f),
+ max_runtime_ (0),
+ fitness_score_ (FLT_MAX),
+ diameter_ (),
+ max_base_diameter_sqr_ (),
+ use_normals_ (false),
+ normalize_delta_ (true),
+ max_pair_diff_ (),
+ max_edge_diff_ (),
+ coincidation_limit_ (),
+ max_mse_ (),
+ max_inlier_dist_sqr_ (),
+ small_error_ (0.00001f)
+{
+ reg_name_ = "pcl::registration::FPCSInitialAlignment";
+ max_iterations_ = 0;
+ ransac_iterations_ = 1000;
+ transformation_estimation_.reset (new pcl::registration::TransformationEstimation3Point <PointSource, PointTarget>);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
+pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::computeTransformation (
+ PointCloudSource &output,
+ const Eigen::Matrix4f &guess)
+{
+ if (!initCompute ())
+ return;
+
+ final_transformation_ = guess;
+ bool abort = false;
+ std::vector <MatchingCandidates> all_candidates (max_iterations_);
+ pcl::StopWatch timer;
+
+ #ifdef _OPENMP
+ #pragma omp parallel num_threads (nr_threads_)
+ #endif
+ {
+ #ifdef _OPENMP
+ std::srand (static_cast <unsigned int> (std::time (NULL)) ^ omp_get_thread_num ());
+ #pragma omp for schedule (dynamic)
+ #endif
+ for (int i = 0; i < max_iterations_; i++)
+ {
+
+ #ifdef _OPENMP
+ #pragma omp flush (abort)
+ #endif
+
+ MatchingCandidates candidates (1);
+ std::vector <int> base_indices (4);
+ float ratio[2];
+ all_candidates[i] = candidates;
+
+ if (!abort)
+ {
+ // select four coplanar point base
+ if (selectBase (base_indices, ratio) == 0)
+ {
+ // calculate candidate pair correspondences using diagonal lenghts of base
+ pcl::Correspondences pairs_a, pairs_b;
+ if (bruteForceCorrespondences (base_indices[0], base_indices[1], pairs_a) == 0 &&
+ bruteForceCorrespondences (base_indices[2], base_indices[3], pairs_b) == 0)
+ {
+ // determine candidate matches by combining pair correspondences based on segment distances
+ std::vector <std::vector <int> > matches;
+ if (determineBaseMatches (base_indices, matches, pairs_a, pairs_b, ratio) == 0)
+ {
+ // check and evaluate candidate matches and store them
+ handleMatches (base_indices, matches, candidates);
+ if (candidates.size () != 0)
+ all_candidates[i] = candidates;
+ }
+ }
+ }
+
+ // check terminate early (time or fitness_score threshold reached)
+ abort = (candidates.size () > 0 ? candidates[0].fitness_score < score_threshold_ : abort);
+ abort = (abort ? abort : timer.getTimeSeconds () > max_runtime_);
+
+
+ #ifdef _OPENMP
+ #pragma omp flush (abort)
+ #endif
+ }
+ }
+ }
+
+
+ // determine best match over all trys
+ finalCompute (all_candidates);
+
+ // apply the final transformation
+ pcl::transformPointCloud (*input_, output, final_transformation_);
+
+ deinitCompute ();
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool
+pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::initCompute ()
+{
+ std::srand (static_cast <unsigned int> (std::time (NULL)));
+
+ // basic pcl initialization
+ if (!pcl::PCLBase <PointSource>::initCompute ())
+ return (false);
+
+ // check if source and target are given
+ if (!input_ || !target_)
+ {
+ PCL_ERROR ("[%s::initCompute] Source or target dataset not given!\n", reg_name_.c_str ());
+ return (false);
+ }
+
+ if (!target_indices_ || target_indices_->size () == 0)
+ {
+ target_indices_.reset (new std::vector <int> (static_cast <int> (target_->size ())));
+ int index = 0;
+ for (std::vector <int>::iterator it = target_indices_->begin (), it_e = target_indices_->end (); it != it_e; it++)
+ *it = index++;
+ target_cloud_updated_ = true;
+ }
+
+ // if a sample size for the point clouds is given; prefarably no sampling of target cloud
+ if (nr_samples_ != 0)
+ {
+ const int ss = static_cast <int> (indices_->size ());
+ const int sample_fraction_src = std::max (1, static_cast <int> (ss / nr_samples_));
+
+ source_indices_ = pcl::IndicesPtr (new std::vector <int>);
+ for (int i = 0; i < ss; i++)
+ if (rand () % sample_fraction_src == 0)
+ source_indices_->push_back ((*indices_) [i]);
+ }
+ else
+ source_indices_ = indices_;
+
+ // check usage of normals
+ if (source_normals_ && target_normals_ && source_normals_->size () == input_->size () && target_normals_->size () == target_->size ())
+ use_normals_ = true;
+
+ // set up tree structures
+ if (target_cloud_updated_)
+ {
+ tree_->setInputCloud (target_, target_indices_);
+ target_cloud_updated_ = false;
+ }
+
+ // set predefined variables
+ const int min_iterations = 4;
+ const float diameter_fraction = 0.3f;
+
+ // get diameter of input cloud (distance between farthest points)
+ Eigen::Vector4f pt_min, pt_max;
+ pcl::getMinMax3D (*target_, *target_indices_, pt_min, pt_max);
+ diameter_ = (pt_max - pt_min).norm ();
+
+ // derive the limits for the random base selection
+ float max_base_diameter = diameter_* approx_overlap_ * 2.f;
+ max_base_diameter_sqr_ = max_base_diameter * max_base_diameter;
+
+ // normalize the delta
+ if (normalize_delta_)
+ {
+ float mean_dist = getMeanPointDensity <PointTarget> (target_, *target_indices_, 0.05f * diameter_, nr_threads_);
+ delta_ *= mean_dist;
+ }
+
+ // heuristic determination of number of trials to have high probabilty of finding a good solution
+ if (max_iterations_ == 0)
+ {
+ float first_est = std::log (small_error_) / std::log (1.0 - std::pow ((double) approx_overlap_, (double) min_iterations));
+ max_iterations_ = static_cast <int> (first_est / (diameter_fraction * approx_overlap_ * 2.f));
+ }
+
+ // set further parameter
+ if (score_threshold_ == FLT_MAX)
+ score_threshold_ = 1.f - approx_overlap_;
+
+ if (max_iterations_ < 4)
+ max_iterations_ = 4;
+
+ if (max_runtime_ < 1)
+ max_runtime_ = INT_MAX;
+
+ // calculate internal parameters based on the the estimated point density
+ max_pair_diff_ = delta_ * 2.f;
+ max_edge_diff_ = delta_ * 4.f;
+ coincidation_limit_ = delta_ * 2.f; // EDITED: originally std::sqrt (delta_ * 2.f)
+ max_mse_ = powf (delta_* 2.f, 2.f);
+ max_inlier_dist_sqr_ = powf (delta_ * 2.f, 2.f);
+
+ // reset fitness_score
+ fitness_score_ = FLT_MAX;
+
+ return (true);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
+pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::selectBase (
+ std::vector <int> &base_indices,
+ float (&ratio)[2])
+{
+ const float too_close_sqr = max_base_diameter_sqr_*0.01;
+
+ Eigen::VectorXf coefficients (4);
+ pcl::SampleConsensusModelPlane <PointTarget> plane (target_);
+ plane.setIndices (target_indices_);
+ Eigen::Vector4f centre_pt;
+ float nearest_to_plane = FLT_MAX;
+
+ // repeat base search until valid quadruple was found or ransac_iterations_ number of trys were unsuccessfull
+ for (int i = 0; i < ransac_iterations_; i++)
+ {
+ // random select an appropriate point triple
+ if (selectBaseTriangle (base_indices) < 0)
+ continue;
+
+ std::vector <int> base_triple (base_indices.begin (), base_indices.end () - 1);
+ plane.computeModelCoefficients (base_triple, coefficients);
+ pcl::compute3DCentroid (*target_, base_triple, centre_pt);
+
+ // loop over all points in source cloud to find most suitable fourth point
+ const PointTarget *pt1 = &(target_->points[base_indices[0]]);
+ const PointTarget *pt2 = &(target_->points[base_indices[1]]);
+ const PointTarget *pt3 = &(target_->points[base_indices[2]]);
+
+ for (std::vector <int>::iterator it = target_indices_->begin (), it_e = target_indices_->end (); it != it_e; it++)
+ {
+ const PointTarget *pt4 = &(target_->points[*it]);
+
+ float d1 = pcl::squaredEuclideanDistance (*pt4, *pt1);
+ float d2 = pcl::squaredEuclideanDistance (*pt4, *pt2);
+ float d3 = pcl::squaredEuclideanDistance (*pt4, *pt3);
+ float d4 = (pt4->getVector3fMap () - centre_pt.head (3)).squaredNorm ();
+
+ // check distance between points w.r.t minimum sampling distance; EDITED -> 4th point now also limited by max base line
+ if (d1 < too_close_sqr || d2 < too_close_sqr || d3 < too_close_sqr || d4 < too_close_sqr ||
+ d1 > max_base_diameter_sqr_ || d2 > max_base_diameter_sqr_ || d3 > max_base_diameter_sqr_)
+ continue;
+
+ // check distance to plane to get point closest to plane
+ float dist_to_plane = pcl::pointToPlaneDistance (*pt4, coefficients);
+ if (dist_to_plane < nearest_to_plane)
+ {
+ base_indices[3] = *it;
+ nearest_to_plane = dist_to_plane;
+ }
+ }
+
+ // check if at least one point fullfilled the conditions
+ if (nearest_to_plane != FLT_MAX)
+ {
+ // order points to build largest quadrangle and calcuate intersection ratios of diagonals
+ setupBase (base_indices, ratio);
+ return (0);
+ }
+ }
+
+ // return unsuccessfull if no quadruple was selected
+ return (-1);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
+pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::selectBaseTriangle (std::vector <int> &base_indices)
+{
+ int nr_points = static_cast <int> (target_indices_->size ());
+ float best_t = 0.f;
+
+ // choose random first point
+ base_indices[0] = (*target_indices_)[rand () % nr_points];
+ int *index1 = &base_indices[0];
+
+ // random search for 2 other points (as far away as overlap allows)
+ for (int i = 0; i < ransac_iterations_; i++)
+ {
+ int *index2 = &(*target_indices_)[rand () % nr_points];
+ int *index3 = &(*target_indices_)[rand () % nr_points];
+
+ Eigen::Vector3f u = target_->points[*index2].getVector3fMap () - target_->points[*index1].getVector3fMap ();
+ Eigen::Vector3f v = target_->points[*index3].getVector3fMap () - target_->points[*index1].getVector3fMap ();
+ float t = u.cross (v).squaredNorm (); // triangle area (0.5 * sqrt(t)) should be maximal
+
+ // check for most suitable point triple
+ if (t > best_t && u.squaredNorm () < max_base_diameter_sqr_ && v.squaredNorm () < max_base_diameter_sqr_)
+ {
+ best_t = t;
+ base_indices[1] = *index2;
+ base_indices[2] = *index3;
+ }
+ }
+
+ // return if a triplet could be selected
+ return (best_t == 0.f ? -1 : 0);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
+pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::setupBase (
+ std::vector <int> &base_indices,
+ float (&ratio)[2])
+{
+ float best_t = FLT_MAX;
+ const std::vector <int> copy (base_indices.begin (), base_indices.end ());
+ std::vector <int> temp (base_indices.begin (), base_indices.end ());
+
+ // loop over all combinations of base points
+ for (std::vector <int>::const_iterator i = copy.begin (), i_e = copy.end (); i != i_e; i++)
+ for (std::vector <int>::const_iterator j = copy.begin (), j_e = copy.end (); j != j_e; j++)
+ {
+ if (i == j)
+ continue;
+
+ for (std::vector <int>::const_iterator k = copy.begin (), k_e = copy.end (); k != k_e; k++)
+ {
+ if (k == j || k == i)
+ continue;
+
+ std::vector <int>::const_iterator l = copy.begin ();
+ while (l == i || l == j || l == k)
+ l++;
+
+ temp[0] = *i;
+ temp[1] = *j;
+ temp[2] = *k;
+ temp[3] = *l;
+
+ // calculate diagonal intersection ratios and check for suitable segment to segment distances
+ float ratio_temp[2];
+ float t = segmentToSegmentDist (temp, ratio_temp);
+ if (t < best_t)
+ {
+ best_t = t;
+ ratio[0] = ratio_temp[0];
+ ratio[1] = ratio_temp[1];
+ base_indices = temp;
+ }
+ }
+ }
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> float
+pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::segmentToSegmentDist (
+ const std::vector <int> &base_indices,
+ float (&ratio)[2])
+{
+ // get point vectors
+ Eigen::Vector3f u = target_->points[base_indices[1]].getVector3fMap () - target_->points[base_indices[0]].getVector3fMap ();
+ Eigen::Vector3f v = target_->points[base_indices[3]].getVector3fMap () - target_->points[base_indices[2]].getVector3fMap ();
+ Eigen::Vector3f w = target_->points[base_indices[0]].getVector3fMap () - target_->points[base_indices[2]].getVector3fMap ();
+
+ // calculate segment distances
+ float a = u.dot (u);
+ float b = u.dot (v);
+ float c = v.dot (v);
+ float d = u.dot (w);
+ float e = v.dot (w);
+ float D = a * c - b * b;
+ float sN = 0.f, sD = D;
+ float tN = 0.f, tD = D;
+
+ // check segments
+ if (D < small_error_)
+ {
+ sN = 0.f;
+ sD = 1.f;
+ tN = e;
+ tD = c;
+ }
+ else
+ {
+ sN = (b * e - c * d);
+ tN = (a * e - b * d);
+
+ if (sN < 0.f)
+ {
+ sN = 0.f;
+ tN = e;
+ tD = c;
+ }
+ else if (sN > sD)
+ {
+ sN = sD;
+ tN = e + b;
+ tD = c;
+ }
+ }
+
+ if (tN < 0.f)
+ {
+ tN = 0.f;
+
+ if (-d < 0.f)
+ sN = 0.f;
+
+ else if (-d > a)
+ sN = sD;
+
+ else
+ {
+ sN = -d;
+ sD = a;
+ }
+ }
+
+ else if (tN > tD)
+ {
+ tN = tD;
+
+ if ((-d + b) < 0.f)
+ sN = 0.f;
+
+ else if ((-d + b) > a)
+ sN = sD;
+
+ else
+ {
+ sN = (-d + b);
+ sD = a;
+ }
+ }
+
+ // set intersection ratios
+ ratio[0] = (std::abs (sN) < small_error_) ? 0.f : sN / sD;
+ ratio[1] = (std::abs (tN) < small_error_) ? 0.f : tN / tD;
+
+ Eigen::Vector3f x = w + (ratio[0] * u) - (ratio[1] * v);
+ return (x.norm ());
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
+pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::bruteForceCorrespondences (
+ int idx1,
+ int idx2,
+ pcl::Correspondences &pairs)
+{
+ const float max_norm_diff = 0.5f * max_norm_diff_ * M_PI / 180.f;
+
+ // calculate reference segment distance and normal angle
+ float ref_dist = pcl::euclideanDistance (target_->points[idx1], target_->points[idx2]);
+ float ref_norm_angle = (use_normals_ ? (target_normals_->points[idx1].getNormalVector3fMap () -
+ target_normals_->points[idx2].getNormalVector3fMap ()).norm () : 0.f);
+
+ // loop over all pairs of points in source point cloud
+ std::vector <int>::iterator it_out = source_indices_->begin (), it_out_e = source_indices_->end () - 1;
+ std::vector <int>::iterator it_in, it_in_e = source_indices_->end ();
+ for ( ; it_out != it_out_e; it_out++)
+ {
+ it_in = it_out + 1;
+ const PointSource *pt1 = &(*input_)[*it_out];
+ for ( ; it_in != it_in_e; it_in++)
+ {
+ const PointSource *pt2 = &(*input_)[*it_in];
+
+ // check point distance compared to reference dist (from base)
+ float dist = pcl::euclideanDistance (*pt1, *pt2);
+ if (std::abs(dist - ref_dist) < max_pair_diff_)
+ {
+ // add here normal evaluation if normals are given
+ if (use_normals_)
+ {
+ const NormalT *pt1_n = &(source_normals_->points[*it_out]);
+ const NormalT *pt2_n = &(source_normals_->points[*it_in]);
+
+ float norm_angle_1 = (pt1_n->getNormalVector3fMap () - pt2_n->getNormalVector3fMap ()).norm ();
+ float norm_angle_2 = (pt1_n->getNormalVector3fMap () + pt2_n->getNormalVector3fMap ()).norm ();
+
+ float norm_diff = std::min <float> (std::abs (norm_angle_1 - ref_norm_angle), std::abs (norm_angle_2 - ref_norm_angle));
+ if (norm_diff > max_norm_diff)
+ continue;
+ }
+
+ pairs.push_back (pcl::Correspondence (*it_in, *it_out, dist));
+ pairs.push_back (pcl::Correspondence (*it_out, *it_in, dist));
+ }
+ }
+ }
+
+ // return success if at least one correspondence was found
+ return (pairs.size () == 0 ? -1 : 0);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
+pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::determineBaseMatches (
+ const std::vector <int> &base_indices,
+ std::vector <std::vector <int> > &matches,
+ const pcl::Correspondences &pairs_a,
+ const pcl::Correspondences &pairs_b,
+ const float (&ratio)[2])
+{
+ // calculate edge lengths of base
+ float dist_base[4];
+ dist_base[0] = pcl::euclideanDistance (target_->points[base_indices[0]], target_->points[base_indices[2]]);
+ dist_base[1] = pcl::euclideanDistance (target_->points[base_indices[0]], target_->points[base_indices[3]]);
+ dist_base[2] = pcl::euclideanDistance (target_->points[base_indices[1]], target_->points[base_indices[2]]);
+ dist_base[3] = pcl::euclideanDistance (target_->points[base_indices[1]], target_->points[base_indices[3]]);
+
+ // loop over first point pair correspondences and store intermediate points 'e' in new point cloud
+ PointCloudSourcePtr cloud_e (new PointCloudSource);
+ cloud_e->resize (pairs_a.size () * 2);
+ PointCloudSourceIterator it_pt = cloud_e->begin ();
+ for (pcl::Correspondences::const_iterator it_pair = pairs_a.begin (), it_pair_e = pairs_a.end () ; it_pair != it_pair_e; it_pair++)
+ {
+ const PointSource *pt1 = &(input_->points[it_pair->index_match]);
+ const PointSource *pt2 = &(input_->points[it_pair->index_query]);
+
+ // calculate intermediate points using both ratios from base (r1,r2)
+ for (int i = 0; i < 2; i++, it_pt++)
+ {
+ it_pt->x = pt1->x + ratio[i] * (pt2->x - pt1->x);
+ it_pt->y = pt1->y + ratio[i] * (pt2->y - pt1->y);
+ it_pt->z = pt1->z + ratio[i] * (pt2->z - pt1->z);
+ }
+ }
+
+ // initialize new kd tree of intermediate points from first point pair correspondences
+ KdTreeReciprocalPtr tree_e (new KdTreeReciprocal);
+ tree_e->setInputCloud (cloud_e);
+
+ std::vector <int> ids;
+ std::vector <float> dists_sqr;
+
+ // loop over second point pair correspondences
+ for (pcl::Correspondences::const_iterator it_pair = pairs_b.begin (), it_pair_e = pairs_b.end () ; it_pair != it_pair_e; it_pair++)
+ {
+ const PointTarget *pt1 = &(input_->points[it_pair->index_match]);
+ const PointTarget *pt2 = &(input_->points[it_pair->index_query]);
+
+ // calculate intermediate points using both ratios from base (r1,r2)
+ for (int i = 0; i < 2; i++)
+ {
+ PointTarget pt_e;
+ pt_e.x = pt1->x + ratio[i] * (pt2->x - pt1->x);
+ pt_e.y = pt1->y + ratio[i] * (pt2->y - pt1->y);
+ pt_e.z = pt1->z + ratio[i] * (pt2->z - pt1->z);
+
+ // search for corresponding intermediate points
+ tree_e->radiusSearch (pt_e, coincidation_limit_, ids, dists_sqr);
+ for (std::vector <int>::iterator it = ids.begin (), it_e = ids.end (); it != it_e; it++)
+ {
+ std::vector <int> match_indices (4);
+
+ match_indices[0] = pairs_a[static_cast <int> (std::floor ((float)(*it/2.f)))].index_match;
+ match_indices[1] = pairs_a[static_cast <int> (std::floor ((float)(*it/2.f)))].index_query;
+ match_indices[2] = it_pair->index_match;
+ match_indices[3] = it_pair->index_query;
+
+ // EDITED: added coarse check of match based on edge length (due to rigid-body )
+ if (checkBaseMatch (match_indices, dist_base) < 0)
+ continue;
+
+ matches.push_back (match_indices);
+ }
+ }
+ }
+
+ // return unsuccessfull if no match was found
+ return (matches.size () > 0 ? 0 : -1);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
+pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::checkBaseMatch (
+ const std::vector <int> &match_indices,
+ const float (&dist_ref)[4])
+{
+ float d0 = pcl::euclideanDistance (input_->points[match_indices[0]], input_->points[match_indices[2]]);
+ float d1 = pcl::euclideanDistance (input_->points[match_indices[0]], input_->points[match_indices[3]]);
+ float d2 = pcl::euclideanDistance (input_->points[match_indices[1]], input_->points[match_indices[2]]);
+ float d3 = pcl::euclideanDistance (input_->points[match_indices[1]], input_->points[match_indices[3]]);
+
+ // check edge distances of match w.r.t the base
+ return (std::abs (d0 - dist_ref[0]) < max_edge_diff_ && std::abs (d1 - dist_ref[1]) < max_edge_diff_ &&
+ std::abs (d2 - dist_ref[2]) < max_edge_diff_ && std::abs (d3 - dist_ref[3]) < max_edge_diff_) ? 0 : -1;
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
+pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::handleMatches (
+ const std::vector <int> &base_indices,
+ std::vector <std::vector <int> > &matches,
+ MatchingCandidates &candidates)
+{
+ candidates.resize (1);
+ float fitness_score = FLT_MAX;
+
+ // loop over all Candidate matches
+ for (std::vector <std::vector <int> >::iterator match_indices = matches.begin (), it_e = matches.end (); match_indices != it_e; match_indices++)
+ {
+ Eigen::Matrix4f transformation_temp;
+ pcl::Correspondences correspondences_temp;
+
+ // determine corresondences between base and match according to their distance to centroid
+ linkMatchWithBase (base_indices, *match_indices, correspondences_temp);
+
+ // check match based on residuals of the corresponding points after
+ if (validateMatch (base_indices, *match_indices, correspondences_temp, transformation_temp) < 0)
+ continue;
+
+ // check resulting using a sub sample of the source point cloud and compare to previous matches
+ if (validateTransformation (transformation_temp, fitness_score) < 0)
+ continue;
+
+ // store best match as well as associated fitness_score and transformation
+ candidates[0].fitness_score = fitness_score;
+ candidates [0].transformation = transformation_temp;
+ correspondences_temp.erase (correspondences_temp.end () - 1);
+ candidates[0].correspondences = correspondences_temp;
+ }
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
+pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::linkMatchWithBase (
+ const std::vector <int> &base_indices,
+ std::vector <int> &match_indices,
+ pcl::Correspondences &correspondences)
+{
+ // calculate centroid of base and target
+ Eigen::Vector4f centre_base, centre_match;
+ pcl::compute3DCentroid (*target_, base_indices, centre_base);
+ pcl::compute3DCentroid (*input_, match_indices, centre_match);
+
+ PointTarget centre_pt_base;
+ centre_pt_base.x = centre_base[0];
+ centre_pt_base.y = centre_base[1];
+ centre_pt_base.z = centre_base[2];
+
+ PointSource centre_pt_match;
+ centre_pt_match.x = centre_match[0];
+ centre_pt_match.y = centre_match[1];
+ centre_pt_match.z = centre_match[2];
+
+ // find corresponding points according to their distance to the centroid
+ std::vector <int> copy = match_indices;
+
+ std::vector <int>::const_iterator it_base = base_indices.begin (), it_base_e = base_indices.end ();
+ std::vector <int>::iterator it_match, it_match_e = copy.end ();
+ std::vector <int>::iterator it_match_orig = match_indices.begin ();
+ for (; it_base != it_base_e; it_base++, it_match_orig++)
+ {
+ float dist_sqr_1 = pcl::squaredEuclideanDistance (target_->points[*it_base], centre_pt_base);
+ float best_diff_sqr = FLT_MAX;
+ int best_index;
+
+ for (it_match = copy.begin (); it_match != it_match_e; it_match++)
+ {
+ // calculate difference of distances to centre point
+ float dist_sqr_2 = pcl::squaredEuclideanDistance (input_->points[*it_match], centre_pt_match);
+ float diff_sqr = std::abs(dist_sqr_1 - dist_sqr_2);
+
+ if (diff_sqr < best_diff_sqr)
+ {
+ best_diff_sqr = diff_sqr;
+ best_index = *it_match;
+ }
+ }
+
+ // assign new correspondence and update indices of matched targets
+ correspondences.push_back (pcl::Correspondence (best_index, *it_base, best_diff_sqr));
+ *it_match_orig = best_index;
+ }
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
+pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::validateMatch (
+ const std::vector <int> &base_indices,
+ const std::vector <int> &match_indices,
+ const pcl::Correspondences &correspondences,
+ Eigen::Matrix4f &transformation)
+{
+ // only use triplet of points to simlify process (possible due to planar case)
+ pcl::Correspondences correspondences_temp = correspondences;
+ correspondences_temp.erase (correspondences_temp.end () - 1);
+
+ // estimate transformation between correspondence set
+ transformation_estimation_->estimateRigidTransformation (*input_, *target_, correspondences_temp, transformation);
+
+ // transform base points
+ PointCloudSource match_transformed;
+ pcl::transformPointCloud (*input_, match_indices, match_transformed, transformation);
+
+ // calculate residuals of transformation and check against maximum threshold
+ std::size_t nr_points = correspondences_temp.size ();
+ float mse = 0.f;
+ for (std::size_t i = 0; i < nr_points; i++)
+ mse += pcl::squaredEuclideanDistance (match_transformed.points [i], target_->points [base_indices[i]]);
+
+ mse /= nr_points;
+ return (mse < max_mse_ ? 0 : -1);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
+pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::validateTransformation (
+ Eigen::Matrix4f &transformation,
+ float &fitness_score)
+{
+ // transform source point cloud
+ PointCloudSource source_transformed;
+ pcl::transformPointCloud (*input_, *source_indices_, source_transformed, transformation);
+
+ std::size_t nr_points = source_transformed.size ();
+ std::size_t terminate_value = fitness_score > 1 ? 0 : static_cast <std::size_t> ((1.f - fitness_score) * nr_points);
+
+ float inlier_score_temp = 0;
+ std::vector <int> ids;
+ std::vector <float> dists_sqr;
+ PointCloudSourceIterator it = source_transformed.begin ();
+
+ for (std::size_t i = 0; i < nr_points; it++, i++)
+ {
+ // search for nearest point using kd tree search
+ tree_->nearestKSearch (*it, 1, ids, dists_sqr);
+ inlier_score_temp += (dists_sqr[0] < max_inlier_dist_sqr_ ? 1 : 0);
+
+ // early terminating
+ if (nr_points - i + inlier_score_temp < terminate_value)
+ break;
+ }
+
+ // check current costs and return unsuccessfull if larger than previous ones
+ inlier_score_temp /= static_cast <float> (nr_points);
+ float fitness_score_temp = 1.f - inlier_score_temp;
+
+ if (fitness_score_temp > fitness_score)
+ return (-1);
+
+ fitness_score = fitness_score_temp;
+ return (0);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
+pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::finalCompute (
+ const std::vector <MatchingCandidates > &candidates)
+{
+ // get best fitness_score over all trys
+ int nr_candidates = static_cast <int> (candidates.size ());
+ int best_index = -1;
+ float best_score = FLT_MAX;
+ for (int i = 0; i < nr_candidates; i++)
+ {
+ const float &fitness_score = candidates [i][0].fitness_score;
+ if (fitness_score < best_score)
+ {
+ best_score = fitness_score;
+ best_index = i;
+ }
+ }
+
+ // check if a valid candidate was available
+ if (!(best_index < 0))
+ {
+ fitness_score_ = candidates [best_index][0].fitness_score;
+ final_transformation_ = candidates [best_index][0].transformation;
+ *correspondences_ = candidates [best_index][0].correspondences;
+
+ // here we define convergence if resulting fitness_score is below 1-threshold
+ converged_ = fitness_score_ < score_threshold_;
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+
+#endif // PCL_REGISTRATION_IMPL_IA_4PCS_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met
+ *
+ * * The use for research only (no for any commercial application).
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_REGISTRATION_IMPL_IA_KFPCS_H_
+#define PCL_REGISTRATION_IMPL_IA_KFPCS_H_
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
+pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::KFPCSInitialAlignment () :
+ lower_trl_boundary_ (-1.f),
+ upper_trl_boundary_ (-1.f),
+ lambda_ (0.5f),
+ candidates_ (),
+ use_trl_score_ (false),
+ indices_validation_ (new std::vector <int>)
+{
+ reg_name_ = "pcl::registration::KFPCSInitialAlignment";
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool
+pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::initCompute ()
+{
+ // due to sparse keypoint cloud, do not normalize delta with estimated point density
+ if (normalize_delta_)
+ {
+ PCL_WARN ("[%s::initCompute] Delta should be set according to keypoint precision! Normalization according to point cloud density is ignored.", reg_name_.c_str ());
+ normalize_delta_ = false;
+ }
+
+ // initialize as in fpcs
+ pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::initCompute ();
+
+ // set the threshold values with respect to keypoint charactersitics
+ max_pair_diff_ = delta_ * 1.414f; // diff between 2 points of delta_ accuracy
+ coincidation_limit_ = delta_ * 2.828f; // diff between diff of 2 points
+ max_edge_diff_ = delta_ * 3.f; // diff between 2 points + some inaccuracy due to quadruple orientation
+ max_mse_ = powf (delta_ * 4.f, 2.f); // diff between 2 points + some registration inaccuracy
+ max_inlier_dist_sqr_ = powf (delta_ * 8.f, 2.f); // set rel. high, because MSAC is used (residual based score function)
+
+ // check use of translation costs and calculate upper boundary if not set by user
+ if (upper_trl_boundary_ < 0)
+ upper_trl_boundary_ = diameter_ * (1.f - approx_overlap_) * 0.5f;
+
+ if (!(lower_trl_boundary_ < 0) && upper_trl_boundary_ > lower_trl_boundary_)
+ use_trl_score_ = true;
+ else
+ lambda_ = 0.f;
+
+ // generate a subset of indices of size ransac_iterations_ on which to evaluate candidates on
+ std::size_t nr_indices = indices_->size ();
+ if (nr_indices < ransac_iterations_)
+ indices_validation_ = indices_;
+ else
+ for (int i = 0; i < ransac_iterations_; i++)
+ indices_validation_->push_back ((*indices_)[rand () % nr_indices]);
+
+ return (true);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
+pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::handleMatches (
+ const std::vector <int> &base_indices,
+ std::vector <std::vector <int> > &matches,
+ MatchingCandidates &candidates)
+{
+ candidates.clear ();
+ float fitness_score = FLT_MAX;
+
+ // loop over all Candidate matches
+ for (std::vector <std::vector <int> >::iterator match_indices = matches.begin (), it_e = matches.end (); match_indices != it_e; match_indices++)
+ {
+ Eigen::Matrix4f transformation_temp;
+ pcl::Correspondences correspondences_temp;
+ fitness_score = FLT_MAX; // reset to FLT_MAX to accept all candidates and not only best
+
+ // determine corresondences between base and match according to their distance to centroid
+ linkMatchWithBase (base_indices, *match_indices, correspondences_temp);
+
+ // check match based on residuals of the corresponding points after transformation
+ if (validateMatch (base_indices, *match_indices, correspondences_temp, transformation_temp) < 0)
+ continue;
+
+ // check resulting transformation using a sub sample of the source point cloud
+ // all candidates are stored and later sorted according to their fitness score
+ validateTransformation (transformation_temp, fitness_score);
+
+ // store all valid match as well as associated score and transformation
+ candidates.push_back (MatchingCandidate (fitness_score, correspondences_temp, transformation_temp));
+ }
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
+pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::validateTransformation (
+ Eigen::Matrix4f &transformation,
+ float &fitness_score)
+{
+ // transform sub sampled source cloud
+ PointCloudSource source_transformed;
+ pcl::transformPointCloud (*input_, *indices_validation_, source_transformed, transformation);
+
+ const std::size_t nr_points = source_transformed.size ();
+ float score_a = 0.f, score_b = 0.f;
+
+ // residual costs based on mse
+ std::vector <int> ids;
+ std::vector <float> dists_sqr;
+ for (PointCloudSourceIterator it = source_transformed.begin (), it_e = source_transformed.end (); it != it_e; ++it)
+ {
+ // search for nearest point using kd tree search
+ tree_->nearestKSearch (*it, 1, ids, dists_sqr);
+ score_a += (dists_sqr[0] < max_inlier_dist_sqr_ ? dists_sqr[0] : max_inlier_dist_sqr_); // MSAC
+ }
+
+ score_a /= (max_inlier_dist_sqr_ * nr_points); // MSAC
+ //score_a = 1.f - (1.f - score_a) / (1.f - approx_overlap_); // make score relative to estimated overlap
+
+ // translation score (solutions with small translation are down-voted)
+ float scale = 1.f;
+ if (use_trl_score_)
+ {
+ float trl = transformation.rightCols <1> ().head (3).norm ();
+ float trl_ratio = (trl - lower_trl_boundary_) / (upper_trl_boundary_ - lower_trl_boundary_);
+
+ score_b = (trl_ratio < 0.f ? 1.f : (trl_ratio > 1.f ? 0.f : 0.5f * sin (M_PI * trl_ratio + M_PI_2) + 0.5f)); // sinusoidal costs
+ scale += lambda_;
+ }
+
+ // calculate the fitness and return unsuccessfull if smaller than previous ones
+ float fitness_score_temp = (score_a + lambda_ * score_b) / scale;
+ if (fitness_score_temp > fitness_score)
+ return (-1);
+
+ fitness_score = fitness_score_temp;
+ return (0);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
+pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::finalCompute (
+ const std::vector <MatchingCandidates > &candidates)
+{
+ // reorganize candidates into single vector
+ size_t total_size = 0;
+ std::vector <MatchingCandidates>::const_iterator it, it_e = candidates.end ();
+ for (it = candidates.begin (); it != it_e; it++)
+ total_size += it->size ();
+
+ candidates_.clear ();
+ candidates_.reserve (total_size);
+
+ MatchingCandidates::const_iterator it_curr, it_curr_e;
+ for (it = candidates.begin (); it != it_e; it++)
+ for (it_curr = it->begin (), it_curr_e = it->end (); it_curr != it_curr_e; it_curr++)
+ candidates_.push_back (*it_curr);
+
+ // sort acoording to score value
+ std::sort (candidates_.begin (), candidates_.end (), by_score ());
+
+ // return here if no score was valid, i.e. all scores are FLT_MAX
+ if (candidates_[0].fitness_score == FLT_MAX)
+ {
+ converged_ = false;
+ return;
+ }
+
+ // save best candidate as output result
+ // note, all other candidates are accessible via getNBestCandidates () and getTBestCandidates ()
+ fitness_score_ = candidates_ [0].fitness_score;
+ final_transformation_ = candidates_ [0].transformation;
+ *correspondences_ = candidates_ [0].correspondences;
+
+ // here we define convergence if resulting score is above threshold
+ converged_ = fitness_score_ < score_threshold_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
+pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::getNBestCandidates (
+ int n,
+ float min_angle3d,
+ float min_translation3d,
+ MatchingCandidates &candidates)
+{
+ candidates.clear ();
+
+ // loop over all candidates starting from the best one
+ for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; it_candidate++)
+ {
+ // stop if current candidate has no valid score
+ if (it_candidate->fitness_score == FLT_MAX)
+ return;
+
+ // check if current candidate is a unique one compared to previous using the min_diff threshold
+ bool unique = true;
+ MatchingCandidates::iterator it = candidates.begin (), it_e2 = candidates.end ();
+ while (unique && it != it_e2)
+ {
+ Eigen::Matrix4f diff = it_candidate->transformation.colPivHouseholderQr ().solve (it->transformation);
+ const float angle3d = Eigen::AngleAxisf (diff.block <3, 3> (0, 0)).angle ();
+ const float translation3d = diff.block <3, 1> (0, 3).norm ();
+ unique = angle3d > min_angle3d && translation3d > min_translation3d;
+ it++;
+ }
+
+ // add candidate to best candidates
+ if (unique)
+ candidates.push_back (*it_candidate);
+
+ // stop if n candidates are reached
+ if (candidates.size () == n)
+ return;
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
+pcl::registration::KFPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::getTBestCandidates (
+ float t,
+ float min_angle3d,
+ float min_translation3d,
+ MatchingCandidates &candidates)
+{
+ candidates.clear ();
+
+ // loop over all candidates starting from the best one
+ for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; it_candidate++)
+ {
+ // stop if current candidate has score below threshold
+ if (it_candidate->fitness_score > t)
+ return;
+
+ // check if current candidate is a unique one compared to previous using the min_diff threshold
+ bool unique = true;
+ MatchingCandidates::iterator it = candidates.begin (), it_e2 = candidates.end ();
+ while (unique && it != it_e2)
+ {
+ Eigen::Matrix4f diff = it_candidate->transformation.colPivHouseholderQr ().solve (it->transformation);
+ const float angle3d = Eigen::AngleAxisf (diff.block <3, 3> (0, 0)).angle ();
+ const float translation3d = diff.block <3, 1> (0, 3).norm ();
+ unique = angle3d > min_angle3d && translation3d > min_translation3d;
+ it++;
+ }
+
+ // add candidate to best candidates
+ if (unique)
+ candidates.push_back (*it_candidate);
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+
+#endif // PCL_REGISTRATION_IMPL_IA_KFPCS_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2015, Michael 'v4hn' Goerner
+ * Copyright (c) 2015-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef PCL_REGISTRATION_IMPL_INCREMENTAL_REGISTRATION_HPP_
+#define PCL_REGISTRATION_IMPL_INCREMENTAL_REGISTRATION_HPP_
+
+template <typename PointT, typename Scalar>
+pcl::registration::IncrementalRegistration<PointT, Scalar>::IncrementalRegistration () :
+ delta_transform_ (Matrix4::Identity ()),
+ abs_transform_ (Matrix4::Identity ())
+{}
+
+template <typename PointT, typename Scalar> bool
+pcl::registration::IncrementalRegistration<PointT, Scalar>::registerCloud (const PointCloudConstPtr& cloud, const Matrix4& delta_estimate)
+{
+ assert (registration_);
+
+ if (!last_cloud_)
+ {
+ last_cloud_ = cloud;
+ abs_transform_ = delta_transform_ = delta_estimate;
+ return (true);
+ }
+
+ registration_->setInputSource (cloud);
+ registration_->setInputTarget (last_cloud_);
+
+ {
+ pcl::PointCloud<PointT> p;
+ registration_->align (p, delta_estimate);
+ }
+
+ bool converged = registration_->hasConverged ();
+
+ if ( converged ){
+ delta_transform_ = registration_->getFinalTransformation ();
+ abs_transform_ = abs_transform_ * delta_transform_;
+ last_cloud_ = cloud;
+ }
+
+ return (converged);
+}
+
+template <typename PointT, typename Scalar> inline typename pcl::registration::IncrementalRegistration<PointT, Scalar>::Matrix4
+pcl::registration::IncrementalRegistration<PointT, Scalar>::getDeltaTransform () const
+{
+ return (delta_transform_);
+}
+
+template <typename PointT, typename Scalar> inline typename pcl::registration::IncrementalRegistration<PointT, Scalar>::Matrix4
+pcl::registration::IncrementalRegistration<PointT, Scalar>::getAbsoluteTransform () const
+{
+ return (abs_transform_);
+}
+
+template <typename PointT, typename Scalar> inline void
+pcl::registration::IncrementalRegistration<PointT, Scalar>::reset ()
+{
+ last_cloud_.reset ();
+ delta_transform_ = abs_transform_ = Matrix4::Identity ();
+}
+
+template <typename PointT, typename Scalar> inline void
+pcl::registration::IncrementalRegistration<PointT, Scalar>::setRegistration (RegistrationPtr registration)
+{
+ registration_ = registration;
+}
+
+#endif /*PCL_REGISTRATION_IMPL_INCREMENTAL_REGISTRATION_HPP_*/
pcl::CorrespondencesPtr corrs = (*slam_graph_)[e].corrs_;
// Build the average and difference vectors for all correspondences
- std::vector <Eigen::Vector3f> corrs_aver (corrs->size ());
- std::vector <Eigen::Vector3f> corrs_diff (corrs->size ());
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > corrs_aver (corrs->size ());
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > corrs_diff (corrs->size ());
int oci = 0; // oci = output correspondence iterator
for (int ici = 0; ici != static_cast<int> (corrs->size ()); ++ici) // ici = input correspondence iterator
{
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2015, Michael 'v4hn' Goerner
+ * Copyright (c) 2015-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef PCL_REGISTRATION_IMPL_META_REGISTRATION_HPP_
+#define PCL_REGISTRATION_IMPL_META_REGISTRATION_HPP_
+
+template <typename PointT, typename Scalar>
+pcl::registration::MetaRegistration<PointT, Scalar>::MetaRegistration () :
+ abs_transform_ (Matrix4::Identity ())
+{}
+
+template <typename PointT, typename Scalar> bool
+pcl::registration::MetaRegistration<PointT, Scalar>::registerCloud (const PointCloudConstPtr& new_cloud, const Matrix4& delta_estimate)
+{
+ assert (registration_);
+
+ PointCloudPtr new_cloud_transformed (new pcl::PointCloud<PointT> ());
+
+ if (!full_cloud_)
+ {
+ pcl::transformPointCloud(*new_cloud, *new_cloud_transformed, delta_estimate);
+ full_cloud_ = new_cloud_transformed;
+ abs_transform_ = delta_estimate;
+ return (true);
+ }
+
+ registration_->setInputSource (new_cloud);
+ registration_->setInputTarget (full_cloud_);
+
+ registration_->align (*new_cloud_transformed, abs_transform_ * delta_estimate);
+
+ bool converged = registration_->hasConverged ();
+
+ if (converged)
+ {
+ abs_transform_ = registration_->getFinalTransformation ();
+ *full_cloud_ += *new_cloud_transformed;
+ }
+
+ return (converged);
+}
+
+template <typename PointT, typename Scalar> inline typename pcl::registration::MetaRegistration<PointT, Scalar>::Matrix4
+pcl::registration::MetaRegistration<PointT, Scalar>::getAbsoluteTransform () const
+{
+ return (abs_transform_);
+}
+
+template <typename PointT, typename Scalar> inline void
+pcl::registration::MetaRegistration<PointT, Scalar>::reset ()
+{
+ full_cloud_.reset ();
+ abs_transform_ = Matrix4::Identity ();
+}
+
+template <typename PointT, typename Scalar> inline void
+pcl::registration::MetaRegistration<PointT, Scalar>::setRegistration (RegistrationPtr reg)
+{
+ registration_ = reg;
+}
+
+template <typename PointT, typename Scalar> inline typename pcl::registration::MetaRegistration<PointT, Scalar>::PointCloudConstPtr
+pcl::registration::MetaRegistration<PointT, Scalar>::getMetaCloud () const
+{
+ return full_cloud_;
+}
+#endif /*PCL_REGISTRATION_IMPL_META_REGISTRATION_HPP_*/
#include <pcl/common/transforms.h>
#include <pcl/features/pfh.h>
-//////////////////////////////////////////////////////////////////////////////////////////////
-void
-pcl::PPFHashMapSearch::setInputFeatureCloud (PointCloud<PPFSignature>::ConstPtr feature_cloud)
-{
- // Discretize the feature cloud and insert it in the hash map
- feature_hash_map_->clear ();
- unsigned int n = static_cast<unsigned int> (sqrt (static_cast<float> (feature_cloud->points.size ())));
- int d1, d2, d3, d4;
- max_dist_ = -1.0;
- alpha_m_.resize (n);
- for (size_t i = 0; i < n; ++i)
- {
- std::vector <float> alpha_m_row (n);
- for (size_t j = 0; j < n; ++j)
- {
- d1 = static_cast<int> (floor (feature_cloud->points[i*n+j].f1 / angle_discretization_step_));
- d2 = static_cast<int> (floor (feature_cloud->points[i*n+j].f2 / angle_discretization_step_));
- d3 = static_cast<int> (floor (feature_cloud->points[i*n+j].f3 / angle_discretization_step_));
- d4 = static_cast<int> (floor (feature_cloud->points[i*n+j].f4 / distance_discretization_step_));
- feature_hash_map_->insert (std::pair<HashKeyStruct, std::pair<size_t, size_t> > (HashKeyStruct (d1, d2, d3, d4), std::pair<size_t, size_t> (i, j)));
- alpha_m_row [j] = feature_cloud->points[i*n + j].alpha_m;
-
- if (max_dist_ < feature_cloud->points[i*n + j].f4)
- max_dist_ = feature_cloud->points[i*n + j].f4;
- }
- alpha_m_[i] = alpha_m_row;
- }
-
- internals_initialized_ = true;
-}
-
-
-//////////////////////////////////////////////////////////////////////////////////////////////
-void
-pcl::PPFHashMapSearch::nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4,
- std::vector<std::pair<size_t, size_t> > &indices)
-{
- if (!internals_initialized_)
- {
- PCL_ERROR("[pcl::PPFRegistration::nearestNeighborSearch]: input feature cloud has not been set - skipping search!\n");
- return;
- }
-
- int d1 = static_cast<int> (floor (f1 / angle_discretization_step_)),
- d2 = static_cast<int> (floor (f2 / angle_discretization_step_)),
- d3 = static_cast<int> (floor (f3 / angle_discretization_step_)),
- d4 = static_cast<int> (floor (f4 / distance_discretization_step_));
-
- indices.clear ();
- HashKeyStruct key = HashKeyStruct (d1, d2, d3, d4);
- std::pair <FeatureHashMapType::iterator, FeatureHashMapType::iterator> map_iterator_pair = feature_hash_map_->equal_range (key);
- for (; map_iterator_pair.first != map_iterator_pair.second; ++ map_iterator_pair.first)
- indices.push_back (std::pair<size_t, size_t> (map_iterator_pair.first->second.first,
- map_iterator_pair.first->second.second));
-}
-
-
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget> void
pcl::PPFRegistration<PointSource, PointTarget>::setInputTarget (const PointCloudTargetConstPtr &cloud)
Eigen::Affine3f &pose2)
{
float position_diff = (pose1.translation () - pose2.translation ()).norm ();
- Eigen::AngleAxisf rotation_diff_mat (pose1.rotation ().inverse () * pose2.rotation ());
+ Eigen::AngleAxisf rotation_diff_mat ((pose1.rotation ().inverse ().lazyProduct (pose2.rotation ()).eval()));
float rotation_diff_angle = fabsf (rotation_diff_mat.angle ());
{
getFitness (inliers, error);
inlier_fraction = static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ());
- error /= static_cast<float> (inliers.size ());
if (inlier_fraction >= inlier_fraction_ && error < lowest_error)
{
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+#ifndef PCL_REGISTRATION_IMPL_TRANSFORMATION_ESTIMATION_3POINT_H_
+#define PCL_REGISTRATION_IMPL_TRANSFORMATION_ESTIMATION_3POINT_H_
+
+#include <pcl/common/eigen.h>
+#include <pcl/registration/transformation_estimation_3point.h>
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename Scalar> inline void
+pcl::registration::TransformationEstimation3Point<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+ const pcl::PointCloud<PointSource> &cloud_src,
+ const pcl::PointCloud<PointTarget> &cloud_tgt,
+ Matrix4 &transformation_matrix) const
+{
+ if (cloud_src.points.size () != 3 || cloud_tgt.points.size () != 3)
+ {
+ PCL_ERROR ("[pcl::TransformationEstimation3Point::estimateRigidTransformation] Number of points in source (%lu) and target (%lu) must be 3!\n",
+ cloud_src.points.size (), cloud_tgt.points.size ());
+ return;
+ }
+
+ ConstCloudIterator<PointSource> source_it (cloud_src);
+ ConstCloudIterator<PointTarget> target_it (cloud_tgt);
+ estimateRigidTransformation (source_it, target_it, transformation_matrix);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename Scalar> void
+pcl::registration::TransformationEstimation3Point<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+ const pcl::PointCloud<PointSource> &cloud_src,
+ const std::vector<int> &indices_src,
+ const pcl::PointCloud<PointTarget> &cloud_tgt,
+ Matrix4 &transformation_matrix) const
+{
+ if (indices_src.size () != 3 || cloud_tgt.points.size () != 3)
+ {
+ PCL_ERROR ("[pcl::TransformationEstimation3Point::estimateRigidTransformation] Number of indices in source (%lu) and points in target (%lu) must be 3!\n",
+ indices_src.size (), cloud_tgt.points.size ());
+ return;
+ }
+
+ ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
+ ConstCloudIterator<PointTarget> target_it (cloud_tgt);
+ estimateRigidTransformation (source_it, target_it, transformation_matrix);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename Scalar> inline void
+pcl::registration::TransformationEstimation3Point<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+ const pcl::PointCloud<PointSource> &cloud_src,
+ const std::vector<int> &indices_src,
+ const pcl::PointCloud<PointTarget> &cloud_tgt,
+ const std::vector<int> &indices_tgt,
+ Matrix4 &transformation_matrix) const
+{
+ if (indices_src.size () != 3 || indices_tgt.size () != 3)
+ {
+ PCL_ERROR ("[pcl::TransformationEstimation3Point::estimateRigidTransformation] Number of indices in source (%lu) and target (%lu) must be 3!\n",
+ indices_src.size (), indices_tgt.size ());
+ return;
+ }
+
+ ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
+ ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
+ estimateRigidTransformation (source_it, target_it, transformation_matrix);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename Scalar> void
+pcl::registration::TransformationEstimation3Point<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+ const pcl::PointCloud<PointSource> &cloud_src,
+ const pcl::PointCloud<PointTarget> &cloud_tgt,
+ const pcl::Correspondences &correspondences,
+ Matrix4 &transformation_matrix) const
+{
+ if (correspondences.size () != 3)
+ {
+ PCL_ERROR ("[pcl::TransformationEstimation3Point::estimateRigidTransformation] Number of correspondences (%lu) must be 3!\n",
+ correspondences.size ());
+ return;
+ }
+
+ ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
+ ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
+ estimateRigidTransformation (source_it, target_it, transformation_matrix);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget, typename Scalar> inline void
+pcl::registration::TransformationEstimation3Point<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
+ ConstCloudIterator<PointSource>& source_it,
+ ConstCloudIterator<PointTarget>& target_it,
+ Matrix4 &transformation_matrix) const
+{
+ transformation_matrix.setIdentity ();
+ source_it.reset ();
+ target_it.reset ();
+
+ Eigen::Matrix <Scalar, 4, 1> source_mean, target_mean;
+ pcl::compute3DCentroid (source_it, source_mean);
+ pcl::compute3DCentroid (target_it, target_mean);
+
+ source_it.reset ();
+ target_it.reset ();
+
+ Eigen::Matrix <Scalar, Eigen::Dynamic, Eigen::Dynamic> source_demean, target_demean;
+ pcl::demeanPointCloud (source_it, source_mean, source_demean, 3);
+ pcl::demeanPointCloud (target_it, target_mean, target_demean, 3);
+
+ source_it.reset ();
+ target_it.reset ();
+
+ Eigen::Matrix <Scalar, 3, 1> s1 = source_demean.col (1).head (3) - source_demean.col (0).head (3);
+ s1.normalize ();
+
+ Eigen::Matrix <Scalar, 3, 1> s2 = source_demean.col (2).head (3) - source_demean.col (0).head (3);
+ s2 -= s2.dot (s1) * s1;
+ s2.normalize ();
+
+ Eigen::Matrix <Scalar, 3, 3> source_rot;
+ source_rot.col (0) = s1;
+ source_rot.col (1) = s2;
+ source_rot.col (2) = s1.cross (s2);
+
+ Eigen::Matrix <Scalar, 3, 1> t1 = target_demean.col (1).head (3) - target_demean.col (0).head (3);
+ t1.normalize ();
+
+ Eigen::Matrix <Scalar, 3, 1> t2 = target_demean.col (2).head (3) - target_demean.col (0).head (3);
+ t2 -= t2.dot (t1) * t1;
+ t2.normalize ();
+
+ Eigen::Matrix <Scalar, 3, 3> target_rot;
+ target_rot.col (0) = t1;
+ target_rot.col (1) = t2;
+ target_rot.col (2) = t1.cross (t2);
+
+ //Eigen::Matrix <Scalar, 3, 3> R = source_rot * target_rot.transpose ();
+ Eigen::Matrix <Scalar, 3, 3> R = target_rot * source_rot.transpose ();
+ transformation_matrix.topLeftCorner (3, 3) = R;
+ //transformation_matrix.block (0, 3, 3, 1) = source_mean.head (3) - R * target_mean.head (3);
+ transformation_matrix.block (0, 3, 3, 1) = target_mean.head (3) - R * source_mean.head (3);
+}
+
+//#define PCL_INSTANTIATE_TransformationEstimation3Point(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimation3Point<T,U>;
+
+#endif // PCL_REGISTRATION_IMPL_TRANSFORMATION_ESTIMATION_3POINT_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2015, Michael 'v4hn' Goerner
+ * Copyright (c) 2015-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef PCL_REGISTRATION_INCREMENTAL_REGISTRATION_H_
+#define PCL_REGISTRATION_INCREMENTAL_REGISTRATION_H_
+
+#include <pcl/point_cloud.h>
+#include <pcl/registration/registration.h>
+
+namespace pcl {
+ namespace registration {
+
+ /** \brief Incremental @ref IterativeClosestPoint class
+ *
+ * This class provides a way to register a stream of clouds where each cloud will be aligned to the previous cloud.
+ *
+ * \code
+ * IterativeClosestPoint<PointXYZ,PointXYZ>::Ptr icp (new IterativeClosestPoint<PointXYZ,PointXYZ>);
+ * icp->setMaxCorrespondenceDistance (0.05);
+ * icp->setMaximumIterations (50);
+ *
+ * IncrementalRegistration<PointXYZ> iicp;
+ * iicp.setRegistration (icp);
+ *
+ * while (true){
+ * PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
+ * read_cloud (*cloud);
+ * iicp.registerCloud (cloud);
+ *
+ * PointCloud<PointXYZ>::Ptr tmp (new PointCloud<PointXYZ>);
+ * transformPointCloud (*cloud, *tmp, iicp.getAbsoluteTransform ());
+ * write_cloud (*tmp);
+ * }
+ * \endcode
+ *
+ * \author Michael 'v4hn' Goerner
+ * \ingroup registration
+ */
+ template <typename PointT, typename Scalar = float>
+ class IncrementalRegistration {
+ public:
+ typedef typename pcl::PointCloud<PointT>::Ptr PointCloudPtr;
+ typedef typename pcl::PointCloud<PointT>::ConstPtr PointCloudConstPtr;
+
+ typedef typename pcl::Registration<PointT,PointT,Scalar>::Ptr RegistrationPtr;
+ typedef typename pcl::Registration<PointT,PointT,Scalar>::Matrix4 Matrix4;
+
+ IncrementalRegistration ();
+
+ /** \brief Empty destructor */
+ virtual ~IncrementalRegistration () {}
+
+ /** \brief Register new point cloud incrementally
+ * \note You have to set a valid registration object with @ref setRegistration before using this
+ * \note The class doesn't copy cloud. If you afterwards change cloud, that will affect this class.
+ * \param[in] cloud point cloud to register
+ * \param[in] delta_estimate estimated transform between last registered cloud and this one
+ * \return true if registration converged
+ */
+ bool
+ registerCloud (const PointCloudConstPtr& cloud, const Matrix4& delta_estimate = Matrix4::Identity ());
+
+ /** \brief Get estimated transform between the last two registered clouds */
+ inline Matrix4
+ getDeltaTransform () const;
+
+ /** \brief Get estimated overall transform */
+ inline Matrix4
+ getAbsoluteTransform () const;
+
+ /** \brief Reset incremental Registration without resetting registration_ */
+ inline void
+ reset ();
+
+ /** \brief Set registration instance used to align clouds */
+ inline void
+ setRegistration (RegistrationPtr);
+ protected:
+
+ /** \brief last registered point cloud */
+ PointCloudConstPtr last_cloud_;
+
+ /** \brief registration instance to align clouds */
+ RegistrationPtr registration_;
+
+ /** \brief estimated transforms */
+ Matrix4 delta_transform_;
+ Matrix4 abs_transform_;
+ };
+
+ }
+}
+
+#include <pcl/registration/impl/incremental_registration.hpp>
+
+#endif /*PCL_REGISTRATION_INCREMENTAL_REGISTRATION_H_*/
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ * Copyright (C) 2008 Ben Gurion University of the Negev, Beer Sheva, Israel.
+ *
+ * All rights reserved
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met
+ *
+ * * The use for research only (no for any commercial application).
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_REGISTRATION_MATCHING_CANDIDATE_H_
+#define PCL_REGISTRATION_MATCHING_CANDIDATE_H_
+
+#include <pcl/registration/registration.h>
+#include <pcl/common/common.h>
+
+namespace pcl
+{
+ namespace registration
+ {
+ /** \brief Container for matching candidate consisting of
+ *
+ * * fitness score value as a result of the matching algorithm
+ * * correspondences between source and target data set
+ * * transformation matrix calculated based on the correspondences
+ *
+ */
+ struct MatchingCandidate
+ {
+ /** \brief Constructor. */
+ MatchingCandidate () :
+ fitness_score (FLT_MAX),
+ correspondences (),
+ transformation (Eigen::Matrix4f::Identity ())
+ {};
+
+ /** \brief Value constructor. */
+ MatchingCandidate (float s, const pcl::Correspondences &c, const Eigen::Matrix4f &m) :
+ fitness_score (s),
+ correspondences (c),
+ transformation (m)
+ {};
+
+ /** \brief Destructor. */
+ ~MatchingCandidate ()
+ {};
+
+
+ /** \brief Fitness score of current candidate resulting from matching algorithm. */
+ float fitness_score;
+
+ /** \brief Correspondences between source <-> target. */
+ pcl::Correspondences correspondences;
+
+ /** \brief Corresponding transformation matrix retrieved using \a corrs. */
+ Eigen::Matrix4f transformation;
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+
+ typedef std::vector<MatchingCandidate, Eigen::aligned_allocator<MatchingCandidate> > MatchingCandidates;
+
+ /** \brief Sorting of candidates based on fitness score value. */
+ struct by_score
+ {
+ /** \brief Operator used to sort candidates based on fitness score. */
+ bool operator () (MatchingCandidate const &left, MatchingCandidate const &right)
+ {
+ return (left.fitness_score < right.fitness_score);
+ }
+ };
+
+ }; // namespace registration
+}; // namespace pcl
+
+
+#endif // PCL_REGISTRATION_MATCHING_CANDIDATE_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2015, Michael 'v4hn' Goerner
+ * Copyright (c) 2015-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef PCL_REGISTRATION_META_REGISTRATION_H_
+#define PCL_REGISTRATION_META_REGISTRATION_H_
+
+#include <pcl/point_cloud.h>
+#include <pcl/registration/registration.h>
+
+namespace pcl {
+ namespace registration {
+
+ /** \brief Meta @ref Registration class
+ * \note This method accumulates all registered points and becomes more costly with each registered point cloud.
+ *
+ * This class provides a way to register a stream of clouds where each cloud
+ * will be aligned to the conglomerate of all previous clouds.
+ *
+ * \code
+ * IterativeClosestPoint<PointXYZ,PointXYZ>::Ptr icp (new IterativeClosestPoint<PointXYZ,PointXYZ>);
+ * icp->setMaxCorrespondenceDistance (0.05);
+ * icp->setMaximumIterations (50);
+ *
+ * MetaRegistration<PointXYZ> mreg;
+ * mreg.setRegistration (icp);
+ *
+ * while (true)
+ * {
+ * PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
+ * read_cloud (*cloud);
+ * mreg.registerCloud (cloud);
+ *
+ * PointCloud<PointXYZ>::Ptr tmp (new PointCloud<PointXYZ>);
+ * transformPointCloud (*cloud, *tmp, mreg.getAbsoluteTransform ());
+ * write_cloud (*tmp);
+ * }
+ * \endcode
+ *
+ * \author Michael 'v4hn' Goerner
+ * \ingroup registration
+ */
+ template <typename PointT, typename Scalar = float>
+ class MetaRegistration {
+ public:
+ typedef typename pcl::PointCloud<PointT>::Ptr PointCloudPtr;
+ typedef typename pcl::PointCloud<PointT>::ConstPtr PointCloudConstPtr;
+
+ typedef typename pcl::Registration<PointT,PointT,Scalar>::Ptr RegistrationPtr;
+ typedef typename pcl::Registration<PointT,PointT,Scalar>::Matrix4 Matrix4;
+
+ MetaRegistration ();
+
+ /** \brief Empty destructor */
+ virtual ~MetaRegistration () {};
+
+ /** \brief Register new point cloud
+ * \note You have to set a valid registration object with @ref setICP before using this
+ * \param[in] cloud point cloud to register
+ * \param[in] delta_estimate estimated transform between last registered cloud and this one
+ * \return true if ICP converged
+ */
+ bool
+ registerCloud (const PointCloudConstPtr& cloud, const Matrix4& delta_estimate = Matrix4::Identity ());
+
+ /** \brief Get estimated transform of the last registered cloud */
+ inline Matrix4
+ getAbsoluteTransform () const;
+
+ /** \brief Reset MetaICP without resetting registration_ */
+ inline void
+ reset ();
+
+ /** \brief Set registration instance used to align clouds */
+ inline void
+ setRegistration (RegistrationPtr);
+
+ /** \brief get accumulated meta point cloud */
+ inline PointCloudConstPtr
+ getMetaCloud () const;
+ protected:
+
+ /** \brief registered accumulated point cloud */
+ PointCloudPtr full_cloud_;
+
+ /** \brief registration instance to align clouds */
+ RegistrationPtr registration_;
+
+ /** \brief estimated transform */
+ Matrix4 abs_transform_;
+ };
+
+ }
+}
+
+#include <pcl/registration/impl/meta_registration.hpp>
+
+#endif /*PCL_REGISTRATION_META_REGISTRATION_H_*/
typedef typename pcl::search::KdTree<PointTarget>::Ptr KdTreePtr;
typedef pcl::search::KdTree<PointSource> KdTreeReciprocal;
- typedef typename KdTree::Ptr KdTreeReciprocalPtr;
+ typedef typename KdTreeReciprocal::Ptr KdTreeReciprocalPtr;
typedef pcl::PointCloud<PointSource> PointCloudSource;
typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_3POINT_H_
+#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_3POINT_H_
+
+#include <pcl/registration/transformation_estimation.h>
+
+namespace pcl
+{
+ namespace registration
+ {
+ /** \brief TransformationEstimation3Points represents the class for transformation estimation based on:
+ * - correspondence vectors of 3 pairs (planar case)
+ * - two point clouds (source and target) of size 3
+ * - a point cloud with a set of 3 indices (source), and another point cloud (target)
+ * - two point clouds with two sets of indices (source and target) of the size 3
+ *
+ * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix
+ * (i.e., float or double). Default: float.
+ *
+ * \author P.W.Theiler
+ * \ingroup registration
+ */
+ template <typename PointSource, typename PointTarget, typename Scalar = float>
+ class TransformationEstimation3Point : public TransformationEstimation <PointSource, PointTarget, Scalar>
+ {
+ public:
+ /** \cond */
+ typedef boost::shared_ptr<TransformationEstimation3Point<PointSource, PointTarget, Scalar> > Ptr;
+ typedef boost::shared_ptr<const TransformationEstimation3Point<PointSource, PointTarget, Scalar> > ConstPtr;
+ typedef typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4 Matrix4;
+ /** \endcond */
+
+ /** \brief Constructor */
+ TransformationEstimation3Point ()
+ {};
+
+ /** \brief Destructor */
+ virtual ~TransformationEstimation3Point ()
+ {};
+
+ /** \brief Estimate a rigid rotation transformation between a source and a target point cloud.
+ * \param[in] cloud_src the source point cloud dataset
+ * \param[in] cloud_tgt the target point cloud dataset
+ * \param[out] transformation_matrix the resultant transformation matrix
+ */
+ virtual void
+ estimateRigidTransformation (
+ const pcl::PointCloud<PointSource> &cloud_src,
+ const pcl::PointCloud<PointTarget> &cloud_tgt,
+ Matrix4 &transformation_matrix) const;
+
+ /** \brief Estimate a rigid rotation transformation between a source and a target point cloud.
+ * \param[in] cloud_src the source point cloud dataset
+ * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
+ * \param[in] cloud_tgt the target point cloud dataset
+ * \param[out] transformation_matrix the resultant transformation matrix
+ */
+ virtual void
+ estimateRigidTransformation (
+ const pcl::PointCloud<PointSource> &cloud_src,
+ const std::vector<int> &indices_src,
+ const pcl::PointCloud<PointTarget> &cloud_tgt,
+ Matrix4 &transformation_matrix) const;
+
+ /** \brief Estimate a rigid rotation transformation between a source and a target point cloud.
+ * \param[in] cloud_src the source point cloud dataset
+ * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
+ * \param[in] cloud_tgt the target point cloud dataset
+ * \param[in] indices_tgt the vector of indices describing the correspondences of the interst points from \a indices_src
+ * \param[out] transformation_matrix the resultant transformation matrix
+ */
+ virtual void
+ estimateRigidTransformation (
+ const pcl::PointCloud<PointSource> &cloud_src,
+ const std::vector<int> &indices_src,
+ const pcl::PointCloud<PointTarget> &cloud_tgt,
+ const std::vector<int> &indices_tgt,
+ Matrix4 &transformation_matrix) const;
+
+ /** \brief Estimate a rigid rotation transformation between a source and a target point cloud.
+ * \param[in] cloud_src the source point cloud dataset
+ * \param[in] cloud_tgt the target point cloud dataset
+ * \param[in] correspondences the vector of correspondences between source and target point cloud
+ * \param[out] transformation_matrix the resultant transformation matrix
+ */
+ virtual void
+ estimateRigidTransformation (
+ const pcl::PointCloud<PointSource> &cloud_src,
+ const pcl::PointCloud<PointTarget> &cloud_tgt,
+ const pcl::Correspondences &correspondences,
+ Matrix4 &transformation_matrix) const;
+
+ protected:
+
+ /** \brief Estimate a rigid rotation transformation between a source and a target
+ * \param[in] source_it an iterator over the source point cloud dataset
+ * \param[in] target_it an iterator over the target point cloud dataset
+ * \param[out] transformation_matrix the resultant transformation matrix
+ */
+ void
+ estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it,
+ ConstCloudIterator<PointTarget>& target_it,
+ Matrix4 &transformation_matrix) const;
+ };
+ }; // namespace registration
+}; // namespace registration
+
+#include <pcl/registration/impl/transformation_estimation_3point.hpp>
+
+#endif // PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_3POINT_H_
// Test each correspondence
for (size_t i = 0; i < original_correspondences.size (); ++i)
{
- if (boost::static_pointer_cast<DataContainer<pcl::PointXYZ, pcl::PointNormal> >
- (data_container_)->getCorrespondenceScoreFromNormals (original_correspondences[i]) > threshold_)
+ if (data_container_->getCorrespondenceScoreFromNormals (original_correspondences[i]) > threshold_)
remaining_correspondences[number_valid_correspondences++] = original_correspondences[i];
}
remaining_correspondences.resize (number_valid_correspondences);
mahalanobis_.resize (N, Eigen::Matrix3d::Identity ());
// Compute target cloud covariance matrices
- computeCovariances<PointTarget> (target_, tree_, target_covariances_);
+ if ((!target_covariances_) || (target_covariances_->empty ()))
+ {
+ target_covariances_.reset (new MatricesVector);
+ computeCovariances<PointTarget> (target_, tree_, *target_covariances_);
+ }
// Compute input cloud covariance matrices
- computeCovariances<PointSource> (input_, tree_reciprocal_,
- input_covariances_);
+ if ((!input_covariances_) || (input_covariances_->empty ()))
+ {
+ input_covariances_.reset (new MatricesVector);
+ computeCovariances<PointSource> (input_, tree_reciprocal_,
+ *input_covariances_);
+ }
base_transformation_ = guess;
nr_iterations_ = 0;
// Check if the distance to the nearest neighbor is smaller than the user imposed threshold
if (nn_dists[0] < dist_threshold)
{
- Eigen::Matrix3d &C1 = input_covariances_[i];
- Eigen::Matrix3d &C2 = target_covariances_[nn_indices[0]];
+ Eigen::Matrix3d &C1 = (*input_covariances_)[i];
+ Eigen::Matrix3d &C2 = (*target_covariances_)[nn_indices[0]];
Eigen::Matrix3d &M = mahalanobis_[i];
// M = R*C1
M = R * C1;
final_transformation_ (0, 3) = previous_transformation_ (0, 3) + guess (0, 3);
final_transformation_ (1, 3) = previous_transformation_ (1, 3) + guess (1, 3);
final_transformation_ (2, 3) = previous_transformation_ (2, 3) + guess (2, 3);
+
+ // Transform the point cloud
+ pcl::transformPointCloud (*input_, output, final_transformation_);
}
}
*
*/
-/** Re-enable these once all of registration is separated into H/HPP correctly. */
+#include <pcl/registration/ppf_registration.h>
+
+//#ifndef PCL_NO_PRECOMPILE
//#include <pcl/point_types.h>
//#include <pcl/impl/instantiate.hpp>
-//#include <pcl/registration/ppf_registration.h>
//#include <pcl/registration/impl/ppf_registration.hpp>
-//
+
//PCL_INSTANTIATE_PRODUCT(PPFRegistration, (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES));
-//
+//#endif // PCL_NO_PRECOMPILE
+
+void
+pcl::PPFHashMapSearch::setInputFeatureCloud (PointCloud<PPFSignature>::ConstPtr feature_cloud)
+{
+ // Discretize the feature cloud and insert it in the hash map
+ feature_hash_map_->clear ();
+ unsigned int n = static_cast<unsigned int> (sqrt (static_cast<float> (feature_cloud->points.size ())));
+ int d1, d2, d3, d4;
+ max_dist_ = -1.0;
+ alpha_m_.resize (n);
+ for (size_t i = 0; i < n; ++i)
+ {
+ std::vector <float> alpha_m_row (n);
+ for (size_t j = 0; j < n; ++j)
+ {
+ d1 = static_cast<int> (floor (feature_cloud->points[i*n+j].f1 / angle_discretization_step_));
+ d2 = static_cast<int> (floor (feature_cloud->points[i*n+j].f2 / angle_discretization_step_));
+ d3 = static_cast<int> (floor (feature_cloud->points[i*n+j].f3 / angle_discretization_step_));
+ d4 = static_cast<int> (floor (feature_cloud->points[i*n+j].f4 / distance_discretization_step_));
+ feature_hash_map_->insert (std::pair<HashKeyStruct, std::pair<size_t, size_t> > (HashKeyStruct (d1, d2, d3, d4), std::pair<size_t, size_t> (i, j)));
+ alpha_m_row [j] = feature_cloud->points[i*n + j].alpha_m;
+
+ if (max_dist_ < feature_cloud->points[i*n + j].f4)
+ max_dist_ = feature_cloud->points[i*n + j].f4;
+ }
+ alpha_m_[i] = alpha_m_row;
+ }
+
+ internals_initialized_ = true;
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::PPFHashMapSearch::nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4,
+ std::vector<std::pair<size_t, size_t> > &indices)
+{
+ if (!internals_initialized_)
+ {
+ PCL_ERROR("[pcl::PPFRegistration::nearestNeighborSearch]: input feature cloud has not been set - skipping search!\n");
+ return;
+ }
+
+ int d1 = static_cast<int> (floor (f1 / angle_discretization_step_)),
+ d2 = static_cast<int> (floor (f2 / angle_discretization_step_)),
+ d3 = static_cast<int> (floor (f3 / angle_discretization_step_)),
+ d4 = static_cast<int> (floor (f4 / distance_discretization_step_));
+
+ indices.clear ();
+ HashKeyStruct key = HashKeyStruct (d1, d2, d3, d4);
+ std::pair <FeatureHashMapType::iterator, FeatureHashMapType::iterator> map_iterator_pair = feature_hash_map_->equal_range (key);
+ for (; map_iterator_pair.first != map_iterator_pair.second; ++ map_iterator_pair.first)
+ indices.push_back (std::pair<size_t, size_t> (map_iterator_pair.first->second.first,
+ map_iterator_pair.first->second.second));
+}
src/sac_model_cylinder.cpp
src/sac_model_cone.cpp
src/sac_model_line.cpp
+ src/sac_model_parallel_line.cpp
src/sac_model_stick.cpp
src/sac_model_normal_parallel_plane.cpp
src/sac_model_normal_plane.cpp
template <typename PointT> bool
pcl::SampleConsensusModelCircle2D<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
{
- // Needs a valid model coefficients
- if (model_coefficients.size () != 3)
- {
- PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
return (false);
- }
if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[2] < radius_min_)
return (false);
template <typename PointT> bool
pcl::SampleConsensusModelCircle3D<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
{
- // Needs a valid model coefficients
- if (model_coefficients.size () != 7)
- {
- PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
return (false);
- }
if (radius_min_ != -DBL_MAX && model_coefficients[3] < radius_min_)
return (false);
PCL_DEBUG ("[pcl::SampleConsensusModelCone::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n",
info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
model_coefficients[4], model_coefficients[5], model_coefficients[6], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5], optimized_coefficients[6]);
+
+ Eigen::Vector3f line_dir (optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5]);
+ line_dir.normalize ();
+ optimized_coefficients[3] = line_dir[0];
+ optimized_coefficients[4] = line_dir[1];
+ optimized_coefficients[5] = line_dir[2];
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCone<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients)
{
- // Needs a valid model coefficients
- if (model_coefficients.size () != 7)
- {
- PCL_ERROR ("[pcl::SampleConsensusModelCone::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
return (false);
- }
-
+
// Check against template, if given
if (eps_angle_ > 0.0)
{
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCylinder<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients)
{
- // Needs a valid model coefficients
- if (model_coefficients.size () != 7)
- {
- PCL_ERROR ("[pcl::SampleConsensusModelCylinder::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
return (false);
- }
-
+
// Check against template, if given
if (eps_angle_ > 0.0)
{
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelNormalParallelPlane<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients)
{
- // Needs a valid model coefficients
- if (model_coefficients.size () != 4)
- {
- PCL_ERROR ("[pcl::SampleConsensusModelNormalParallelPlane::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
return (false);
- }
// Check against template, if given
if (eps_angle_ > 0.0)
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients)
{
- // Needs a valid model coefficients
- if (model_coefficients.size () != 4)
- {
- PCL_ERROR ("[pcl::SampleConsensusModelNormalSphere::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
return (false);
- }
if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[3] < radius_min_)
return (false);
*
*/
-#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PARALLEL_LINE_H_
-#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PARALLEL_LINE_H_
+#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PARALLEL_LINE_HPP_
+#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PARALLEL_LINE_HPP_
+#include <pcl/common/common.h>
#include <pcl/sample_consensus/sac_model_parallel_line.h>
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelParallelLine<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
{
- // Needs a valid model coefficients
- if (model_coefficients.size () != 6)
- {
- PCL_ERROR ("[pcl::SampleConsensusParallelLine::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
return (false);
- }
// Check against template, if given
if (eps_angle_ > 0.0)
Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0);
double angle_diff = fabs (getAngle3D (axis, line_dir));
- //angle_diff = (std::min) (angle_diff, M_PI - angle_diff);
- angle_diff = fabs (angle_diff - (M_PI/2.0));
- // Check whether the current plane model satisfies our angle threshold criterion with respect to the given axis
+ angle_diff = (std::min) (angle_diff, M_PI - angle_diff);
+ // Check whether the current line model satisfies our angle threshold criterion with respect to the given axis
if (angle_diff > eps_angle_)
return (false);
}
#define PCL_INSTANTIATE_SampleConsensusModelParallelLine(T) template class PCL_EXPORTS pcl::SampleConsensusModelParallelLine<T>;
-#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PARALLEL_LINE_H_
+#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PARALLEL_LINE_HPP_
template <typename PointT> bool
pcl::SampleConsensusModelParallelPlane<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
{
- // Needs a valid model coefficients
- if (model_coefficients.size () != 4)
- {
- PCL_ERROR ("[pcl::SampleConsensusModelParallelPlane::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
return (false);
- }
// Check against template, if given
if (eps_angle_ > 0.0)
template <typename PointT> bool
pcl::SampleConsensusModelPerpendicularPlane<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
{
- // Needs a valid model coefficients
- if (model_coefficients.size () != 4)
- {
- PCL_ERROR ("[pcl::SampleConsensusModelPerpendicularPlane::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
return (false);
- }
// Check against template, if given
if (eps_angle_ > 0.0)
};
}
-// Define the number of samples in SacModel needs
typedef std::map<pcl::SacModel, unsigned int>::value_type SampleSizeModel;
+// Warning: sample_size_pairs is deprecated and is kept only to prevent breaking existing user code.
+// Starting from PCL 1.8.0 model sample size is a protected member of the SampleConsensusModel class.
const static SampleSizeModel sample_size_pairs[] = {SampleSizeModel (pcl::SACMODEL_PLANE, 3),
SampleSizeModel (pcl::SACMODEL_LINE, 2),
SampleSizeModel (pcl::SACMODEL_CIRCLE2D, 3),
namespace pcl
{
- const static std::map<pcl::SacModel, unsigned int> SAC_SAMPLE_SIZE (sample_size_pairs, sample_size_pairs
- + sizeof (sample_size_pairs) / sizeof (SampleSizeModel));
+ const static std::map<pcl::SacModel, unsigned int>
+ PCL_DEPRECATED("This map is deprecated and is kept only to prevent breaking "
+ "existing user code. Starting from PCL 1.8.0 model sample size "
+ "is a protected member of the SampleConsensusModel class")
+ SAC_SAMPLE_SIZE (sample_size_pairs, sample_size_pairs + sizeof (sample_size_pairs) / sizeof (SampleSizeModel));
}
#endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_TYPES_H_
virtual SacModel
getModelType () const = 0;
- /** \brief Return the size of a sample from which a model is computed */
- inline unsigned int
- getSampleSize () const
- {
- std::map<pcl::SacModel, unsigned int>::const_iterator it = SAC_SAMPLE_SIZE.find (getModelType ());
- if (it == SAC_SAMPLE_SIZE.end ())
- throw InvalidSACModelTypeException ("No sample size defined for given model type!\n");
- return (it->second);
+ /** \brief Get a string representation of the name of this class. */
+ inline const std::string&
+ getClassName () const
+ {
+ return (model_name_);
+ }
+
+ /** \brief Return the size of a sample from which the model is computed. */
+ inline unsigned int
+ getSampleSize () const
+ {
+ return sample_size_;
+ }
+
+ /** \brief Return the number of coefficients in the model. */
+ inline unsigned int
+ getModelSize () const
+ {
+ return model_size_;
}
/** \brief Set the minimum and maximum allowable radius limits for the
computeVariance (const std::vector<double> &error_sqr_dists)
{
std::vector<double> dists (error_sqr_dists);
- std::sort (dists.begin (), dists.end ());
- double median_error_sqr = dists[dists.size () >> 1];
+ const size_t medIdx = dists.size () >> 1;
+ std::nth_element (dists.begin (), dists.begin () + medIdx, dists.end ());
+ double median_error_sqr = dists[medIdx];
return (2.1981 * median_error_sqr);
}
return (computeVariance (error_sqr_dists_));
}
- protected:
+ protected:
+
/** \brief Fills a sample array with random samples from the indices_ vector
* \param[out] sample the set of indices of target_ to analyze
*/
}
/** \brief Check whether a model is valid given the user constraints.
+ *
+ * Default implementation verifies that the number of coefficients in the supplied model is as expected for this
+ * SAC model type. Specific SAC models should extend this function by checking the user constraints (if any).
+ *
* \param[in] model_coefficients the set of model coefficients
*/
- virtual inline bool
- isModelValid (const Eigen::VectorXf &model_coefficients) = 0;
+ virtual bool
+ isModelValid (const Eigen::VectorXf &model_coefficients)
+ {
+ if (model_coefficients.size () != model_size_)
+ {
+ PCL_ERROR ("[pcl::%s::isModelValid] Invalid number of model coefficients given (%lu)!\n", getClassName ().c_str (), model_coefficients.size ());
+ return (false);
+ }
+ return (true);
+ }
/** \brief Check if a sample of indices results in a good sample of points
* indices. Pure virtual.
virtual bool
isSampleGood (const std::vector<int> &samples) const = 0;
+ /** \brief The model name. */
+ std::string model_name_;
+
/** \brief A boost shared pointer to the point cloud data array. */
PointCloudConstPtr input_;
/** \brief A vector holding the distances to the computed model. Used internally. */
std::vector<double> error_sqr_dists_;
+ /** \brief The size of a sample from which the model is computed. Every subclass should initialize this appropriately. */
+ unsigned int sample_size_;
+
+ /** \brief The number of coefficients in the model. Every subclass should initialize this appropriately. */
+ unsigned int model_size_;
+
/** \brief Boost-based random number generator. */
inline int
rnd ()
typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;
- /** \brief Empty Construtor. */
+ /** \brief Empty Constructor. */
Functor () : m_data_points_ (ValuesAtCompileTime) {}
/** \brief Constructor
class SampleConsensusModelCircle2D : public SampleConsensusModel<PointT>
{
public:
+ using SampleConsensusModel<PointT>::model_name_;
using SampleConsensusModel<PointT>::input_;
using SampleConsensusModel<PointT>::indices_;
using SampleConsensusModel<PointT>::radius_min_;
*/
SampleConsensusModelCircle2D (const PointCloudConstPtr &cloud, bool random = false)
: SampleConsensusModel<PointT> (cloud, random), tmp_inliers_ ()
- {};
+ {
+ model_name_ = "SampleConsensusModelCircle2D";
+ sample_size_ = 3;
+ model_size_ = 3;
+ }
/** \brief Constructor for base SampleConsensusModelCircle2D.
* \param[in] cloud the input point cloud dataset
const std::vector<int> &indices,
bool random = false)
: SampleConsensusModel<PointT> (cloud, indices, random), tmp_inliers_ ()
- {};
+ {
+ model_name_ = "SampleConsensusModelCircle2D";
+ sample_size_ = 3;
+ model_size_ = 3;
+ }
/** \brief Copy constructor.
* \param[in] source the model to copy into this
SampleConsensusModel<PointT> (), tmp_inliers_ ()
{
*this = source;
+ model_name_ = "SampleConsensusModelCircle2D";
}
/** \brief Empty destructor */
const double threshold);
/** \brief Recompute the 2d circle coefficients using the given inlier set and return them to the user.
- * @note: these are the coefficients of the 2d circle model after refinement (eg. after SVD)
+ * @note: these are the coefficients of the 2d circle model after refinement (e.g. after SVD)
* \param[in] inliers the data inliers found as supporting the model
* \param[in] model_coefficients the initial guess for the optimization
* \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
getModelType () const { return (SACMODEL_CIRCLE2D); }
protected:
+ using SampleConsensusModel<PointT>::sample_size_;
+ using SampleConsensusModel<PointT>::model_size_;
+
/** \brief Check whether a model is valid given the user constraints.
* \param[in] model_coefficients the set of model coefficients
*/
- bool
+ virtual bool
isModelValid (const Eigen::VectorXf &model_coefficients);
/** \brief Check if a sample of indices results in a good sample of points indices.
class SampleConsensusModelCircle3D : public SampleConsensusModel<PointT>
{
public:
+ using SampleConsensusModel<PointT>::model_name_;
using SampleConsensusModel<PointT>::input_;
using SampleConsensusModel<PointT>::indices_;
using SampleConsensusModel<PointT>::radius_min_;
*/
SampleConsensusModelCircle3D (const PointCloudConstPtr &cloud,
bool random = false)
- : SampleConsensusModel<PointT> (cloud, random) {};
+ : SampleConsensusModel<PointT> (cloud, random)
+ {
+ model_name_ = "SampleConsensusModelCircle3D";
+ sample_size_ = 3;
+ model_size_ = 7;
+ }
/** \brief Constructor for base SampleConsensusModelCircle3D.
* \param[in] cloud the input point cloud dataset
SampleConsensusModelCircle3D (const PointCloudConstPtr &cloud,
const std::vector<int> &indices,
bool random = false)
- : SampleConsensusModel<PointT> (cloud, indices, random) {};
+ : SampleConsensusModel<PointT> (cloud, indices, random)
+ {
+ model_name_ = "SampleConsensusModelCircle3D";
+ sample_size_ = 3;
+ model_size_ = 7;
+ }
/** \brief Empty destructor */
virtual ~SampleConsensusModelCircle3D () {}
SampleConsensusModel<PointT> (), tmp_inliers_ ()
{
*this = source;
+ model_name_ = "SampleConsensusModelCircle3D";
}
/** \brief Copy constructor.
const double threshold);
/** \brief Recompute the 3d circle coefficients using the given inlier set and return them to the user.
- * @note: these are the coefficients of the 3d circle model after refinement (eg. after SVD)
+ * @note: these are the coefficients of the 3d circle model after refinement (e.g. after SVD)
* \param[in] inliers the data inliers found as supporting the model
* \param[in] model_coefficients the initial guess for the optimization
* \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
getModelType () const { return (SACMODEL_CIRCLE3D); }
protected:
+ using SampleConsensusModel<PointT>::sample_size_;
+ using SampleConsensusModel<PointT>::model_size_;
+
/** \brief Check whether a model is valid given the user constraints.
* \param[in] model_coefficients the set of model coefficients
*/
- bool
+ virtual bool
isModelValid (const Eigen::VectorXf &model_coefficients);
/** \brief Check if a sample of indices results in a good sample of points indices.
#include <pcl/sample_consensus/sac_model.h>
#include <pcl/sample_consensus/model_types.h>
+#include <pcl/pcl_macros.h>
#include <pcl/common/common.h>
#include <pcl/common/distances.h>
#include <limits.h>
class SampleConsensusModelCone : public SampleConsensusModel<PointT>, public SampleConsensusModelFromNormals<PointT, PointNT>
{
public:
+ using SampleConsensusModel<PointT>::model_name_;
using SampleConsensusModel<PointT>::input_;
using SampleConsensusModel<PointT>::indices_;
using SampleConsensusModel<PointT>::radius_min_;
, max_angle_ (std::numeric_limits<double>::max ())
, tmp_inliers_ ()
{
+ model_name_ = "SampleConsensusModelCone";
+ sample_size_ = 3;
+ model_size_ = 7;
}
/** \brief Constructor for base SampleConsensusModelCone.
, max_angle_ (std::numeric_limits<double>::max ())
, tmp_inliers_ ()
{
+ model_name_ = "SampleConsensusModelCone";
+ sample_size_ = 3;
+ model_size_ = 7;
}
/** \brief Copy constructor.
axis_ (), eps_angle_ (), min_angle_ (), max_angle_ (), tmp_inliers_ ()
{
*this = source;
+ model_name_ = "SampleConsensusModelCone";
}
/** \brief Empty destructor */
operator = (const SampleConsensusModelCone &source)
{
SampleConsensusModel<PointT>::operator=(source);
+ SampleConsensusModelFromNormals<PointT, PointNT>::operator=(source);
axis_ = source.axis_;
eps_angle_ = source.eps_angle_;
min_angle_ = source.min_angle_;
/** \brief Set the minimum and maximum allowable opening angle for a cone model
* given from a user.
- * \param[in] min_angle the minimum allwoable opening angle of a cone model
- * \param[in] max_angle the maximum allwoable opening angle of a cone model
+ * \param[in] min_angle the minimum allowable opening angle of a cone model
+ * \param[in] max_angle the maximum allowable opening angle of a cone model
*/
inline void
setMinMaxOpeningAngle (const double &min_angle, const double &max_angle)
max_angle_ = max_angle;
}
- /** \brief Get the opening angle which we need minumum to validate a cone model.
- * \param[out] min_angle the minimum allwoable opening angle of a cone model
- * \param[out] max_angle the maximum allwoable opening angle of a cone model
+ /** \brief Get the opening angle which we need minimum to validate a cone model.
+ * \param[out] min_angle the minimum allowable opening angle of a cone model
+ * \param[out] max_angle the maximum allowable opening angle of a cone model
*/
inline void
getMinMaxOpeningAngle (double &min_angle, double &max_angle) const
/** \brief Recompute the cone coefficients using the given inlier set and return them to the user.
- * @note: these are the coefficients of the cone model after refinement (eg. after SVD)
+ * @note: these are the coefficients of the cone model after refinement (e.g. after SVD)
* \param[in] inliers the data inliers found as supporting the model
* \param[in] model_coefficients the initial guess for the optimization
* \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
getModelType () const { return (SACMODEL_CONE); }
protected:
+ using SampleConsensusModel<PointT>::sample_size_;
+ using SampleConsensusModel<PointT>::model_size_;
+
/** \brief Get the distance from a point to a line (represented by a point and a direction)
* \param[in] pt a point
* \param[in] model_coefficients the line coefficients (a point on the line, line direction)
pointToAxisDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients);
/** \brief Get a string representation of the name of this class. */
- std::string
- getName () const { return ("SampleConsensusModelCone"); }
+ PCL_DEPRECATED ("[pcl::SampleConsensusModelCone::getName] getName is deprecated. Please use getClassName instead.")
+ std::string
+ getName () const { return (model_name_); }
protected:
/** \brief Check whether a model is valid given the user constraints.
* \param[in] model_coefficients the set of model coefficients
*/
- bool
+ virtual bool
isModelValid (const Eigen::VectorXf &model_coefficients);
/** \brief Check if a sample of indices results in a good sample of points
#include <pcl/sample_consensus/sac_model.h>
#include <pcl/sample_consensus/model_types.h>
+#include <pcl/pcl_macros.h>
#include <pcl/common/common.h>
#include <pcl/common/distances.h>
class SampleConsensusModelCylinder : public SampleConsensusModel<PointT>, public SampleConsensusModelFromNormals<PointT, PointNT>
{
public:
+ using SampleConsensusModel<PointT>::model_name_;
using SampleConsensusModel<PointT>::input_;
using SampleConsensusModel<PointT>::indices_;
using SampleConsensusModel<PointT>::radius_min_;
, eps_angle_ (0)
, tmp_inliers_ ()
{
+ model_name_ = "SampleConsensusModelCylinder";
+ sample_size_ = 2;
+ model_size_ = 7;
}
/** \brief Constructor for base SampleConsensusModelCylinder.
, eps_angle_ (0)
, tmp_inliers_ ()
{
+ model_name_ = "SampleConsensusModelCylinder";
+ sample_size_ = 2;
+ model_size_ = 7;
}
/** \brief Copy constructor.
tmp_inliers_ ()
{
*this = source;
+ model_name_ = "SampleConsensusModelCylinder";
}
/** \brief Empty destructor */
operator = (const SampleConsensusModelCylinder &source)
{
SampleConsensusModel<PointT>::operator=(source);
+ SampleConsensusModelFromNormals<PointT, PointNT>::operator=(source);
axis_ = source.axis_;
eps_angle_ = source.eps_angle_;
tmp_inliers_ = source.tmp_inliers_;
}
/** \brief Set the angle epsilon (delta) threshold.
- * \param[in] ea the maximum allowed difference between the cyilinder axis and the given axis.
+ * \param[in] ea the maximum allowed difference between the cylinder axis and the given axis.
*/
inline void
setEpsAngle (const double ea) { eps_angle_ = ea; }
const double threshold);
/** \brief Recompute the cylinder coefficients using the given inlier set and return them to the user.
- * @note: these are the coefficients of the cylinder model after refinement (eg. after SVD)
+ * @note: these are the coefficients of the cylinder model after refinement (e.g. after SVD)
* \param[in] inliers the data inliers found as supporting the model
* \param[in] model_coefficients the initial guess for the optimization
* \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
getModelType () const { return (SACMODEL_CYLINDER); }
protected:
+ using SampleConsensusModel<PointT>::sample_size_;
+ using SampleConsensusModel<PointT>::model_size_;
+
/** \brief Get the distance from a point to a line (represented by a point and a direction)
* \param[in] pt a point
* \param[in] model_coefficients the line coefficients (a point on the line, line direction)
Eigen::Vector4f &pt_proj);
/** \brief Get a string representation of the name of this class. */
- std::string
- getName () const { return ("SampleConsensusModelCylinder"); }
+ PCL_DEPRECATED ("[pcl::SampleConsensusModelCylinder::getName] getName is deprecated. Please use getClassName instead.")
+ std::string
+ getName () const { return (model_name_); }
protected:
/** \brief Check whether a model is valid given the user constraints.
* \param[in] model_coefficients the set of model coefficients
*/
- bool
+ virtual bool
isModelValid (const Eigen::VectorXf &model_coefficients);
/** \brief Check if a sample of indices results in a good sample of points
class SampleConsensusModelLine : public SampleConsensusModel<PointT>
{
public:
+ using SampleConsensusModel<PointT>::model_name_;
using SampleConsensusModel<PointT>::input_;
using SampleConsensusModel<PointT>::indices_;
using SampleConsensusModel<PointT>::error_sqr_dists_;
+ using SampleConsensusModel<PointT>::isModelValid;
typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud;
typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModelLine (const PointCloudConstPtr &cloud, bool random = false)
- : SampleConsensusModel<PointT> (cloud, random) {};
+ : SampleConsensusModel<PointT> (cloud, random)
+ {
+ model_name_ = "SampleConsensusModelLine";
+ sample_size_ = 2;
+ model_size_ = 6;
+ }
/** \brief Constructor for base SampleConsensusModelLine.
* \param[in] cloud the input point cloud dataset
SampleConsensusModelLine (const PointCloudConstPtr &cloud,
const std::vector<int> &indices,
bool random = false)
- : SampleConsensusModel<PointT> (cloud, indices, random) {};
+ : SampleConsensusModel<PointT> (cloud, indices, random)
+ {
+ model_name_ = "SampleConsensusModelLine";
+ sample_size_ = 2;
+ model_size_ = 6;
+ }
/** \brief Empty destructor */
virtual ~SampleConsensusModelLine () {}
const double threshold);
/** \brief Recompute the line coefficients using the given inlier set and return them to the user.
- * @note: these are the coefficients of the line model after refinement (eg. after SVD)
+ * @note: these are the coefficients of the line model after refinement (e.g. after SVD)
* \param[in] inliers the data inliers found as supporting the model
* \param[in] model_coefficients the initial guess for the model coefficients
* \param[out] optimized_coefficients the resultant recomputed coefficients after optimization
getModelType () const { return (SACMODEL_LINE); }
protected:
- /** \brief Check whether a model is valid given the user constraints.
- * \param[in] model_coefficients the set of model coefficients
- */
- inline bool
- isModelValid (const Eigen::VectorXf &model_coefficients)
- {
- if (model_coefficients.size () != 6)
- {
- PCL_ERROR ("[pcl::SampleConsensusModelLine::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
- return (false);
- }
-
- return (true);
- }
+ using SampleConsensusModel<PointT>::sample_size_;
+ using SampleConsensusModel<PointT>::model_size_;
/** \brief Check if a sample of indices results in a good sample of points
* indices.
class SampleConsensusModelNormalParallelPlane : public SampleConsensusModelNormalPlane<PointT, PointNT>
{
public:
+ using SampleConsensusModel<PointT>::model_name_;
using SampleConsensusModel<PointT>::input_;
using SampleConsensusModel<PointT>::indices_;
using SampleConsensusModelFromNormals<PointT, PointNT>::normals_;
, cos_angle_ (-1.0)
, eps_dist_ (0.0)
{
+ model_name_ = "SampleConsensusModelNormalParallelPlane";
+ sample_size_ = 3;
+ model_size_ = 4;
}
/** \brief Constructor for base SampleConsensusModelNormalParallelPlane.
, cos_angle_ (-1.0)
, eps_dist_ (0.0)
{
+ model_name_ = "SampleConsensusModelNormalParallelPlane";
+ sample_size_ = 3;
+ model_size_ = 4;
}
/** \brief Empty destructor */
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
protected:
+ using SampleConsensusModel<PointT>::sample_size_;
+ using SampleConsensusModel<PointT>::model_size_;
+
/** \brief Check whether a model is valid given the user constraints.
* \param[in] model_coefficients the set of model coefficients
*/
- bool
+ virtual bool
isModelValid (const Eigen::VectorXf &model_coefficients);
private:
class SampleConsensusModelNormalPlane : public SampleConsensusModelPlane<PointT>, public SampleConsensusModelFromNormals<PointT, PointNT>
{
public:
+ using SampleConsensusModel<PointT>::model_name_;
using SampleConsensusModel<PointT>::input_;
using SampleConsensusModel<PointT>::indices_;
using SampleConsensusModelFromNormals<PointT, PointNT>::normals_;
using SampleConsensusModelFromNormals<PointT, PointNT>::normal_distance_weight_;
using SampleConsensusModel<PointT>::error_sqr_dists_;
- using SampleConsensusModel
Plane<PointT>::isModelValid;
+ using SampleConsensusModel<PointT>::isModelValid;
typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud;
typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
: SampleConsensusModelPlane<PointT> (cloud, random)
, SampleConsensusModelFromNormals<PointT, PointNT> ()
{
+ model_name_ = "SampleConsensusModelNormalPlane";
+ sample_size_ = 3;
+ model_size_ = 4;
}
/** \brief Constructor for base SampleConsensusModelNormalPlane.
: SampleConsensusModelPlane<PointT> (cloud, indices, random)
, SampleConsensusModelFromNormals<PointT, PointNT> ()
{
+ model_name_ = "SampleConsensusModelNormalPlane";
+ sample_size_ = 3;
+ model_size_ = 4;
}
/** \brief Empty destructor */
getModelType () const { return (SACMODEL_NORMAL_PLANE); }
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ protected:
+ using SampleConsensusModel<PointT>::sample_size_;
+ using SampleConsensusModel<PointT>::model_size_;
};
}
class SampleConsensusModelNormalSphere : public SampleConsensusModelSphere<PointT>, public SampleConsensusModelFromNormals<PointT, PointNT>
{
public:
+ using SampleConsensusModel<PointT>::model_name_;
using SampleConsensusModel<PointT>::input_;
using SampleConsensusModel<PointT>::indices_;
using SampleConsensusModel<PointT>::radius_min_;
: SampleConsensusModelSphere<PointT> (cloud, random)
, SampleConsensusModelFromNormals<PointT, PointNT> ()
{
+ model_name_ = "SampleConsensusModelNormalSphere";
+ sample_size_ = 4;
+ model_size_ = 4;
}
/** \brief Constructor for base SampleConsensusModelNormalSphere.
: SampleConsensusModelSphere<PointT> (cloud, indices, random)
, SampleConsensusModelFromNormals<PointT, PointNT> ()
{
+ model_name_ = "SampleConsensusModelNormalSphere";
+ sample_size_ = 4;
+ model_size_ = 4;
}
/** \brief Empty destructor */
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
protected:
+ using SampleConsensusModel<PointT>::sample_size_;
+ using SampleConsensusModel<PointT>::model_size_;
+
/** \brief Check whether a model is valid given the user constraints.
* \param[in] model_coefficients the set of model coefficients
*/
- bool
+ virtual bool
isModelValid (const Eigen::VectorXf &model_coefficients);
};
*
*/
-#ifndef PCL_SAMPLE_CONSENSUS_MODEL_PARALLELLINE_H_
-#define PCL_SAMPLE_CONSENSUS_MODEL_PARALLELLINE_H_
+#ifndef PCL_SAMPLE_CONSENSUS_MODEL_PARALLEL_LINE_H_
+#define PCL_SAMPLE_CONSENSUS_MODEL_PARALLEL_LINE_H_
#include <pcl/sample_consensus/sac_model_line.h>
-#include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
namespace pcl
{
/** \brief SampleConsensusModelParallelLine defines a model for 3D line segmentation using additional angular
* constraints.
+ *
+ * Checking for inliers will not only involve a "distance to model" criterion, but also an additional "maximum
+ * angular deviation" between the line's direction and a user-specified axis.
+ *
* The model coefficients are defined as:
* - \b point_on_line.x : the X coordinate of a point on the line
* - \b point_on_line.y : the Y coordinate of a point on the line
* - \b line_direction.x : the X coordinate of a line's direction
* - \b line_direction.y : the Y coordinate of a line's direction
* - \b line_direction.z : the Z coordinate of a line's direction
- *
+ *
* \author Radu B. Rusu
* \ingroup sample_consensus
*/
class SampleConsensusModelParallelLine : public SampleConsensusModelLine<PointT>
{
public:
+ using SampleConsensusModel<PointT>::model_name_;
+
typedef typename SampleConsensusModelLine<PointT>::PointCloud PointCloud;
typedef typename SampleConsensusModelLine<PointT>::PointCloudPtr PointCloudPtr;
typedef typename SampleConsensusModelLine<PointT>::PointCloudConstPtr PointCloudConstPtr;
* \param[in] cloud the input point cloud dataset
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
- SampleConsensusModelParallelLine (const PointCloudConstPtr &cloud,
- bool random = false)
+ SampleConsensusModelParallelLine (const PointCloudConstPtr &cloud,
+ bool random = false)
: SampleConsensusModelLine<PointT> (cloud, random)
, axis_ (Eigen::Vector3f::Zero ())
, eps_angle_ (0.0)
{
+ model_name_ = "SampleConsensusModelParallelLine";
+ sample_size_ = 2;
+ model_size_ = 6;
}
/** \brief Constructor for base SampleConsensusModelParallelLine.
* \param[in] indices a vector of point indices to be used from \a cloud
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
- SampleConsensusModelParallelLine (const PointCloudConstPtr &cloud,
+ SampleConsensusModelParallelLine (const PointCloudConstPtr &cloud,
const std::vector<int> &indices,
- bool random = false)
+ bool random = false)
: SampleConsensusModelLine<PointT> (cloud, indices, random)
, axis_ (Eigen::Vector3f::Zero ())
, eps_angle_ (0.0)
{
+ model_name_ = "SampleConsensusModelParallelLine";
+ sample_size_ = 2;
+ model_size_ = 6;
}
-
+
/** \brief Empty destructor */
virtual ~SampleConsensusModelParallelLine () {}
- /** \brief Set the axis along which we need to search for a plane perpendicular to.
- * \param[in] ax the axis along which we need to search for a plane perpendicular to
+ /** \brief Set the axis along which we need to search for a line.
+ * \param[in] ax the axis along which we need to search for a line
*/
- inline void
+ inline void
setAxis (const Eigen::Vector3f &ax) { axis_ = ax; axis_.normalize (); }
- /** \brief Get the axis along which we need to search for a plane perpendicular to. */
- inline Eigen::Vector3f
- getAxis () { return (axis_); }
+ /** \brief Get the axis along which we need to search for a line. */
+ inline Eigen::Vector3f
+ getAxis () const { return (axis_); }
/** \brief Set the angle epsilon (delta) threshold.
- * \param[in] ea the maximum allowed difference between the plane normal and the given axis.
+ * \param[in] ea the maximum allowed difference between the line direction and the given axis (in radians).
*/
- inline void
+ inline void
setEpsAngle (const double ea) { eps_angle_ = ea; }
- /** \brief Get the angle epsilon (delta) threshold. */
- inline double getEpsAngle () { return (eps_angle_); }
+ /** \brief Get the angle epsilon (delta) threshold (in radians). */
+ inline double getEpsAngle () const { return (eps_angle_); }
/** \brief Select all the points which respect the given model coefficients as inliers.
* \param[in] model_coefficients the coefficients of a line model that we need to compute distances to
* \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
* \param[out] inliers the resultant model inliers
*/
- void
- selectWithinDistance (const Eigen::VectorXf &model_coefficients,
- const double threshold,
+ void
+ selectWithinDistance (const Eigen::VectorXf &model_coefficients,
+ const double threshold,
std::vector<int> &inliers);
- /** \brief Count all the points which respect the given model coefficients as inliers.
- *
+ /** \brief Count all the points which respect the given model coefficients as inliers.
+ *
* \param[in] model_coefficients the coefficients of a model that we need to compute distances to
* \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
* \return the resultant number of inliers
*/
virtual int
- countWithinDistance (const Eigen::VectorXf &model_coefficients,
+ countWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold);
/** \brief Compute all squared distances from the cloud data to a given line model.
* \param[in] model_coefficients the coefficients of a line model that we need to compute distances to
* \param[out] distances the resultant estimated squared distances
*/
- void
- getDistancesToModel (const Eigen::VectorXf &model_coefficients,
+ void
+ getDistancesToModel (const Eigen::VectorXf &model_coefficients,
std::vector<double> &distances);
/** \brief Return an unique id for this model (SACMODEL_PARALLEL_LINE). */
- inline pcl::SacModel
+ inline pcl::SacModel
getModelType () const { return (SACMODEL_PARALLEL_LINE); }
protected:
+ using SampleConsensusModel<PointT>::sample_size_;
+ using SampleConsensusModel<PointT>::model_size_;
+
/** \brief Check whether a model is valid given the user constraints.
* \param[in] model_coefficients the set of model coefficients
*/
- bool
+ virtual bool
isModelValid (const Eigen::VectorXf &model_coefficients);
- protected:
- /** \brief The axis along which we need to search for a plane perpendicular to. */
+ /** \brief The axis along which we need to search for a line. */
Eigen::Vector3f axis_;
- /** \brief The maximum allowed difference between the plane normal and the given axis. */
+ /** \brief The maximum allowed difference between the line direction and the given axis. */
double eps_angle_;
};
}
#include <pcl/sample_consensus/impl/sac_model_parallel_line.hpp>
#endif
-#endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_PARALLELLINE_H_
+#endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_PARALLEL_LINE_H_
class SampleConsensusModelParallelPlane : public SampleConsensusModelPlane<PointT>
{
public:
+ using SampleConsensusModel<PointT>::model_name_;
+
typedef typename SampleConsensusModelPlane<PointT>::PointCloud PointCloud;
typedef typename SampleConsensusModelPlane<PointT>::PointCloudPtr PointCloudPtr;
typedef typename SampleConsensusModelPlane<PointT>::PointCloudConstPtr PointCloudConstPtr;
, eps_angle_ (0.0)
, sin_angle_ (-1.0)
{
+ model_name_ = "SampleConsensusModelParallelPlane";
+ sample_size_ = 3;
+ model_size_ = 4;
}
/** \brief Constructor for base SampleConsensusModelParallelPlane.
, eps_angle_ (0.0)
, sin_angle_ (-1.0)
{
+ model_name_ = "SampleConsensusModelParallelPlane";
+ sample_size_ = 3;
+ model_size_ = 4;
}
/** \brief Empty destructor */
getModelType () const { return (SACMODEL_PARALLEL_PLANE); }
protected:
+ using SampleConsensusModel<PointT>::sample_size_;
+ using SampleConsensusModel<PointT>::model_size_;
+
/** \brief Check whether a model is valid given the user constraints.
* \param[in] model_coefficients the set of model coefficients
*/
- bool
+ virtual bool
isModelValid (const Eigen::VectorXf &model_coefficients);
/** \brief The axis along which we need to search for a plane perpendicular to. */
class SampleConsensusModelPerpendicularPlane : public SampleConsensusModelPlane<PointT>
{
public:
+ using SampleConsensusModel<PointT>::model_name_;
+
typedef typename SampleConsensusModelPlane<PointT>::PointCloud PointCloud;
typedef typename SampleConsensusModelPlane<PointT>::PointCloudPtr PointCloudPtr;
typedef typename SampleConsensusModelPlane<PointT>::PointCloudConstPtr PointCloudConstPtr;
, axis_ (Eigen::Vector3f::Zero ())
, eps_angle_ (0.0)
{
+ model_name_ = "SampleConsensusModelPerpendicularPlane";
+ sample_size_ = 3;
+ model_size_ = 4;
}
/** \brief Constructor for base SampleConsensusModelPerpendicularPlane.
, axis_ (Eigen::Vector3f::Zero ())
, eps_angle_ (0.0)
{
+ model_name_ = "SampleConsensusModelPerpendicularPlane";
+ sample_size_ = 3;
+ model_size_ = 4;
}
-
+
/** \brief Empty destructor */
virtual ~SampleConsensusModelPerpendicularPlane () {}
getModelType () const { return (SACMODEL_PERPENDICULAR_PLANE); }
protected:
+ using SampleConsensusModel<PointT>::sample_size_;
+ using SampleConsensusModel<PointT>::model_size_;
+
/** \brief Check whether a model is valid given the user constraints.
* \param[in] model_coefficients the set of model coefficients
*/
- bool
+ virtual bool
isModelValid (const Eigen::VectorXf &model_coefficients);
/** \brief The axis along which we need to search for a plane perpendicular to. */
class SampleConsensusModelPlane : public SampleConsensusModel<PointT>
{
public:
+ using SampleConsensusModel<PointT>::model_name_;
using SampleConsensusModel<PointT>::input_;
using SampleConsensusModel<PointT>::indices_;
using SampleConsensusModel<PointT>::error_sqr_dists_;
+ using SampleConsensusModel<PointT>::isModelValid;
typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud;
typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensusModelPlane (const PointCloudConstPtr &cloud, bool random = false)
- : SampleConsensusModel<PointT> (cloud, random) {};
+ : SampleConsensusModel<PointT> (cloud, random)
+ {
+ model_name_ = "SampleConsensusModelPlane";
+ sample_size_ = 3;
+ model_size_ = 4;
+ }
/** \brief Constructor for base SampleConsensusModelPlane.
* \param[in] cloud the input point cloud dataset
SampleConsensusModelPlane (const PointCloudConstPtr &cloud,
const std::vector<int> &indices,
bool random = false)
- : SampleConsensusModel<PointT> (cloud, indices, random) {};
+ : SampleConsensusModel<PointT> (cloud, indices, random)
+ {
+ model_name_ = "SampleConsensusModelPlane";
+ sample_size_ = 3;
+ model_size_ = 4;
+ }
/** \brief Empty destructor */
virtual ~SampleConsensusModelPlane () {}
const double threshold);
/** \brief Recompute the plane coefficients using the given inlier set and return them to the user.
- * @note: these are the coefficients of the plane model after refinement (eg. after SVD)
+ * @note: these are the coefficients of the plane model after refinement (e.g. after SVD)
* \param[in] inliers the data inliers found as supporting the model
* \param[in] model_coefficients the initial guess for the model coefficients
* \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
getModelType () const { return (SACMODEL_PLANE); }
protected:
- /** \brief Check whether a model is valid given the user constraints.
- * \param[in] model_coefficients the set of model coefficients
- */
- inline bool
- isModelValid (const Eigen::VectorXf &model_coefficients)
- {
- // Needs a valid model coefficients
- if (model_coefficients.size () != 4)
- {
- PCL_ERROR ("[pcl::SampleConsensusModelPlane::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
- return (false);
- }
- return (true);
- }
+ using SampleConsensusModel<PointT>::sample_size_;
+ using SampleConsensusModel<PointT>::model_size_;
private:
/** \brief Check if a sample of indices results in a good sample of points
class SampleConsensusModelRegistration : public SampleConsensusModel<PointT>
{
public:
+ using SampleConsensusModel<PointT>::model_name_;
using SampleConsensusModel<PointT>::input_;
using SampleConsensusModel<PointT>::indices_;
using SampleConsensusModel<PointT>::error_sqr_dists_;
+ using SampleConsensusModel<PointT>::isModelValid;
typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud;
typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
{
// Call our own setInputCloud
setInputCloud (cloud);
+ model_name_ = "SampleConsensusModelRegistration";
+ sample_size_ = 3;
+ model_size_ = 16;
}
/** \brief Constructor for base SampleConsensusModelRegistration.
{
computeOriginalIndexMapping ();
computeSampleDistanceThreshold (cloud, indices);
+ model_name_ = "SampleConsensusModelRegistration";
+ sample_size_ = 3;
+ model_size_ = 16;
}
/** \brief Empty destructor */
getModelType () const { return (SACMODEL_REGISTRATION); }
protected:
- /** \brief Check whether a model is valid given the user constraints.
- * \param[in] model_coefficients the set of model coefficients
- */
- inline bool
- isModelValid (const Eigen::VectorXf &model_coefficients)
- {
- // Needs a valid model coefficients
- if (model_coefficients.size () != 16)
- return (false);
-
- return (true);
- }
+ using SampleConsensusModel<PointT>::sample_size_;
+ using SampleConsensusModel<PointT>::model_size_;
/** \brief Check if a sample of indices results in a good sample of points
* indices.
class SampleConsensusModelRegistration2D : public pcl::SampleConsensusModelRegistration<PointT>
{
public:
+ using pcl::SampleConsensusModelRegistration<PointT>::model_name_;
using pcl::SampleConsensusModelRegistration<PointT>::input_;
using pcl::SampleConsensusModelRegistration<PointT>::target_;
using pcl::SampleConsensusModelRegistration<PointT>::indices_;
using pcl::SampleConsensusModelRegistration<PointT>::correspondences_;
using pcl::SampleConsensusModelRegistration<PointT>::sample_dist_thresh_;
using pcl::SampleConsensusModelRegistration<PointT>::computeOriginalIndexMapping;
+ using pcl::SampleConsensusModel<PointT>::isModelValid;
typedef typename pcl::SampleConsensusModel<PointT>::PointCloud PointCloud;
typedef typename pcl::SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
{
// Call our own setInputCloud
setInputCloud (cloud);
+ model_name_ = "SampleConsensusModelRegistration2D";
+ sample_size_ = 3;
+ model_size_ = 16;
}
/** \brief Constructor for base SampleConsensusModelRegistration2D.
{
computeOriginalIndexMapping ();
computeSampleDistanceThreshold (cloud, indices);
+ model_name_ = "SampleConsensusModelRegistration2D";
+ sample_size_ = 3;
+ model_size_ = 16;
}
/** \brief Empty destructor */
{ return (projection_matrix_); }
protected:
+ using SampleConsensusModel<PointT>::sample_size_;
+ using SampleConsensusModel<PointT>::model_size_;
+
/** \brief Check if a sample of indices results in a good sample of points
* indices.
* \param[in] samples the resultant index samples
class SampleConsensusModelSphere : public SampleConsensusModel<PointT>
{
public:
+ using SampleConsensusModel<PointT>::model_name_;
using SampleConsensusModel<PointT>::input_;
using SampleConsensusModel<PointT>::indices_;
using SampleConsensusModel<PointT>::radius_min_;
SampleConsensusModelSphere (const PointCloudConstPtr &cloud,
bool random = false)
: SampleConsensusModel<PointT> (cloud, random), tmp_inliers_ ()
- {}
+ {
+ model_name_ = "SampleConsensusModelSphere";
+ sample_size_ = 4;
+ model_size_ = 4;
+ }
/** \brief Constructor for base SampleConsensusModelSphere.
* \param[in] cloud the input point cloud dataset
const std::vector<int> &indices,
bool random = false)
: SampleConsensusModel<PointT> (cloud, indices, random), tmp_inliers_ ()
- {}
+ {
+ model_name_ = "SampleConsensusModelSphere";
+ sample_size_ = 4;
+ model_size_ = 4;
+ }
/** \brief Empty destructor */
virtual ~SampleConsensusModelSphere () {}
SampleConsensusModel<PointT> (), tmp_inliers_ ()
{
*this = source;
+ model_name_ = "SampleConsensusModelSphere";
}
/** \brief Copy constructor.
const double threshold);
/** \brief Recompute the sphere coefficients using the given inlier set and return them to the user.
- * @note: these are the coefficients of the sphere model after refinement (eg. after SVD)
+ * @note: these are the coefficients of the sphere model after refinement (e.g. after SVD)
* \param[in] inliers the data inliers found as supporting the model
* \param[in] model_coefficients the initial guess for the optimization
* \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
inline pcl::SacModel getModelType () const { return (SACMODEL_SPHERE); }
protected:
+ using SampleConsensusModel<PointT>::sample_size_;
+ using SampleConsensusModel<PointT>::model_size_;
+
/** \brief Check whether a model is valid given the user constraints.
* \param[in] model_coefficients the set of model coefficients
*/
- inline bool
+ virtual bool
isModelValid (const Eigen::VectorXf &model_coefficients)
{
- // Needs a valid model coefficients
- if (model_coefficients.size () != 4)
- {
- PCL_ERROR ("[pcl::SampleConsensusModelSphere::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+ if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
return (false);
- }
if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[3] < radius_min_)
return (false);
class SampleConsensusModelStick : public SampleConsensusModel<PointT>
{
public:
+ using SampleConsensusModel<PointT>::model_name_;
using SampleConsensusModel<PointT>::input_;
using SampleConsensusModel<PointT>::indices_;
using SampleConsensusModel<PointT>::radius_min_;
using SampleConsensusModel<PointT>::radius_max_;
using SampleConsensusModel<PointT>::error_sqr_dists_;
+ using SampleConsensusModel<PointT>::isModelValid;
typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud;
typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
*/
SampleConsensusModelStick (const PointCloudConstPtr &cloud,
bool random = false)
- : SampleConsensusModel<PointT> (cloud, random) {};
+ : SampleConsensusModel<PointT> (cloud, random)
+ {
+ model_name_ = "SampleConsensusModelStick";
+ sample_size_ = 2;
+ model_size_ = 7;
+ }
/** \brief Constructor for base SampleConsensusModelStick.
* \param[in] cloud the input point cloud dataset
SampleConsensusModelStick (const PointCloudConstPtr &cloud,
const std::vector<int> &indices,
bool random = false)
- : SampleConsensusModel<PointT> (cloud, indices, random) {};
+ : SampleConsensusModel<PointT> (cloud, indices, random)
+ {
+ model_name_ = "SampleConsensusModelStick";
+ sample_size_ = 2;
+ model_size_ = 7;
+ }
/** \brief Empty destructor */
virtual ~SampleConsensusModelStick () {}
const double threshold);
/** \brief Recompute the stick coefficients using the given inlier set and return them to the user.
- * @note: these are the coefficients of the stick model after refinement (eg. after SVD)
+ * @note: these are the coefficients of the stick model after refinement (e.g. after SVD)
* \param[in] inliers the data inliers found as supporting the model
* \param[in] model_coefficients the initial guess for the model coefficients
* \param[out] optimized_coefficients the resultant recomputed coefficients after optimization
const Eigen::VectorXf &model_coefficients,
const double threshold);
- /** \brief Return an unique id for this model (SACMODEL_STACK). */
+ /** \brief Return an unique id for this model (SACMODEL_STICK). */
inline pcl::SacModel
getModelType () const { return (SACMODEL_STICK); }
protected:
- /** \brief Check whether a model is valid given the user constraints.
- * \param[in] model_coefficients the set of model coefficients
- */
- inline bool
- isModelValid (const Eigen::VectorXf &model_coefficients)
- {
- if (model_coefficients.size () != 7)
- {
- PCL_ERROR ("[pcl::SampleConsensusModelStick::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
- return (false);
- }
-
- return (true);
- }
+ using SampleConsensusModel<PointT>::sample_size_;
+ using SampleConsensusModel<PointT>::model_size_;
/** \brief Check if a sample of indices results in a good sample of points
* indices.
*/
#include <pcl/sample_consensus/impl/sac_model_line.hpp>
-#include <pcl/sample_consensus/impl/sac_model_parallel_line.hpp>
#ifndef PCL_NO_PRECOMPILE
#include <pcl/impl/instantiate.hpp>
// Instantiations of specific point types
#ifdef PCL_ONLY_CORE_POINT_TYPES
PCL_INSTANTIATE(SampleConsensusModelLine, (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA)(pcl::PointXYZRGB)(pcl::PointXYZRGBNormal))
- PCL_INSTANTIATE(SampleConsensusModelParallelLine, (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA)(pcl::PointXYZRGB)(pcl::PointXYZRGBNormal))
#else
PCL_INSTANTIATE(SampleConsensusModelLine, PCL_XYZ_POINT_TYPES)
- PCL_INSTANTIATE(SampleConsensusModelParallelLine, PCL_XYZ_POINT_TYPES)
#endif
#endif // PCL_NO_PRECOMPILE
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/sample_consensus/impl/sac_model_parallel_line.hpp>
+
+#ifndef PCL_NO_PRECOMPILE
+#include <pcl/impl/instantiate.hpp>
+#include <pcl/point_types.h>
+// Instantiations of specific point types
+#ifdef PCL_ONLY_CORE_POINT_TYPES
+ PCL_INSTANTIATE(SampleConsensusModelParallelLine, (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA)(pcl::PointXYZRGB)(pcl::PointXYZRGBNormal))
+#else
+ PCL_INSTANTIATE(SampleConsensusModelParallelLine, PCL_XYZ_POINT_TYPES)
+#endif
+#endif // PCL_NO_PRECOMPILE
+
k_sqr_distances.reserve (max_nn);
unsigned yEnd = (bottom + 1) * input_->width + right + 1;
- register unsigned idx = top * input_->width + left;
+ unsigned idx = top * input_->width + left;
unsigned skip = input_->width - right + left - 1;
unsigned xEnd = idx - left + right + 1;
std::vector<int> indices;
indices.reserve (input_->size () >> (pyramid_level_ << 1));
- for (unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += ySkip, idx += input_->width * (ySkip - 1))
+ for (unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += ySkip, idx += input_->width * ySkip)
{
- for (unsigned xIdx = 0; xIdx < input_->width; xIdx += xSkip, idx += xSkip)
+ for (unsigned xIdx = 0, idx2 = idx; xIdx < input_->width; xIdx += xSkip, idx2 += xSkip)
{
- if (!mask_ [idx])
+ if (!mask_ [idx2])
continue;
- indices.push_back (idx);
+ indices.push_back (idx2);
}
}
* \param[in] k number of maximum nn interested in
* \param[in] queue priority queue with k NN
* \param[in] index index on point to be tested
- * \return wheter the top element changed or not.
+ * \return whether the top element changed or not.
*/
inline bool
testPoint (const PointT& query, unsigned k, std::priority_queue<Entry>& queue, unsigned index) const
set(SUBSYS_NAME segmentation)
set(SUBSYS_DESC "Point cloud segmentation library")
-set(SUBSYS_DEPS common geometry search sample_consensus kdtree octree features filters)
+set(SUBSYS_DEPS common geometry search sample_consensus kdtree octree features filters ml)
set(build TRUE)
PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
src/organized_connected_component_segmentation.cpp
src/organized_multi_plane_segmentation.cpp
src/planar_polygon_fusion.cpp
+ src/crf_segmentation.cpp
src/crf_normal_segmentation.cpp
+ src/unary_classifier.cpp
src/conditional_euclidean_clustering.cpp
src/supervoxel_clustering.cpp
src/grabcut_segmentation.cpp
src/progressive_morphological_filter.cpp
src/approximate_progressive_morphological_filter.cpp
+ src/lccp_segmentation.cpp
+ src/cpc_segmentation.cpp
)
# NOTE: boost/graph/boykov_kolmogorov_max_flow.hpp only exists for versions > 1.43
if(Boost_MAJOR_VERSION GREATER 1 OR Boost_MINOR_VERSION GREATER 43)
"include/pcl/${SUBSYS_NAME}/region_3d.h"
"include/pcl/${SUBSYS_NAME}/planar_region.h"
"include/pcl/${SUBSYS_NAME}/planar_polygon_fusion.h"
+ "include/pcl/${SUBSYS_NAME}/crf_segmentation.h"
"include/pcl/${SUBSYS_NAME}/crf_normal_segmentation.h"
+ "include/pcl/${SUBSYS_NAME}/unary_classifier.h"
"include/pcl/${SUBSYS_NAME}/conditional_euclidean_clustering.h"
"include/pcl/${SUBSYS_NAME}/supervoxel_clustering.h"
"include/pcl/${SUBSYS_NAME}/grabcut_segmentation.h"
"include/pcl/${SUBSYS_NAME}/progressive_morphological_filter.h"
"include/pcl/${SUBSYS_NAME}/approximate_progressive_morphological_filter.h"
+ "include/pcl/${SUBSYS_NAME}/lccp_segmentation.h"
+ "include/pcl/${SUBSYS_NAME}/cpc_segmentation.h"
)
# NOTE: boost/graph/boykov_kolmogorov_max_flow.hpp only exists for versions > 1.43
if(Boost_MAJOR_VERSION GREATER 1 OR Boost_MINOR_VERSION GREATER 43)
"include/pcl/${SUBSYS_NAME}/min_cut_segmentation.h"
)
endif()
+ # Random walker requires Eigen::Sparse module that is available since 3.1.0
+ if(NOT ("${EIGEN_VERSION}" VERSION_LESS 3.1.0))
+ list(APPEND incs
+ "include/pcl/${SUBSYS_NAME}/random_walker.h"
+ )
+ endif()
set(impl_incs
"include/pcl/${SUBSYS_NAME}/impl/extract_clusters.hpp"
"include/pcl/${SUBSYS_NAME}/impl/organized_connected_component_segmentation.hpp"
"include/pcl/${SUBSYS_NAME}/impl/organized_multi_plane_segmentation.hpp"
"include/pcl/${SUBSYS_NAME}/impl/planar_polygon_fusion.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/crf_segmentation.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/unary_classifier.hpp"
"include/pcl/${SUBSYS_NAME}/impl/crf_normal_segmentation.hpp"
"include/pcl/${SUBSYS_NAME}/impl/conditional_euclidean_clustering.hpp"
"include/pcl/${SUBSYS_NAME}/impl/supervoxel_clustering.hpp"
"include/pcl/${SUBSYS_NAME}/impl/grabcut_segmentation.hpp"
"include/pcl/${SUBSYS_NAME}/impl/progressive_morphological_filter.hpp"
"include/pcl/${SUBSYS_NAME}/impl/approximate_progressive_morphological_filter.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/lccp_segmentation.hpp"
+ "include/pcl/${SUBSYS_NAME}/impl/cpc_segmentation.hpp"
)
# NOTE: boost/graph/boykov_kolmogorov_max_flow.hpp only exists for versions > 1.43
if(Boost_MAJOR_VERSION GREATER 1 OR Boost_MINOR_VERSION GREATER 43)
"include/pcl/${SUBSYS_NAME}/impl/min_cut_segmentation.hpp"
)
endif()
+ # Random walker requires Eigen::Sparse module that is available since 3.1.0
+ if(NOT ("${EIGEN_VERSION}" VERSION_LESS 3.1.0))
+ list(APPEND impl_incs
+ "include/pcl/${SUBSYS_NAME}/impl/random_walker.hpp"
+ )
+ endif()
set(LIB_NAME "pcl_${SUBSYS_NAME}")
include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs})
- target_link_libraries("${LIB_NAME}" pcl_search pcl_sample_consensus pcl_filters pcl_features)
+ target_link_libraries("${LIB_NAME}" pcl_search pcl_sample_consensus pcl_filters pcl_ml pcl_features)
PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "")
# Install include files
#pragma GCC system_header
#endif
+#ifndef Q_MOC_RUN
// Marking all Boost headers as system headers to remove warnings
#include <boost/version.hpp>
#include <boost/make_shared.hpp>
#if (BOOST_VERSION >= 104400)
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>
#endif
-
+#endif
#endif // PCL_SEGMENTATION_BOOST_H_
#ifndef PCL_SEGMENTATION_CONDITIONAL_EUCLIDEAN_CLUSTERING_H_
#define PCL_SEGMENTATION_CONDITIONAL_EUCLIDEAN_CLUSTERING_H_
+#include <boost/function.hpp>
+
#include <pcl/pcl_base.h>
#include <pcl/search/pcl_search.h>
condition_function_ = condition_function;
}
+ /** \brief Set the condition that needs to hold for neighboring points to be considered part of the same cluster.
+ * This is an overloaded function provided for convenience. See the documentation for setConditionFunction(). */
+ inline void
+ setConditionFunction (boost::function<bool (const PointT&, const PointT&, float)> condition_function)
+ {
+ condition_function_ = condition_function;
+ }
+
/** \brief Set the spatial tolerance for new cluster candidates.
* \details Any two points within this distance from one another will need to evaluate a certain condition in order to be made part of the same cluster.
* The condition can be set using setConditionFunction().
SearcherPtr searcher_;
/** \brief The condition function that needs to hold for clustering */
- bool (*condition_function_) (const PointT&, const PointT&, float);
+ boost::function<bool (const PointT&, const PointT&, float)> condition_function_;
/** \brief The distance to scan for cluster candidates (default = 0.0) */
float cluster_tolerance_;
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_SEGMENTATION_CPC_SEGMENTATION_H_
+#define PCL_SEGMENTATION_CPC_SEGMENTATION_H_
+
+// common includes
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+// segmentation and sample consensus includes
+#include <pcl/segmentation/supervoxel_clustering.h>
+#include <pcl/segmentation/lccp_segmentation.h>
+#include <pcl/sample_consensus/sac.h>
+
+#include <pcl/sample_consensus/sac_model_plane.h>
+#include <pcl/segmentation/extract_clusters.h>
+
+#define PCL_INSTANTIATE_CPCSegmentation(T) template class PCL_EXPORTS pcl::CPCSegmentation<T>;
+
+namespace pcl
+{
+ /** \brief A segmentation algorithm partitioning a supervoxel graph. It uses planar cuts induced by local concavities for the recursive segmentation. Cuts are estimated using locally constrained directed RANSAC.
+ * \note If you use this in a scientific work please cite the following paper:
+ * M. Schoeler, J. Papon, F. Woergoetter
+ * Constrained Planar Cuts - Object Partitioning for Point Clouds
+ * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2015
+ * Inherits most of its functionality from \ref LCCPSegmentation
+ * \author Markus Schoeler (mschoeler@web.de)
+ * \ingroup segmentation
+ */
+ template <typename PointT>
+ class CPCSegmentation : public LCCPSegmentation<PointT>
+ {
+ typedef PointXYZINormal WeightSACPointType;
+ typedef LCCPSegmentation<PointT> LCCP;
+ // LCCP typedefs
+ typedef typename LCCP::EdgeID EdgeID;
+ typedef typename LCCP::EdgeIterator EdgeIterator;
+ // LCCP methods
+ using LCCP::calculateConvexConnections;
+ using LCCP::applyKconvexity;
+ using LCCP::doGrouping;
+ using LCCP::mergeSmallSegments;
+ // LCCP variables
+ using LCCP::sv_adjacency_list_;
+ using LCCP::k_factor_;
+ using LCCP::grouping_data_valid_;
+ using LCCP::sv_label_to_seg_label_map_;
+ using LCCP::sv_label_to_supervoxel_map_;
+ using LCCP::concavity_tolerance_threshold_;
+ using LCCP::seed_resolution_;
+ using LCCP::supervoxels_set_;
+
+ public:
+ CPCSegmentation ();
+ virtual
+ ~CPCSegmentation ();
+
+ /** \brief Merge supervoxels using cuts through local convexities. The input parameters are generated by using the \ref SupervoxelClustering class. To retrieve the output use the \ref relabelCloud method.
+ * \note There are three ways to retrieve the segmentation afterwards (inherited from \ref LCCPSegmentation): \ref relabelCloud, \ref getSegmentSupervoxelMap and \ref getSupervoxelSegmentMap*/
+ void
+ segment ();
+
+ /** \brief Determines if we want to use cutting planes
+ * \param[in] max_cuts Maximum number of cuts
+ * \param[in] cutting_min_segments Minimum segment size for cutting
+ * \param[in] cutting_min_score Minimum score a proposed cut has to achieve for being performed
+ * \param[in] locally_constrained Decide if we constrain our cuts locally
+ * \param[in] directed_cutting Decide if we prefer cuts perpendicular to the edge-direction
+ * \param[in] clean_cutting Decide if we cut only edges with supervoxels on opposite sides of the plane (clean) or all edges within the seed_resolution_ distance to the plane (not clean). The later was used in the paper.
+ */
+ inline void
+ setCutting (const uint32_t max_cuts = 20,
+ const uint32_t cutting_min_segments = 0,
+ const float cutting_min_score = 0.16,
+ const bool locally_constrained = true,
+ const bool directed_cutting = true,
+ const bool clean_cutting = false)
+ {
+ max_cuts_ = max_cuts;
+ min_segment_size_for_cutting_ = cutting_min_segments;
+ min_cut_score_ = cutting_min_score;
+ use_local_constrains_ = locally_constrained;
+ use_directed_weights_ = directed_cutting;
+ use_clean_cutting_ = clean_cutting;
+ }
+
+ /** \brief Set the number of iterations for the weighted RANSAC step (best cut estimations)
+ * \param[in] ransac_iterations The number of iterations */
+ inline void
+ setRANSACIterations (const uint32_t ransac_iterations)
+ {
+ ransac_itrs_ = ransac_iterations;
+ }
+
+ private:
+
+ /** \brief Used in for CPC to find and fit cutting planes to the pointcloud.
+ * \note Is used recursively
+ * \param[in] depth_levels_left When first calling the function set this parameter to the maximum levels you want to cut down */
+ void
+ applyCuttingPlane (uint32_t depth_levels_left);
+
+ /// *** Parameters *** ///
+
+ /** \brief Maximum number of cuts */
+ uint32_t max_cuts_;
+
+ /** \brief Minimum segment size for cutting */
+ uint32_t min_segment_size_for_cutting_;
+
+ /** \brief Cut_score threshold */
+ float min_cut_score_;
+
+ /** \brief Use local constrains for cutting */
+ bool use_local_constrains_;
+
+ /** \brief Use directed weights for the cutting */
+ bool use_directed_weights_;
+
+ /** \brief Use clean cutting */
+ bool use_clean_cutting_;
+
+ /** \brief Interations for RANSAC */
+ uint32_t ransac_itrs_;
+
+
+/******************************************* Directional weighted RANSAC declarations ******************************************************************/
+ /** \brief @b WeightedRandomSampleConsensus represents an implementation of the Directionally Weighted RANSAC algorithm, as described in: "Constrained Planar Cuts - Part Segmentation for Point Clouds", CVPR 2015, M. Schoeler, J. Papon, F. Wörgötter.
+ * \note It only uses points with a weight > 0 for the model calculation, but uses all points for the evaluation (scoring of the model)
+ * Only use in conjunction with sac_model_plane
+ * If you use this in a scientific work please cite the following paper:
+ * M. Schoeler, J. Papon, F. Woergoetter
+ * Constrained Planar Cuts - Object Partitioning for Point Clouds
+ * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2015
+ * \author Markus Schoeler (mschoeler@web.de)
+ * \ingroup segmentation
+ */
+
+ class WeightedRandomSampleConsensus : public SampleConsensus<WeightSACPointType>
+ {
+ typedef typename SampleConsensusModel<WeightSACPointType>::Ptr SampleConsensusModelPtr;
+
+ public:
+ typedef boost::shared_ptr<WeightedRandomSampleConsensus> Ptr;
+ typedef boost::shared_ptr<const WeightedRandomSampleConsensus> ConstPtr;
+
+ /** \brief WeightedRandomSampleConsensus (Weighted RAndom SAmple Consensus) main constructor
+ * \param[in] model a Sample Consensus model
+ * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
+ */
+ WeightedRandomSampleConsensus (const SampleConsensusModelPtr &model,
+ bool random = false)
+ : SampleConsensus<WeightSACPointType> (model, random)
+ {
+ initialize ();
+ }
+
+ /** \brief WeightedRandomSampleConsensus (Weighted RAndom SAmple Consensus) main constructor
+ * \param[in] model a Sample Consensus model
+ * \param[in] threshold distance to model threshold
+ * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
+ */
+ WeightedRandomSampleConsensus (const SampleConsensusModelPtr &model,
+ double threshold,
+ bool random = false)
+ : SampleConsensus<WeightSACPointType> (model, threshold, random)
+ {
+ initialize ();
+ }
+
+ /** \brief Compute the actual model and find the inliers
+ * \param[in] debug_verbosity_level enable/disable on-screen debug information and set the verbosity level
+ */
+ bool
+ computeModel (int debug_verbosity_level = 0);
+
+ /** \brief Set the weights for the input points
+ * \param[in] weights Weights for input samples. Negative weights are counted as penalty.
+ */
+ void
+ setWeights (const std::vector<double> &weights,
+ const bool directed_weights = false)
+ {
+ if (weights.size () != full_cloud_pt_indices_->size ())
+ {
+ PCL_ERROR ("[pcl::WeightedRandomSampleConsensus::setWeights] Cannot assign weights. Weight vector needs to have the same length as the input pointcloud\n");
+ return;
+ }
+ weights_ = weights;
+ model_pt_indices_->clear ();
+ for (size_t i = 0; i < weights.size (); ++i)
+ {
+ if (weights[i] > std::numeric_limits<double>::epsilon ())
+ model_pt_indices_->push_back (i);
+ }
+ use_directed_weights_ = directed_weights;
+ }
+
+ /** \brief Get the best score
+ * \returns The best score found.
+ */
+ double
+ getBestScore () const
+ {
+ return (best_score_);
+ }
+
+ protected:
+ /** \brief Initialize the model parameters. Called by the constructors. */
+ void
+ initialize ()
+ {
+ // Maximum number of trials before we give up.
+ max_iterations_ = 10000;
+ use_directed_weights_ = false;
+ model_pt_indices_ = boost::shared_ptr<std::vector<int> > (new std::vector<int> ());
+ full_cloud_pt_indices_.reset (new std::vector<int> (* (sac_model_->getIndices ())));
+ point_cloud_ptr_ = sac_model_->getInputCloud ();
+ }
+
+ /** \brief weight each positive weight point by the inner product between the normal and the plane normal */
+ bool use_directed_weights_;
+
+ /** \brief vector of weights assigned to points. Set by the setWeights-method */
+ std::vector<double> weights_;
+
+ /** \brief The indices used for estimating the RANSAC model. Only those whose weight is > 0 */
+ boost::shared_ptr<std::vector<int> > model_pt_indices_;
+
+ /** \brief The complete list of indices used for the model evaluation */
+ boost::shared_ptr<std::vector<int> > full_cloud_pt_indices_;
+
+ /** \brief Pointer to the input PointCloud */
+ boost::shared_ptr<const pcl::PointCloud<WeightSACPointType> > point_cloud_ptr_;
+
+ /** \brief Highest score found so far */
+ double best_score_;
+ };
+
+ };
+}
+
+#ifdef PCL_NO_PRECOMPILE
+ #include <pcl/segmentation/impl/cpc_segmentation.hpp>
+#elif defined(PCL_ONLY_CORE_POINT_TYPES)
+ //pcl::PointXYZINormal is not a core point type (so we cannot use the precompiled classes here)
+ #include <pcl/sample_consensus/impl/sac_model_plane.hpp>
+ #include <pcl/segmentation/impl/extract_clusters.hpp>
+#endif // PCL_NO_PRECOMPILE / PCL_ONLY_CORE_POINT_TYPES
+
+#endif // PCL_SEGMENTATION_CPC_SEGMENTATION_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author : Christian Potthast
+ * Email : potthast@usc.edu
+ *
+ */
+
+#ifndef PCL_CRF_SEGMENTATION_H_
+#define PCL_CRF_SEGMENTATION_H_
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <pcl/ml/densecrf.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/voxel_grid_label.h>
+
+//#include <pcl/ml/densecrfORI.h>
+
+namespace pcl
+{
+ /** \brief
+ *
+ */
+ template <typename PointT>
+ class PCL_EXPORTS CrfSegmentation
+ {
+ public:
+
+ //typedef boost::shared_ptr<std::vector<int> > pcl::IndicesPtr;
+
+
+ /** \brief Constructor that sets default values for member variables. */
+ CrfSegmentation ();
+
+ /** \brief This destructor destroys the cloud...
+ *
+ */
+ ~CrfSegmentation ();
+
+ /** \brief This method sets the input cloud.
+ * \param[in] input_cloud input point cloud
+ */
+ void
+ setInputCloud (typename pcl::PointCloud<PointT>::Ptr input_cloud);
+
+ void
+ setAnnotatedCloud (typename pcl::PointCloud<pcl::PointXYZRGBL>::Ptr anno_cloud);
+
+ void
+ setNormalCloud (typename pcl::PointCloud<pcl::PointNormal>::Ptr normal_cloud);
+
+
+ /** \brief Set the leaf size for the voxel grid.
+ * \param[in] x leaf size x-axis
+ * \param[in] y leaf size y-axis
+ * \param[in] z leaf size z-axis
+ */
+ void
+ setVoxelGridLeafSize (const float x, const float y, const float z);
+
+ void
+ setNumberOfIterations (unsigned int n_iterations = 10) {n_iterations_ = n_iterations;};
+
+ /** \brief This method simply launches the segmentation algorithm */
+ void
+ segmentPoints (pcl::PointCloud<pcl::PointXYZRGBL> &output);
+
+ /** \brief Create a voxel grid to discretize the scene */
+ void
+ createVoxelGrid ();
+
+ /** \brief Get the data from the voxel grid and convert it into a vector */
+ void
+ createDataVectorFromVoxelGrid ();
+
+
+ void
+ createUnaryPotentials (std::vector<float> &unary,
+ std::vector<int> &colors,
+ unsigned int n_labels);
+
+
+ /** \brief Set the smoothness kernel parameters.
+ * \param[in] sx standard deviation x
+ * \param[in] sy standard deviation y
+ * \param[in] sz standard deviation z
+ * \param[in] w weight
+ */
+ void
+ setSmoothnessKernelParameters (const float sx, const float sy, const float sz, const float w);
+
+ /** \brief Set the appearanche kernel parameters.
+ * \param[in] sx standard deviation x
+ * \param[in] sy standard deviation y
+ * \param[in] sz standard deviation z
+ * \param[in] sr standard deviation red
+ * \param[in] sg standard deviation green
+ * \param[in] sb standard deviation blue
+ * \param[in] w weight
+ */
+ void
+ setAppearanceKernelParameters (float sx, float sy, float sz,
+ float sr, float sg, float sb,
+ float w);
+
+
+ void
+ setSurfaceKernelParameters (float sx, float sy, float sz,
+ float snx, float sny, float snz,
+ float w);
+
+
+ protected:
+ /** \brief Voxel grid to discretize the scene */
+ typename pcl::VoxelGrid<PointT> voxel_grid_;
+
+ /** \brief input cloud that will be segmented. */
+ typename pcl::PointCloud<PointT>::Ptr input_cloud_;
+ typename pcl::PointCloud<pcl::PointXYZRGBL>::Ptr anno_cloud_;
+ typename pcl::PointCloud<pcl::PointNormal>::Ptr normal_cloud_;
+
+ /** \brief voxel grid filtered cloud. */
+ typename pcl::PointCloud<PointT>::Ptr filtered_cloud_;
+ typename pcl::PointCloud<pcl::PointXYZRGBL>::Ptr filtered_anno_;
+ typename pcl::PointCloud<pcl::PointNormal>::Ptr filtered_normal_;
+
+ /** \brief indices of the filtered cloud. */
+ //typename pcl::VoxelGrid::IndicesPtr cloud_indices_;
+
+ /** \brief Voxel grid leaf size */
+ Eigen::Vector4f voxel_grid_leaf_size_;
+
+ /** \brief Voxel grid dimensions */
+ Eigen::Vector3i dim_;
+
+ /** \brief voxel grid data points
+ packing order [x0y0z0, x1y0z0,x2y0z0,...,x0y1z0,x1y1z0,...,x0y0z1,x1y0z1,...]
+ */
+ std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > data_;
+
+ std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > color_;
+
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > normal_;
+
+ /** \brief smoothness kernel parameters
+ * [0] = standard deviation x
+ * [1] = standard deviation y
+ * [2] = standard deviation z
+ * [3] = weight
+ */
+ float smoothness_kernel_param_[4];
+
+ /** \brief appearance kernel parameters
+ * [0] = standard deviation x
+ * [1] = standard deviation y
+ * [2] = standard deviation z
+ * [3] = standard deviation red
+ * [4] = standard deviation green
+ * [5] = standard deviation blue
+ * [6] = weight
+ */
+ float appearance_kernel_param_[7];
+
+ float surface_kernel_param_[7];
+
+
+ unsigned int n_iterations_;
+
+
+ /** \brief Contains normals of the points that will be segmented. */
+ //typename pcl::PointCloud<pcl::Normal>::Ptr normals_;
+
+ /** \brief Stores the cloud that will be segmented. */
+ //typename pcl::PointCloud<PointT>::Ptr cloud_for_segmentation_;
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+}
+
+#ifdef PCL_NO_PRECOMPILE
+#include <pcl/segmentation/impl/crf_segmentation.hpp>
+#endif
+
+#endif
private:
/// parents_ flag for terminal state
- static const int TERMINAL = -1;
+ static const int TERMINAL; // -1
/// search tree (also uses cut_)
std::vector<std::pair<int, edge_pair> > parents_;
/// doubly-linked list (prev, next)
{
// ...then test for lower points within the cell
PointT p = input_->points[i];
- int row = std::floor(p.y - global_min.y ());
- int col = std::floor(p.x - global_min.x ());
+ int row = std::floor((p.y - global_min.y ()) / cell_size_);
+ int col = std::floor((p.x - global_min.x ()) / cell_size_);
if (p.z < A (row, col) || pcl_isnan (A (row, col)))
{
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_SEGMENTATION_IMPL_CPC_SEGMENTATION_HPP_
+#define PCL_SEGMENTATION_IMPL_CPC_SEGMENTATION_HPP_
+
+#include <pcl/segmentation/cpc_segmentation.h>
+
+template <typename PointT>
+pcl::CPCSegmentation<PointT>::CPCSegmentation () :
+ max_cuts_ (20),
+ min_segment_size_for_cutting_ (400),
+ min_cut_score_ (0.16),
+ use_local_constrains_ (true),
+ use_directed_weights_ (true),
+ ransac_itrs_ (10000)
+{
+}
+
+template <typename PointT>
+pcl::CPCSegmentation<PointT>::~CPCSegmentation ()
+{
+}
+
+template <typename PointT> void
+pcl::CPCSegmentation<PointT>::segment ()
+{
+ if (supervoxels_set_)
+ {
+ // Calculate for every Edge if the connection is convex or invalid
+ // This effectively performs the segmentation.
+ calculateConvexConnections (sv_adjacency_list_);
+
+ // Correct edge relations using extended convexity definition if k>0
+ applyKconvexity (k_factor_);
+
+ // Determine wether to use cutting planes
+ doGrouping ();
+
+ grouping_data_valid_ = true;
+
+ applyCuttingPlane (max_cuts_);
+
+ // merge small segments
+ mergeSmallSegments ();
+ }
+ else
+ PCL_WARN ("[pcl::CPCSegmentation::segment] WARNING: Call function setInputSupervoxels first. Nothing has been done. \n");
+}
+
+template <typename PointT> void
+pcl::CPCSegmentation<PointT>::applyCuttingPlane (uint32_t depth_levels_left)
+{
+ typedef std::map<uint32_t, pcl::PointCloud<WeightSACPointType>::Ptr> SegLabel2ClusterMap;
+
+ pcl::console::print_info ("Cutting at level %d (maximum %d)\n", max_cuts_ - depth_levels_left + 1, max_cuts_);
+ // stop if we reached the 0 level
+ if (depth_levels_left <= 0)
+ return;
+
+ SegLabel2ClusterMap seg_to_edge_points_map;
+ std::map<uint32_t, std::vector<EdgeID> > seg_to_edgeIDs_map;
+ EdgeIterator edge_itr, edge_itr_end, next_edge;
+ boost::tie (edge_itr, edge_itr_end) = boost::edges (sv_adjacency_list_);
+ for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
+ {
+ next_edge++; // next_edge iterator is neccessary, because removing an edge invalidates the iterator to the current edge
+ uint32_t source_sv_label = sv_adjacency_list_[boost::source (*edge_itr, sv_adjacency_list_)];
+ uint32_t target_sv_label = sv_adjacency_list_[boost::target (*edge_itr, sv_adjacency_list_)];
+
+ uint32_t source_segment_label = sv_label_to_seg_label_map_[source_sv_label];
+ uint32_t target_segment_label = sv_label_to_seg_label_map_[target_sv_label];
+
+ // do not process edges which already split two segments
+ if (source_segment_label != target_segment_label)
+ continue;
+
+ // if edge has been used for cutting already do not use it again
+ if (sv_adjacency_list_[*edge_itr].used_for_cutting)
+ continue;
+ // get centroids of vertices
+ const pcl::PointXYZRGBA source_centroid = sv_label_to_supervoxel_map_[source_sv_label]->centroid_;
+ const pcl::PointXYZRGBA target_centroid = sv_label_to_supervoxel_map_[target_sv_label]->centroid_;
+
+ // stores the information about the edge cloud (used for the weighted ransac)
+ // we use the normal to express the direction of the connection
+ // we use the intensity to express the normal differences between supervoxel patches. <=0: Convex, >0: Concave
+ WeightSACPointType edge_centroid;
+ edge_centroid.getVector3fMap () = (source_centroid.getVector3fMap () + target_centroid.getVector3fMap ()) / 2;
+
+ // we use the normal to express the direction of the connection!
+ edge_centroid.getNormalVector3fMap () = (target_centroid.getVector3fMap () - source_centroid.getVector3fMap ()).normalized ();
+
+ // we use the intensity to express the normal differences between supervoxel patches. <=0: Convex, >0: Concave
+ edge_centroid.intensity = sv_adjacency_list_[*edge_itr].is_convex ? -sv_adjacency_list_[*edge_itr].normal_difference : sv_adjacency_list_[*edge_itr].normal_difference;
+ if (seg_to_edge_points_map.find (source_segment_label) == seg_to_edge_points_map.end ())
+ {
+ seg_to_edge_points_map[source_segment_label] = pcl::PointCloud<WeightSACPointType>::Ptr (new pcl::PointCloud<WeightSACPointType> ());
+ }
+ seg_to_edge_points_map[source_segment_label]->push_back (edge_centroid);
+ seg_to_edgeIDs_map[source_segment_label].push_back (*edge_itr);
+ }
+ bool cut_found = false;
+ // do the following processing for each segment separately
+ for (SegLabel2ClusterMap::iterator itr = seg_to_edge_points_map.begin (); itr != seg_to_edge_points_map.end (); ++itr)
+ {
+ // if too small do not process
+ if (itr->second->size () < min_segment_size_for_cutting_)
+ {
+ continue;
+ }
+
+ std::vector<double> weights;
+ weights.resize (itr->second->size ());
+ for (std::size_t cp = 0; cp < itr->second->size (); ++cp)
+ {
+ float& cur_weight = itr->second->points[cp].intensity;
+ cur_weight = cur_weight < concavity_tolerance_threshold_ ? 0 : 1;
+ weights[cp] = cur_weight;
+ }
+
+ pcl::PointCloud<WeightSACPointType>::Ptr edge_cloud_cluster = itr->second;
+ pcl::SampleConsensusModelPlane<WeightSACPointType>::Ptr model_p (new pcl::SampleConsensusModelPlane<WeightSACPointType> (edge_cloud_cluster));
+
+ WeightedRandomSampleConsensus weight_sac (model_p, seed_resolution_, true);
+
+ weight_sac.setWeights (weights, use_directed_weights_);
+ weight_sac.setMaxIterations (ransac_itrs_);
+
+ // if not enough inliers are found
+ if (!weight_sac.computeModel ())
+ {
+ continue;
+ }
+
+ Eigen::VectorXf model_coefficients;
+ weight_sac.getModelCoefficients (model_coefficients);
+
+ model_coefficients[3] += std::numeric_limits<float>::epsilon ();
+
+ std::vector<int> support_indices;
+ weight_sac.getInliers (support_indices);
+
+ // the support_indices which are actually cut (if not locally constrain: cut_support_indices = support_indices
+ std::vector<int> cut_support_indices;
+
+ if (use_local_constrains_)
+ {
+ Eigen::Vector3f plane_normal (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
+ // Cut the connections.
+ // We only interate through the points which are within the support (when we are local, otherwise all points in the segment).
+ // We also just acutally cut when the edge goes through the plane. This is why we check the planedistance
+ std::vector<pcl::PointIndices> cluster_indices;
+ pcl::EuclideanClusterExtraction<WeightSACPointType> euclidean_clusterer;
+ pcl::search::KdTree<WeightSACPointType>::Ptr tree (new pcl::search::KdTree<WeightSACPointType>);
+ tree->setInputCloud (edge_cloud_cluster);
+ euclidean_clusterer.setClusterTolerance (seed_resolution_);
+ euclidean_clusterer.setMinClusterSize (1);
+ euclidean_clusterer.setMaxClusterSize (25000);
+ euclidean_clusterer.setSearchMethod (tree);
+ euclidean_clusterer.setInputCloud (edge_cloud_cluster);
+ euclidean_clusterer.setIndices (boost::make_shared <std::vector <int> > (support_indices));
+ euclidean_clusterer.extract (cluster_indices);
+// sv_adjacency_list_[seg_to_edgeID_map[itr->first][point_index]].used_for_cutting = true;
+
+ for (size_t cc = 0; cc < cluster_indices.size (); ++cc)
+ {
+ // get centroids of vertices
+ int cluster_concave_pts = 0;
+ float cluster_score = 0;
+// std::cout << "Cluster has " << cluster_indices[cc].indices.size () << " points" << std::endl;
+ for (size_t cp = 0; cp < cluster_indices[cc].indices.size (); ++cp)
+ {
+ int current_index = cluster_indices[cc].indices[cp];
+ double index_score;
+ if (use_directed_weights_)
+ index_score = weights[current_index] * 1.414 * (fabsf (plane_normal.dot (edge_cloud_cluster->at (current_index).getNormalVector3fMap ())));
+ else
+ index_score = weights[current_index];
+ cluster_score += index_score;
+ if (weights[current_index] > 0)
+ ++cluster_concave_pts;
+ }
+ // check if the score is below the threshold. If that is the case this segment should not be split
+ cluster_score = cluster_score * 1.0 / cluster_indices[cc].indices.size ();
+// std::cout << "Cluster score: " << cluster_score << std::endl;
+ if (cluster_score >= min_cut_score_)
+ {
+ cut_support_indices.insert (cut_support_indices.end (), cluster_indices[cc].indices.begin (), cluster_indices[cc].indices.end ());
+ }
+ }
+ if (cut_support_indices.size () == 0)
+ {
+// std::cout << "Could not find planes which exceed required minumum score (threshold " << min_cut_score_ << "), not cutting" << std::endl;
+ continue;
+ }
+ }
+ else
+ {
+ double current_score = weight_sac.getBestScore ();
+ cut_support_indices = support_indices;
+ // check if the score is below the threshold. If that is the case this segment should not be split
+ if (current_score < min_cut_score_)
+ {
+// std::cout << "Score too low, no cutting" << std::endl;
+ continue;
+ }
+ }
+
+ int number_connections_cut = 0;
+ for (size_t cs = 0; cs < cut_support_indices.size (); ++cs)
+ {
+ const int point_index = cut_support_indices[cs];
+
+ if (use_clean_cutting_)
+ {
+ // skip edges where both centroids are on one side of the cutting plane
+ uint32_t source_sv_label = sv_adjacency_list_[boost::source (seg_to_edgeIDs_map[itr->first][point_index], sv_adjacency_list_)];
+ uint32_t target_sv_label = sv_adjacency_list_[boost::target (seg_to_edgeIDs_map[itr->first][point_index], sv_adjacency_list_)];
+ // get centroids of vertices
+ const pcl::PointXYZRGBA source_centroid = sv_label_to_supervoxel_map_[source_sv_label]->centroid_;
+ const pcl::PointXYZRGBA target_centroid = sv_label_to_supervoxel_map_[target_sv_label]->centroid_;
+ // this makes a clean cut
+ if (pcl::pointToPlaneDistanceSigned (source_centroid, model_coefficients) * pcl::pointToPlaneDistanceSigned (target_centroid, model_coefficients) > 0)
+ {
+ continue;
+ }
+ }
+ sv_adjacency_list_[seg_to_edgeIDs_map[itr->first][point_index]].used_for_cutting = true;
+ if (sv_adjacency_list_[seg_to_edgeIDs_map[itr->first][point_index]].is_valid)
+ {
+ ++number_connections_cut;
+ sv_adjacency_list_[seg_to_edgeIDs_map[itr->first][point_index]].is_valid = false;
+ }
+ }
+// std::cout << "We cut " << number_connections_cut << " connections" << std::endl;
+ if (number_connections_cut > 0)
+ cut_found = true;
+ }
+
+ // if not cut has been performed we can stop the recursion
+ if (cut_found)
+ {
+ doGrouping ();
+ --depth_levels_left;
+ applyCuttingPlane (depth_levels_left);
+ }
+ else
+ pcl::console::print_info ("Could not find any more cuts, stopping recursion\n");
+}
+
+/******************************************* Directional weighted RANSAC definitions ******************************************************************/
+
+
+template <typename PointT> bool
+pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel (int)
+{
+ // Warn and exit if no threshold was set
+ if (threshold_ == std::numeric_limits<double>::max ())
+ {
+ PCL_ERROR ("[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] No threshold set!\n");
+ return (false);
+ }
+
+ iterations_ = 0;
+ best_score_ = -std::numeric_limits<double>::max ();
+
+ std::vector<int> selection;
+ Eigen::VectorXf model_coefficients;
+
+ unsigned skipped_count = 0;
+ // supress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
+ const unsigned max_skip = max_iterations_ * 10;
+
+ // Iterate
+ while (iterations_ < max_iterations_ && skipped_count < max_skip)
+ {
+ // Get X samples which satisfy the model criteria and which have a weight > 0
+ sac_model_->setIndices (model_pt_indices_);
+ sac_model_->getSamples (iterations_, selection);
+
+ if (selection.empty ())
+ {
+ PCL_ERROR ("[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] No samples could be selected!\n");
+ break;
+ }
+
+ // Search for inliers in the point cloud for the current plane model M
+ if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
+ {
+ //++iterations_;
+ ++skipped_count;
+ continue;
+ }
+ // weight distances to get the score (only using connected inliers)
+ sac_model_->setIndices (full_cloud_pt_indices_);
+
+ boost::shared_ptr<std::vector<int> > current_inliers (new std::vector<int>);
+ sac_model_->selectWithinDistance (model_coefficients, threshold_, *current_inliers);
+ double current_score = 0;
+ Eigen::Vector3f plane_normal (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
+ for (size_t i = 0; i < current_inliers->size (); ++i)
+ {
+ int current_index = current_inliers->at (i);
+ double index_score;
+ if (use_directed_weights_)
+ // the sqrt(2) factor was used in the paper and was meant for making the scores better comparable between directed and undirected weights
+ index_score = weights_[current_index] * 1.414 * (fabsf (plane_normal.dot (point_cloud_ptr_->at (current_index).getNormalVector3fMap ())));
+ else
+ index_score = weights_[current_index];
+
+ current_score += index_score;
+ }
+ // normalize by the total number of inliers
+ current_score = current_score * 1.0 / current_inliers->size ();
+
+ // Better match ?
+ if (current_score > best_score_)
+ {
+ best_score_ = current_score;
+ // Save the current model/inlier/coefficients selection as being the best so far
+ model_ = selection;
+ model_coefficients_ = model_coefficients;
+ }
+
+ ++iterations_;
+ PCL_DEBUG ("[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] Trial %d (max %d): score is %f (best is: %f so far).\n", iterations_, max_iterations_, current_score, best_score_);
+ if (iterations_ > max_iterations_)
+ {
+ PCL_DEBUG ("[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n");
+ break;
+ }
+ }
+// std::cout << "Took us " << iterations_ - 1 << " iterations" << std::endl;
+ PCL_DEBUG ("[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] Model: %lu size, %f score.\n", model_.size (), best_score_);
+
+ if (model_.empty ())
+ {
+ inliers_.clear ();
+ return (false);
+ }
+
+ // Get the set of inliers that correspond to the best model found so far
+ sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_);
+ return (true);
+}
+
+#endif // PCL_SEGMENTATION_IMPL_CPC_SEGMENTATION_HPP_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author : Christian Potthast
+ * Email : potthast@usc.edu
+ *
+ */
+
+#ifndef PCL_CRF_SEGMENTATION_HPP_
+#define PCL_CRF_SEGMENTATION_HPP_
+
+#include <pcl/segmentation/crf_segmentation.h>
+
+#include <pcl/common/io.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <time.h>
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT>
+pcl::CrfSegmentation<PointT>::CrfSegmentation () :
+ voxel_grid_ (),
+ input_cloud_ (new pcl::PointCloud<PointT>),
+ normal_cloud_ (new pcl::PointCloud<pcl::PointNormal>),
+ filtered_cloud_ (new pcl::PointCloud<PointT>),
+ filtered_anno_ (new pcl::PointCloud<pcl::PointXYZRGBL>),
+ filtered_normal_ (new pcl::PointCloud<pcl::PointNormal>),
+ voxel_grid_leaf_size_ (Eigen::Vector4f (0.001f, 0.001f, 0.001f, 0.0f))
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT>
+pcl::CrfSegmentation<PointT>::~CrfSegmentation ()
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::CrfSegmentation<PointT>::setInputCloud (typename pcl::PointCloud<PointT>::Ptr input_cloud)
+{
+ if (input_cloud_ != NULL)
+ input_cloud_.reset ();
+
+ input_cloud_ = input_cloud;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::CrfSegmentation<PointT>::setAnnotatedCloud (typename pcl::PointCloud<pcl::PointXYZRGBL>::Ptr anno_cloud)
+{
+ if (anno_cloud_ != NULL)
+ anno_cloud_.reset ();
+
+ anno_cloud_ = anno_cloud;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::CrfSegmentation<PointT>::setNormalCloud (typename pcl::PointCloud<pcl::PointNormal>::Ptr normal_cloud)
+{
+ if (normal_cloud_ != NULL)
+ normal_cloud_.reset ();
+
+ normal_cloud_ = normal_cloud;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::CrfSegmentation<PointT>::setVoxelGridLeafSize (const float x, const float y, const float z)
+{
+ voxel_grid_leaf_size_.x () = x;
+ voxel_grid_leaf_size_.y () = y;
+ voxel_grid_leaf_size_.z () = z;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::CrfSegmentation<PointT>::setSmoothnessKernelParameters (const float sx, const float sy, const float sz,
+ const float w)
+{
+ smoothness_kernel_param_[0] = sx;
+ smoothness_kernel_param_[1] = sy;
+ smoothness_kernel_param_[2] = sz;
+ smoothness_kernel_param_[3] = w;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::CrfSegmentation<PointT>::setAppearanceKernelParameters (float sx, float sy, float sz,
+ float sr, float sg, float sb,
+ float w)
+{
+ appearance_kernel_param_[0] = sx;
+ appearance_kernel_param_[1] = sy;
+ appearance_kernel_param_[2] = sz;
+ appearance_kernel_param_[3] = sr;
+ appearance_kernel_param_[4] = sg;
+ appearance_kernel_param_[5] = sb;
+ appearance_kernel_param_[6] = w;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::CrfSegmentation<PointT>::setSurfaceKernelParameters (float sx, float sy, float sz,
+ float snx, float sny, float snz,
+ float w)
+{
+ surface_kernel_param_[0] = sx;
+ surface_kernel_param_[1] = sy;
+ surface_kernel_param_[2] = sz;
+ surface_kernel_param_[3] = snx;
+ surface_kernel_param_[4] = sny;
+ surface_kernel_param_[5] = snz;
+ surface_kernel_param_[6] = w;
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::CrfSegmentation<PointT>::createVoxelGrid ()
+{
+ // Filter the input cloud
+ // Set the voxel grid input cloud
+ voxel_grid_.setInputCloud (input_cloud_);
+ // Set the voxel grid leaf size
+ voxel_grid_.setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () );
+ // Only downsample XYZ (if this is set to false, color values set to 0)
+ voxel_grid_.setDownsampleAllData (true);
+ // Save leaf information
+ //voxel_grid_.setSaveLeafLayout (true);
+ // apply the filter
+ voxel_grid_.filter (*filtered_cloud_);
+
+ // Filter the annotated cloud
+ if (anno_cloud_->points.size () > 0)
+ {
+ pcl::VoxelGridLabel vg;
+
+ vg.setInputCloud (anno_cloud_);
+ // Set the voxel grid leaf size
+ vg.setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () );
+ // Only downsample XYZ
+ vg.setDownsampleAllData (true);
+ // Save leaf information
+ //vg.setSaveLeafLayout (false);
+ // apply the filter
+ vg.filter (*filtered_anno_);
+ }
+
+ // Filter the annotated cloud
+ if (normal_cloud_->points.size () > 0)
+ {
+ pcl::VoxelGrid<pcl::PointNormal> vg;
+ vg.setInputCloud (normal_cloud_);
+ // Set the voxel grid leaf size
+ vg.setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () );
+ // Only downsample XYZ
+ vg.setDownsampleAllData (true);
+ // Save leaf information
+ //vg.setSaveLeafLayout (false);
+ // apply the filter
+ vg.filter (*filtered_normal_);
+ }
+
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::CrfSegmentation<PointT>::createDataVectorFromVoxelGrid ()
+{
+ // get the dimension of the voxel grid
+ //Eigen::Vector3i min_b, max_b;
+ //min_b = voxel_grid_.getMinBoxCoordinates ();
+ //max_b = voxel_grid_.getMaxBoxCoordinates ();
+
+ //std::cout << "min_b: " << min_b.x () << " " << min_b.y () << " " << min_b.z () << std::endl;
+ //std::cout << "max_b: " << max_b.x () << " " << max_b.y () << " " << max_b.z () << std::endl;
+
+ // compute the voxel grid dimensions
+ //dim_.x () = abs (max_b.x () - min_b.x ());
+ //dim_.y () = abs (max_b.y () - min_b.y ());
+ //dim_.z () = abs (max_b.z () - min_b.z ());
+
+ //std::cout << dim_.x () * dim_.y () * dim_.z () << std::endl;
+
+ // reserve the space for the data vector
+ //data_.reserve (dim_.x () * dim_.y () * dim_.z ());
+
+/*
+ std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > data;
+ std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > color;
+ // fill the data vector
+ for (int kk = min_b.z (), k = 0; kk <= max_b.z (); kk++, k++)
+ {
+ for (int jj = min_b.y (), j = 0; jj <= max_b.y (); jj++, j++)
+ {
+ for (int ii = min_b.x (), i = 0; ii <= max_b.x (); ii++, i++)
+ {
+ Eigen::Vector3i ijk (ii, jj, kk);
+ int index = voxel_grid_.getCentroidIndexAt (ijk);
+ if (index != -1)
+ {
+ data_.push_back (Eigen::Vector3i (i, j, k));
+ color_.push_back (input_cloud_->points[index].getRGBVector3i ());
+ }
+ }
+ }
+ }
+*/
+
+
+/*
+ // get the size of the input fields
+ std::vector< pcl::PCLPointField > fields;
+ pcl::getFields (*input_cloud_, fields);
+
+ for (int i = 0; i < fields.size (); i++)
+ std::cout << fields[i] << std::endl;
+*/
+
+
+ // reserve space for the data vector
+ data_.resize (filtered_cloud_->points.size ());
+
+ std::vector< pcl::PCLPointField > fields;
+ // check if we have color data
+ bool color_data = false;
+ int rgba_index = -1;
+ rgba_index = pcl::getFieldIndex (*input_cloud_, "rgb", fields);
+ if (rgba_index == -1)
+ rgba_index = pcl::getFieldIndex (*input_cloud_, "rgba", fields);
+ if (rgba_index >= 0)
+ {
+ color_data = true;
+ color_.resize (filtered_cloud_->points.size ());
+ }
+
+
+/*
+ // check if we have normal data
+ bool normal_data = false;
+ int normal_index = -1;
+ rgba_index = pcl::getFieldIndex (*input_cloud_, "normal_x", fields);
+ if (rgba_index >= 0)
+ {
+ normal_data = true;
+ normal_.resize (filtered_cloud_->points.size ());
+ }
+*/
+
+ // fill the data vector
+ for (size_t i = 0; i < filtered_cloud_->points.size (); i++)
+ {
+ Eigen::Vector3f p (filtered_anno_->points[i].x,
+ filtered_anno_->points[i].y,
+ filtered_anno_->points[i].z);
+ Eigen::Vector3i c = voxel_grid_.getGridCoordinates (p.x (), p.y (), p.y ());
+ data_[i] = c;
+
+ if (color_data)
+ {
+ uint32_t rgb = *reinterpret_cast<int*>(&filtered_cloud_->points[i].rgba);
+ uint8_t r = (rgb >> 16) & 0x0000ff;
+ uint8_t g = (rgb >> 8) & 0x0000ff;
+ uint8_t b = (rgb) & 0x0000ff;
+ color_[i] = Eigen::Vector3i (r, g, b);
+ }
+
+/*
+ if (normal_data)
+ {
+ float n_x = filtered_cloud_->points[i].normal_x;
+ float n_y = filtered_cloud_->points[i].normal_y;
+ float n_z = filtered_cloud_->points[i].normal_z;
+ normal_[i] = Eigen::Vector3f (n_x, n_y, n_z);
+ }
+*/
+ }
+
+ normal_.resize (filtered_normal_->points.size ());
+ for (size_t i = 0; i < filtered_normal_->points.size (); i++)
+ {
+ float n_x = filtered_normal_->points[i].normal_x;
+ float n_y = filtered_normal_->points[i].normal_y;
+ float n_z = filtered_normal_->points[i].normal_z;
+ normal_[i] = Eigen::Vector3f (n_x, n_y, n_z);
+ }
+
+
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::CrfSegmentation<PointT>::createUnaryPotentials (std::vector<float> &unary,
+ std::vector<int> &labels,
+ unsigned int n_labels)
+{
+ /* initialize random seed: */
+ srand ( static_cast<unsigned int> (time (NULL)) );
+ //srand ( time (NULL) );
+
+ // Certainty that the groundtruth is correct
+ const float GT_PROB = 0.9f;
+ const float u_energy = -logf ( 1.0f / static_cast<float> (n_labels) );
+ const float n_energy = -logf ( (1.0f - GT_PROB) / static_cast<float>(n_labels - 1) );
+ const float p_energy = -logf ( GT_PROB );
+
+ for (size_t k = 0; k < filtered_anno_->points.size (); k++)
+ {
+ int label = filtered_anno_->points[k].label;
+
+ if (labels.size () == 0 && label > 0)
+ labels.push_back (label);
+
+ // add color to the color vector if not added yet
+ int c_idx;
+ for (c_idx = 0; c_idx < static_cast<int> (labels.size ()) ; c_idx++)
+ {
+ if (labels[c_idx] == label)
+ break;
+
+ if (c_idx == static_cast<int>(labels.size () -1) && label > 0)
+ {
+ if (labels.size () < n_labels)
+ labels.push_back (label);
+ else
+ label = 0;
+ }
+ }
+
+ /* generate secret number: */
+ //double iSecret = static_cast<double> (rand ()) / static_cast<double> (RAND_MAX);
+
+ /*
+ if (k < 100)
+ std::cout << iSecret << std::endl;
+ */
+
+/*
+ int gg = 5; //static_cast<int> (labels.size ());
+ if (iSecret < 0.5)
+ {
+ int r = 0;
+ if (gg != 0)
+ r = rand () % (gg - 1 + 1) + 1;
+ else
+ r = 0;
+ c_idx = r;
+ }
+*/
+
+ // set the engeries for the labels
+ size_t u_idx = k * n_labels;
+ if (label > 0)
+ {
+ for (size_t i = 0; i < n_labels; i++)
+ unary[u_idx + i] = n_energy;
+ unary[u_idx + c_idx] = p_energy;
+
+ if (label == 1)
+ {
+ const float PROB = 0.2f;
+ const float n_energy2 = -logf ( (1.0f - PROB) / static_cast<float>(n_labels - 1) );
+ const float p_energy2 = -logf ( PROB );
+
+ for (size_t i = 0; i < n_labels; i++)
+ unary[u_idx + i] = n_energy2;
+ unary[u_idx + c_idx] = p_energy2;
+ }
+
+ }
+ else
+ {
+ for (size_t i = 0; i < n_labels; i++)
+ unary[u_idx + i] = u_energy;
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::CrfSegmentation<PointT>::segmentPoints (pcl::PointCloud<pcl::PointXYZRGBL> &output)
+{
+ // create the voxel grid
+ createVoxelGrid ();
+ std::cout << "create Voxel Grid - DONE" << std::endl;
+
+ // create the data Vector
+ createDataVectorFromVoxelGrid ();
+ std::cout << "create Data Vector from Voxel Grid - DONE" << std::endl;
+
+ // number of labels
+ const int n_labels = 10;
+ // number of data points
+ int N = static_cast<int> (data_.size ());
+
+ // create unary potentials
+ std::vector<int> labels;
+ std::vector<float> unary;
+ if (anno_cloud_->points.size () > 0)
+ {
+ unary.resize (N * n_labels);
+ createUnaryPotentials (unary, labels, n_labels);
+
+
+ std::cout << "labels size: " << labels.size () << std::endl;
+ for (size_t i = 0; i < labels.size (); i++)
+ {
+ std::cout << labels[i] << std::endl;
+ }
+
+ }
+ std::cout << "create unary potentials - DONE" << std::endl;
+
+
+/*
+ pcl::PointCloud<pcl::PointXYZRGBL> tmp_cloud_OLD;
+ tmp_cloud_OLD = *filtered_anno_;
+
+ // Setup the CRF model
+ DenseCRF2D crfOLD(N, 1, n_labels);
+
+ float * unaryORI = new float[N*n_labels];
+ for (int i = 0; i < N*n_labels; i++)
+ unaryORI[i] = unary[i];
+ crfOLD.setUnaryEnergy( unaryORI );
+
+ float * pos = new float[N*3];
+ for (int i = 0; i < N; i++)
+ {
+ pos[i * 3] = data_[i].x ();
+ pos[i * 3 +1] = data_[i].y ();
+ pos[i * 3 +2] = data_[i].z ();
+ }
+ crfOLD.addPairwiseGaussian( pos, 3, 3, 3, 2.0 );
+
+ float * col = new float[N*3];
+ for (int i = 0; i < N; i++)
+ {
+ col[i * 3] = color_[i].x ();
+ col[i * 3 +1] = color_[i].y ();
+ col[i * 3 +2] = color_[i].z ();
+ }
+ crfOLD.addPairwiseBilateral(pos, col, 20, 20, 20, 10, 10, 10, 1.3 );
+
+ short * map = new short[N];
+ crfOLD.map(10, map);
+
+ for (size_t i = 0; i < N; i++)
+ {
+ tmp_cloud_OLD.points[i].label = map[i];
+ }
+
+
+*/
+
+ //float * resultOLD = new float[N*n_labels];
+ //crfOLD.inference (10, resultOLD);
+
+ //std::vector<float> baryOLD;
+ //crfOLD.getBarycentric (0, baryOLD);
+ //std::vector<float> featuresOLD;
+ //crfOLD.getFeature (1, featuresOLD);
+
+/*
+ for(int i = 0; i < 25; i++)
+ {
+ std::cout << baryOLD[i] << std::endl;
+ }
+*/
+
+
+ // create the output cloud
+ //output = *filtered_anno_;
+
+
+
+ // ----------------------------------//
+ // -------- -------------------//
+
+ pcl::PointCloud<pcl::PointXYZRGBL> tmp_cloud;
+ tmp_cloud = *filtered_anno_;
+
+ // create dense CRF
+ DenseCrf crf (N, n_labels);
+
+ // set the unary potentials
+ crf.setUnaryEnergy (unary);
+
+ // set the data vector
+ crf.setDataVector (data_);
+
+ // set the color vector
+ crf.setColorVector (color_);
+
+ std::cout << "create dense CRF - DONE" << std::endl;
+
+
+ // add the smoothness kernel
+ crf.addPairwiseGaussian (smoothness_kernel_param_[0],
+ smoothness_kernel_param_[1],
+ smoothness_kernel_param_[2],
+ smoothness_kernel_param_[3]);
+ std::cout << "add smoothness kernel - DONE" << std::endl;
+
+ // add the appearance kernel
+ crf.addPairwiseBilateral (appearance_kernel_param_[0],
+ appearance_kernel_param_[1],
+ appearance_kernel_param_[2],
+ appearance_kernel_param_[3],
+ appearance_kernel_param_[4],
+ appearance_kernel_param_[5],
+ appearance_kernel_param_[6]);
+ std::cout << "add appearance kernel - DONE" << std::endl;
+
+ crf.addPairwiseNormals (data_, normal_,
+ surface_kernel_param_[0],
+ surface_kernel_param_[1],
+ surface_kernel_param_[2],
+ surface_kernel_param_[3],
+ surface_kernel_param_[4],
+ surface_kernel_param_[5],
+ surface_kernel_param_[6]);
+ std::cout << "add surface kernel - DONE" << std::endl;
+
+ // map inference
+ std::vector<int> r (N);
+ crf.mapInference (n_iterations_, r);
+
+ //std::vector<float> result (N*n_labels);
+ //crf.inference (n_iterations_, result);
+
+ //std::vector<float> bary;
+ //crf.getBarycentric (0, bary);
+ //std::vector<float> features;
+ //crf.getFeatures (1, features);
+
+ std::cout << "Map inference - DONE" << std::endl;
+
+ // create the output cloud
+ output = *filtered_anno_;
+
+ for (int i = 0; i < N; i++)
+ {
+ output.points[i].label = labels[r[i]];
+ }
+
+
+/*
+ bool c = true;
+ for (size_t i = 0; i < tmp_cloud.points.size (); i++)
+ {
+ if (tmp_cloud.points[i].label != tmp_cloud_OLD.points[i].label)
+ {
+
+ std::cout << "idx: " << i << " = " <<tmp_cloud.points[i].label << " | " << tmp_cloud_OLD.points[i].label << std::endl;
+ c = false;
+ break;
+ }
+ }
+
+ if (c)
+ std::cout << "DEBUG - OUTPUT - OK" << std::endl;
+ else
+ std::cout << "DEBUG - OUTPUT - ERROR" << std::endl;
+*/
+
+
+
+/*
+ for (size_t i = 0; i < 25; i++)
+ {
+ std::cout << result[i] << " | " << resultOLD[i] << std::endl;
+ }
+
+
+ c = true;
+ for (size_t i = 0; i < result.size (); i++)
+ {
+ if (result[i] != resultOLD[i])
+ {
+ std::cout << result[i] << " | " << resultOLD[i] << std::endl;
+
+ c = false;
+ break;
+ }
+ }
+
+ if (c)
+ std::cout << "DEBUG - OUTPUT - OK" << std::endl;
+ else
+ std::cout << "DEBUG - OUTPUT - ERROR" << std::endl;
+*/
+
+
+}
+
+#define PCL_INSTANTIATE_CrfSegmentation(T) template class pcl::CrfSegmentation<T>;
+
+#endif // PCL_CRF_SEGMENTATION_HPP_
double x1, x2, y1, y2;
int nr_poly_points = static_cast<int> (polygon.points.size ());
+ // start with the last point to make the check last point<->first point the first one
double xold = polygon.points[nr_poly_points - 1].x;
double yold = polygon.points[nr_poly_points - 1].y;
for (int i = 0; i < nr_poly_points; i++)
yold = ynew;
}
- // And a last check for the polygon line formed by the last and the first points
- double xnew = polygon.points[0].x;
- double ynew = polygon.points[0].y;
- if (xnew > xold)
- {
- x1 = xold;
- x2 = xnew;
- y1 = yold;
- y2 = ynew;
- }
- else
- {
- x1 = xnew;
- x2 = xold;
- y1 = ynew;
- y2 = yold;
- }
-
- if ( (xnew < point.x) == (point.x <= xold) && (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1) )
- {
- in_poly = !in_poly;
- }
-
return (in_poly);
}
graph_nodes_.clear ();
graph_nodes_.resize (indices_->size ());
int start = graph_.addNodes (indices_->size ());
- for (int idx = 0; idx < indices_->size (); ++idx)
+ for (size_t idx = 0; idx < indices_->size (); ++idx)
{
graph_nodes_[idx] = start;
++start;
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_
+#define PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_
+
+#include <pcl/segmentation/lccp_segmentation.h>
+
+
+//////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////
+/////////////////// Public Functions /////////////////////
+//////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////
+
+
+
+template <typename PointT>
+pcl::LCCPSegmentation<PointT>::LCCPSegmentation () :
+ concavity_tolerance_threshold_ (10),
+ grouping_data_valid_ (false),
+ supervoxels_set_ (false),
+ use_smoothness_check_ (false),
+ smoothness_threshold_ (0.1),
+ use_sanity_check_ (false),
+ seed_resolution_ (0),
+ voxel_resolution_ (0),
+ k_factor_ (0),
+ min_segment_size_ (0)
+{
+}
+
+template <typename PointT>
+pcl::LCCPSegmentation<PointT>::~LCCPSegmentation ()
+{
+}
+
+template <typename PointT> void
+pcl::LCCPSegmentation<PointT>::reset ()
+{
+ sv_adjacency_list_.clear ();
+ processed_.clear ();
+ sv_label_to_supervoxel_map_.clear ();
+ sv_label_to_seg_label_map_.clear ();
+ seg_label_to_sv_list_map_.clear ();
+ seg_label_to_neighbor_set_map_.clear ();
+ grouping_data_valid_ = false;
+ supervoxels_set_ = false;
+}
+
+template <typename PointT> void
+pcl::LCCPSegmentation<PointT>::segment ()
+{
+ if (supervoxels_set_)
+ {
+ // Calculate for every Edge if the connection is convex or invalid
+ // This effectively performs the segmentation.
+ calculateConvexConnections (sv_adjacency_list_);
+
+ // Correct edge relations using extended convexity definition if k>0
+ applyKconvexity (k_factor_);
+
+ // group supervoxels
+ doGrouping ();
+
+ grouping_data_valid_ = true;
+
+ // merge small segments
+ mergeSmallSegments ();
+ }
+ else
+ PCL_WARN ("[pcl::LCCPSegmentation::segment] WARNING: Call function setInputSupervoxels first. Nothing has been done. \n");
+}
+
+
+template <typename PointT> void
+pcl::LCCPSegmentation<PointT>::relabelCloud (pcl::PointCloud<pcl::PointXYZL> &labeled_cloud_arg)
+{
+ if (grouping_data_valid_)
+ {
+ // Relabel all Points in cloud with new labels
+ typename pcl::PointCloud<pcl::PointXYZL>::iterator voxel_itr = labeled_cloud_arg.begin ();
+ for (; voxel_itr != labeled_cloud_arg.end (); ++voxel_itr)
+ {
+ voxel_itr->label = sv_label_to_seg_label_map_[voxel_itr->label];
+ }
+ }
+ else
+ {
+ PCL_WARN ("[pcl::LCCPSegmentation::relabelCloud] WARNING: Call function segment first. Nothing has been done. \n");
+ }
+}
+
+
+
+//////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////
+/////////////////// Protected Functions //////////////////
+//////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////
+
+template <typename PointT> void
+pcl::LCCPSegmentation<PointT>::computeSegmentAdjacency ()
+{
+ seg_label_to_neighbor_set_map_.clear ();
+
+ //The vertices in the supervoxel adjacency list are the supervoxel centroids
+ std::pair<VertexIterator, VertexIterator> vertex_iterator_range;
+ vertex_iterator_range = boost::vertices (sv_adjacency_list_);
+
+ uint32_t current_segLabel;
+ uint32_t neigh_segLabel;
+
+ // For every Supervoxel..
+ for (VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr) // For all supervoxels
+ {
+ const uint32_t& sv_label = sv_adjacency_list_[*sv_itr];
+ current_segLabel = sv_label_to_seg_label_map_[sv_label];
+
+ // ..look at all neighbors and insert their labels into the neighbor set
+ std::pair<AdjacencyIterator, AdjacencyIterator> neighbors = boost::adjacent_vertices (*sv_itr, sv_adjacency_list_);
+ for (AdjacencyIterator itr_neighbor = neighbors.first; itr_neighbor != neighbors.second; ++itr_neighbor)
+ {
+ const uint32_t& neigh_label = sv_adjacency_list_[*itr_neighbor];
+ neigh_segLabel = sv_label_to_seg_label_map_[neigh_label];
+
+ if (current_segLabel != neigh_segLabel)
+ {
+ seg_label_to_neighbor_set_map_[current_segLabel].insert (neigh_segLabel);
+ }
+ }
+ }
+}
+
+template <typename PointT> void
+pcl::LCCPSegmentation<PointT>::mergeSmallSegments ()
+{
+ if (min_segment_size_ == 0)
+ return;
+
+ computeSegmentAdjacency ();
+
+ std::set<uint32_t> filteredSegLabels;
+
+ uint32_t largest_neigh_size = 0;
+ uint32_t largest_neigh_seg_label = 0;
+ uint32_t current_seg_label;
+
+ std::pair<VertexIterator, VertexIterator> vertex_iterator_range;
+ vertex_iterator_range = boost::vertices (sv_adjacency_list_);
+
+ bool continue_filtering = true;
+
+ while (continue_filtering)
+ {
+ continue_filtering = false;
+ unsigned int nr_filtered = 0;
+
+ // Iterate through all supervoxels, check if they are in a "small" segment -> change label to largest neighborID
+ for (VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr) // For all supervoxels
+ {
+ const uint32_t& sv_label = sv_adjacency_list_[*sv_itr];
+ current_seg_label = sv_label_to_seg_label_map_[sv_label];
+ largest_neigh_seg_label = current_seg_label;
+ largest_neigh_size = seg_label_to_sv_list_map_[current_seg_label].size ();
+
+ const uint32_t& nr_neighbors = seg_label_to_neighbor_set_map_[current_seg_label].size ();
+ if (nr_neighbors == 0)
+ continue;
+
+ if (seg_label_to_sv_list_map_[current_seg_label].size () <= min_segment_size_)
+ {
+ continue_filtering = true;
+ nr_filtered++;
+
+ // Find largest neighbor
+ std::set<uint32_t>::const_iterator neighbors_itr = seg_label_to_neighbor_set_map_[current_seg_label].begin ();
+ for (; neighbors_itr != seg_label_to_neighbor_set_map_[current_seg_label].end (); ++neighbors_itr)
+ {
+ if (seg_label_to_sv_list_map_[*neighbors_itr].size () >= largest_neigh_size)
+ {
+ largest_neigh_seg_label = *neighbors_itr;
+ largest_neigh_size = seg_label_to_sv_list_map_[*neighbors_itr].size ();
+ }
+ }
+
+ // Add to largest neighbor
+ if (largest_neigh_seg_label != current_seg_label)
+ {
+ if (filteredSegLabels.count (largest_neigh_seg_label) > 0)
+ continue; // If neighbor was already assigned to someone else
+
+ sv_label_to_seg_label_map_[sv_label] = largest_neigh_seg_label;
+ filteredSegLabels.insert (current_seg_label);
+
+ // Assign supervoxel labels of filtered segment to new owner
+ std::set<uint32_t>::iterator sv_ID_itr = seg_label_to_sv_list_map_[current_seg_label].begin ();
+ sv_ID_itr = seg_label_to_sv_list_map_[current_seg_label].begin ();
+ for (; sv_ID_itr != seg_label_to_sv_list_map_[current_seg_label].end (); ++sv_ID_itr)
+ {
+ seg_label_to_sv_list_map_[largest_neigh_seg_label].insert (*sv_ID_itr);
+ }
+ }
+ }
+ }
+
+ // Erase filtered Segments from segment map
+ std::set<uint32_t>::iterator filtered_ID_itr = filteredSegLabels.begin ();
+ for (; filtered_ID_itr != filteredSegLabels.end (); ++filtered_ID_itr)
+ {
+ seg_label_to_sv_list_map_.erase (*filtered_ID_itr);
+ }
+
+ // After filtered Segments are deleted, compute completely new adjacency map
+ // NOTE Recomputing the adjacency of every segment in every iteration is an easy but inefficient solution.
+ // Because the number of segments in an average scene is usually well below 1000, the time spend for noise filtering is still neglible in most cases
+ computeSegmentAdjacency ();
+ } // End while (Filtering)
+}
+
+template <typename PointT> void
+pcl::LCCPSegmentation<PointT>::prepareSegmentation (const std::map<uint32_t, typename pcl::Supervoxel<PointT>::Ptr>& supervoxel_clusters_arg,
+ const std::multimap<uint32_t, uint32_t>& label_adjaceny_arg)
+{
+ // Clear internal data
+ reset ();
+
+ // Copy map with supervoxel pointers
+ sv_label_to_supervoxel_map_ = supervoxel_clusters_arg;
+
+ // Build a boost adjacency list from the adjacency multimap
+ std::map<uint32_t, VertexID> label_ID_map;
+
+ // Add all supervoxel labels as vertices
+ for (typename std::map<uint32_t, typename pcl::Supervoxel<PointT>::Ptr>::iterator svlabel_itr = sv_label_to_supervoxel_map_.begin ();
+ svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
+ {
+ const uint32_t& sv_label = svlabel_itr->first;
+ VertexID node_id = boost::add_vertex (sv_adjacency_list_);
+ sv_adjacency_list_[node_id] = sv_label;
+ label_ID_map[sv_label] = node_id;
+ }
+
+ // Add all edges
+ for (std::multimap<uint32_t, uint32_t>::const_iterator sv_neighbors_itr = label_adjaceny_arg.begin (); sv_neighbors_itr != label_adjaceny_arg.end ();
+ ++sv_neighbors_itr)
+ {
+ const uint32_t& sv_label = sv_neighbors_itr->first;
+ const uint32_t& neighbor_label = sv_neighbors_itr->second;
+
+ VertexID u = label_ID_map[sv_label];
+ VertexID v = label_ID_map[neighbor_label];
+
+ boost::add_edge (u, v, sv_adjacency_list_);
+ }
+
+ // Initialization
+ // clear the processed_ map
+ seg_label_to_sv_list_map_.clear ();
+ for (typename std::map<uint32_t, typename pcl::Supervoxel<PointT>::Ptr>::iterator svlabel_itr = sv_label_to_supervoxel_map_.begin ();
+ svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
+ {
+ const uint32_t& sv_label = svlabel_itr->first;
+ processed_[sv_label] = false;
+ sv_label_to_seg_label_map_[sv_label] = 0;
+ }
+}
+
+
+
+
+template <typename PointT> void
+pcl::LCCPSegmentation<PointT>::doGrouping ()
+{
+ // clear the processed_ map
+ seg_label_to_sv_list_map_.clear ();
+ for (typename std::map<uint32_t, typename pcl::Supervoxel<PointT>::Ptr>::iterator svlabel_itr = sv_label_to_supervoxel_map_.begin ();
+ svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
+ {
+ const uint32_t& sv_label = svlabel_itr->first;
+ processed_[sv_label] = false;
+ sv_label_to_seg_label_map_[sv_label] = 0;
+ }
+
+ // Perform depth search on the graph and recursively group all supervoxels with convex connections
+ //The vertices in the supervoxel adjacency list are the supervoxel centroids
+ std::pair< VertexIterator, VertexIterator> vertex_iterator_range;
+ vertex_iterator_range = boost::vertices (sv_adjacency_list_);
+
+ // Note: *sv_itr is of type " boost::graph_traits<VoxelAdjacencyList>::vertex_descriptor " which it nothing but a typedef of size_t..
+ unsigned int segment_label = 1; // This starts at 1, because 0 is reserved for errors
+ for (VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr) // For all supervoxels
+ {
+ const VertexID sv_vertex_id = *sv_itr;
+ const uint32_t& sv_label = sv_adjacency_list_[sv_vertex_id];
+ if (!processed_[sv_label])
+ {
+ // Add neighbors (and their neighbors etc.) to group if similarity constraint is met
+ recursiveSegmentGrowing (sv_vertex_id, segment_label);
+ ++segment_label; // After recursive grouping ended (no more neighbors to consider) -> go to next group
+ }
+ }
+}
+
+template <typename PointT> void
+pcl::LCCPSegmentation<PointT>::recursiveSegmentGrowing (const VertexID &query_point_id,
+ const unsigned int segment_label)
+{
+ const uint32_t& sv_label = sv_adjacency_list_[query_point_id];
+
+ processed_[sv_label] = true;
+
+ // The next two lines add the supervoxel to the segment
+ sv_label_to_seg_label_map_[sv_label] = segment_label;
+ seg_label_to_sv_list_map_[segment_label].insert (sv_label);
+
+ // Iterate through all neighbors of this supervoxel and check wether they should be merged with the current supervoxel
+ std::pair<OutEdgeIterator, OutEdgeIterator> out_edge_iterator_range;
+ out_edge_iterator_range = boost::out_edges (query_point_id, sv_adjacency_list_); // adjacent vertices to node (*itr) in graph sv_adjacency_list_
+ for (OutEdgeIterator out_Edge_itr = out_edge_iterator_range.first; out_Edge_itr != out_edge_iterator_range.second; ++out_Edge_itr)
+ {
+ const VertexID neighbor_ID = boost::target (*out_Edge_itr, sv_adjacency_list_);
+ const uint32_t& neighbor_label = sv_adjacency_list_[neighbor_ID];
+
+ if (!processed_[neighbor_label]) // If neighbor was not already processed
+ {
+ if (sv_adjacency_list_[*out_Edge_itr].is_valid)
+ {
+ recursiveSegmentGrowing (neighbor_ID, segment_label);
+ }
+ }
+ } // End neighbor loop
+}
+
+template <typename PointT> void
+pcl::LCCPSegmentation<PointT>::applyKconvexity (const unsigned int k_arg)
+{
+ if (k_arg == 0)
+ return;
+
+ bool is_convex;
+ unsigned int kcount = 0;
+
+ EdgeIterator edge_itr, edge_itr_end, next_edge;
+ boost::tie (edge_itr, edge_itr_end) = boost::edges (sv_adjacency_list_);
+
+ std::pair<OutEdgeIterator, OutEdgeIterator> source_neighbors_range;
+ std::pair<OutEdgeIterator, OutEdgeIterator> target_neighbors_range;
+
+ // Check all edges in the graph for k-convexity
+ for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
+ {
+ next_edge++; // next_edge iterator is neccessary, because removing an edge invalidates the iterator to the current edge
+
+ is_convex = sv_adjacency_list_[*edge_itr].is_convex;
+
+ if (is_convex) // If edge is (0-)convex
+ {
+ kcount = 0;
+
+ const VertexID source = boost::source (*edge_itr, sv_adjacency_list_);
+ const VertexID target = boost::target (*edge_itr, sv_adjacency_list_);
+
+ source_neighbors_range = boost::out_edges (source, sv_adjacency_list_);
+ target_neighbors_range = boost::out_edges (target, sv_adjacency_list_);
+
+ // Find common neighbors, check their connection
+ for (OutEdgeIterator source_neighbors_itr = source_neighbors_range.first; source_neighbors_itr != source_neighbors_range.second; ++source_neighbors_itr) // For all supervoxels
+ {
+ VertexID source_neighbor_ID = boost::target (*source_neighbors_itr, sv_adjacency_list_);
+
+ for (OutEdgeIterator target_neighbors_itr = target_neighbors_range.first; target_neighbors_itr != target_neighbors_range.second; ++target_neighbors_itr) // For all supervoxels
+ {
+ VertexID target_neighbor_ID = boost::target (*target_neighbors_itr, sv_adjacency_list_);
+ if (source_neighbor_ID == target_neighbor_ID) // Common neighbor
+ {
+ EdgeID src_edge = boost::edge (source, source_neighbor_ID, sv_adjacency_list_).first;
+ EdgeID tar_edge = boost::edge (target, source_neighbor_ID, sv_adjacency_list_).first;
+
+ bool src_is_convex = (sv_adjacency_list_)[src_edge].is_convex;
+ bool tar_is_convex = (sv_adjacency_list_)[tar_edge].is_convex;
+
+ if (src_is_convex && tar_is_convex)
+ ++kcount;
+
+ break;
+ }
+ }
+
+ if (kcount >= k_arg) // Connection is k-convex, stop search
+ break;
+ }
+
+ // Check k convexity
+ if (kcount < k_arg)
+ (sv_adjacency_list_)[*edge_itr].is_valid = false;
+ }
+ }
+}
+
+template <typename PointT> void
+pcl::LCCPSegmentation<PointT>::calculateConvexConnections (SupervoxelAdjacencyList& adjacency_list_arg)
+{
+ bool is_convex;
+
+ EdgeIterator edge_itr, edge_itr_end, next_edge;
+ boost::tie (edge_itr, edge_itr_end) = boost::edges (adjacency_list_arg);
+
+ for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
+ {
+ next_edge++; // next_edge iterator is neccessary, because removing an edge invalidates the iterator to the current edge
+
+ uint32_t source_sv_label = adjacency_list_arg[boost::source (*edge_itr, adjacency_list_arg)];
+ uint32_t target_sv_label = adjacency_list_arg[boost::target (*edge_itr, adjacency_list_arg)];
+
+ float normal_difference;
+ is_convex = connIsConvex (source_sv_label, target_sv_label, normal_difference);
+ adjacency_list_arg[*edge_itr].is_convex = is_convex;
+ adjacency_list_arg[*edge_itr].is_valid = is_convex;
+ adjacency_list_arg[*edge_itr].normal_difference = normal_difference;
+ }
+}
+
+template <typename PointT> bool
+pcl::LCCPSegmentation<PointT>::connIsConvex (const uint32_t source_label_arg,
+ const uint32_t target_label_arg,
+ float &normal_angle)
+{
+ typename pcl::Supervoxel<PointT>::Ptr& sv_source = sv_label_to_supervoxel_map_[source_label_arg];
+ typename pcl::Supervoxel<PointT>::Ptr& sv_target = sv_label_to_supervoxel_map_[target_label_arg];
+
+ const Eigen::Vector3f& source_centroid = sv_source->centroid_.getVector3fMap ();
+ const Eigen::Vector3f& target_centroid = sv_target->centroid_.getVector3fMap ();
+
+ const Eigen::Vector3f& source_normal = sv_source->normal_.getNormalVector3fMap (). normalized ();
+ const Eigen::Vector3f& target_normal = sv_target->normal_.getNormalVector3fMap (). normalized ();
+
+ //NOTE For angles below 0 nothing will be merged
+ if (concavity_tolerance_threshold_ < 0)
+ {
+ return (false);
+ }
+
+ bool is_convex = true;
+ bool is_smooth = true;
+
+ normal_angle = getAngle3D (source_normal, target_normal, true);
+ // Geometric comparisons
+ Eigen::Vector3f vec_t_to_s, vec_s_to_t;
+
+ vec_t_to_s = source_centroid - target_centroid;
+ vec_s_to_t = -vec_t_to_s;
+
+ Eigen::Vector3f ncross;
+ ncross = source_normal.cross (target_normal);
+
+ // Smoothness Check: Check if there is a step between adjacent patches
+ if (use_smoothness_check_)
+ {
+ float expected_distance = ncross.norm () * seed_resolution_;
+ float dot_p_1 = vec_t_to_s.dot (source_normal);
+ float dot_p_2 = vec_s_to_t.dot (target_normal);
+ float point_dist = (std::fabs (dot_p_1) < std::fabs (dot_p_2)) ? std::fabs (dot_p_1) : std::fabs (dot_p_2);
+ const float dist_smoothing = smoothness_threshold_ * voxel_resolution_; // This is a slacking variable especially important for patches with very similar normals
+
+ if (point_dist > (expected_distance + dist_smoothing))
+ {
+ is_smooth &= false;
+ }
+ }
+ // ----------------
+
+ // Sanity Criterion: Check if definition convexity/concavity makes sense for connection of given patches
+ float intersection_angle = getAngle3D (ncross, vec_t_to_s, true);
+ float min_intersect_angle = (intersection_angle < 90.) ? intersection_angle : 180. - intersection_angle;
+
+ float intersect_thresh = 60. * 1. / (1. + exp (-0.25 * (normal_angle - 25.)));
+ if (min_intersect_angle < intersect_thresh && use_sanity_check_)
+ {
+ // std::cout << "Concave/Convex not defined for given case!" << std::endl;
+ is_convex &= false;
+ }
+
+
+ // vec_t_to_s is the reference direction for angle measurements
+ // Convexity Criterion: Check if connection of patches is convex. If this is the case the two supervoxels should be merged.
+ if ((getAngle3D (vec_t_to_s, source_normal) - getAngle3D (vec_t_to_s, target_normal)) <= 0)
+ {
+ is_convex &= true; // connection convex
+ }
+ else
+ {
+ is_convex &= (normal_angle < concavity_tolerance_threshold_); // concave connections will be accepted if difference of normals is small
+ }
+ return (is_convex && is_smooth);
+}
+
+#endif // PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_
std::vector<unsigned> run_ids;
unsigned invalid_label = std::numeric_limits<unsigned>::max ();
- pcl::Label invalid_pt;
+ PointLT invalid_pt;
invalid_pt.label = std::numeric_limits<unsigned>::max ();
labels.points.resize (input_->points.size (), invalid_pt);
labels.width = input_->width;
compare_->setDistanceThreshold (static_cast<float> (distance_threshold_), true);
// Set up the output
- OrganizedConnectedComponentSegmentation<PointT,pcl::Label> connected_component (compare_);
+ OrganizedConnectedComponentSegmentation<PointT,PointLT> connected_component (compare_);
connected_component.setInputCloud (input_);
connected_component.segment (labels, label_indices);
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_SEGMENTATION_IMPL_RANDOM_WALKER_HPP
+#define PCL_SEGMENTATION_IMPL_RANDOM_WALKER_HPP
+
+#include <boost/bimap.hpp>
+
+#include <Eigen/Sparse>
+
+namespace pcl
+{
+
+ namespace segmentation
+ {
+
+ namespace detail
+ {
+
+ /** \brief Multilabel graph segmentation using random walks.
+ *
+ * This is an implementation of the algorithm described in "Random Walks
+ * for Image Segmentation" by Leo Grady.
+ *
+ * See the documentation of the randomWalker() function for details.
+ *
+ * \author Sergey Alexandrov
+ * \ingroup segmentation
+ */
+ template <class Graph, class EdgeWeightMap, class VertexColorMap>
+ class RandomWalker
+ {
+
+ public:
+
+ typedef typename boost::property_traits<VertexColorMap>::value_type Color;
+ typedef typename boost::property_traits<EdgeWeightMap>::value_type Weight;
+ typedef boost::graph_traits<Graph> GraphTraits;
+ typedef typename GraphTraits::edge_descriptor EdgeDescriptor;
+ typedef typename GraphTraits::vertex_descriptor VertexDescriptor;
+ typedef typename GraphTraits::edge_iterator EdgeIterator;
+ typedef typename GraphTraits::out_edge_iterator OutEdgeIterator;
+ typedef typename GraphTraits::vertex_iterator VertexIterator;
+ typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMap;
+ typedef boost::iterator_property_map<typename std::vector<Weight>::iterator, VertexIndexMap> VertexDegreeMap;
+ typedef Eigen::SparseMatrix<Weight> SparseMatrix;
+ typedef Eigen::Matrix<Weight, Eigen::Dynamic, Eigen::Dynamic> Matrix;
+ typedef Eigen::Matrix<Weight, Eigen::Dynamic, 1> Vector;
+
+ RandomWalker (Graph& g, EdgeWeightMap weights, VertexColorMap colors)
+ : g_ (g)
+ , weight_map_ (weights)
+ , color_map_ (colors)
+ , index_map_ (boost::get (boost::vertex_index, g_))
+ , degree_storage_ (boost::num_vertices (g_), 0)
+ , degree_map_ (boost::make_iterator_property_map (degree_storage_.begin (), index_map_))
+ {
+ }
+
+ bool
+ segment ()
+ {
+ computeVertexDegrees ();
+ buildLinearSystem ();
+ return solveLinearSystem ();
+ }
+
+ void
+ computeVertexDegrees ()
+ {
+ using namespace boost;
+ EdgeIterator ei, e_end;
+ for (tie (ei, e_end) = edges (g_); ei != e_end; ++ei)
+ {
+ Weight w = weight_map_[*ei];
+ degree_map_[source (*ei, g_)] += w;
+ degree_map_[target (*ei, g_)] += w;
+ }
+ }
+
+ void
+ buildLinearSystem ()
+ {
+ using namespace boost;
+
+ typedef Eigen::Triplet<float> T;
+ typedef std::vector<T> Triplets;
+ Triplets L_triplets;
+ Triplets B_triplets;
+
+ VertexIterator vi, v_end;
+ for (tie (vi, v_end) = vertices (g_); vi != v_end; ++vi)
+ {
+ // If this is a labeled vertex add it to the seeds list and register its color
+ if (color_map_[*vi])
+ {
+ seeds_.push_back (*vi);
+ colors_.insert (color_map_[*vi]);
+ }
+ // Skip seeds and vertices with zero connectivity
+ if (color_map_[*vi] || std::fabs (degree_map_[*vi]) < std::numeric_limits<Weight>::epsilon ())
+ continue;
+ // Create a row in L matrix for the vertex
+ size_t current_row = insertInBimap (L_vertex_bimap, *vi);
+ // Add diagonal degree entry for the vertex
+ L_triplets.push_back (T (current_row, current_row, degree_map_[*vi]));
+ // Iterate over incident vertices and add entries on corresponding columns of L or B
+ OutEdgeIterator ei, e_end;
+ for (tie (ei, e_end) = out_edges (*vi, g_); ei != e_end; ++ei)
+ {
+ Weight w = weight_map_[*ei];
+ VertexDescriptor tgt = target (*ei, g_);
+ Color color = color_map_[tgt];
+ if (color)
+ {
+ // This is a seed and will go to B matrix
+ size_t column;
+ if (B_color_bimap.right.count (color) == 0)
+ {
+ // This is the first time we encountered this color, create a new column in B
+ column = insertInBimap (B_color_bimap, color);
+ }
+ else
+ {
+ column = B_color_bimap.right.at (color);
+ }
+ B_triplets.push_back (T (current_row, column, w));
+ }
+ else
+ {
+ // This is a non-seed and will go to L matrix,
+ // but only if a row for this vertex already exists
+ if (L_vertex_bimap.right.count (tgt) && L_vertex_bimap.right.at (tgt) != current_row)
+ {
+ L_triplets.push_back (T (current_row, L_vertex_bimap.right.at (tgt), -w));
+ }
+ }
+ }
+ }
+
+ size_t num_equations = L_vertex_bimap.size ();
+ size_t num_colors = B_color_bimap.size ();
+ L.resize (num_equations, num_equations);
+ B.resize (num_equations, num_colors);
+ if (L_triplets.size ())
+ L.setFromTriplets(L_triplets.begin(), L_triplets.end());
+ if (B_triplets.size ())
+ B.setFromTriplets(B_triplets.begin(), B_triplets.end());
+ }
+
+ bool solveLinearSystem()
+ {
+ X.resize (L.rows (), B.cols ());
+
+ // Nothing to solve
+ if (L.rows () == 0 || B.cols () == 0)
+ return true;
+
+ Eigen::SimplicialCholesky<SparseMatrix, Eigen::Lower> cg;
+ cg.compute (L);
+ bool succeeded = true;
+ for (int i = 0; i < B.cols (); ++i)
+ {
+ Vector b = B.col (i);
+ X.col (i) = cg.solve (b);
+ if (cg.info () != Eigen::Success)
+ succeeded = false;
+ }
+
+ assignColors ();
+ return succeeded;
+ }
+
+ void
+ assignColors ()
+ {
+ using namespace boost;
+ if (X.cols ())
+ for (int i = 0; i < X.rows (); ++i)
+ {
+ size_t max_column;
+ X.row (i).maxCoeff (&max_column);
+ VertexDescriptor vertex = L_vertex_bimap.left.at (i);
+ Color color = B_color_bimap.left.at (max_column);
+ color_map_[vertex] = color;
+ }
+ }
+
+ void
+ getPotentials (Matrix& potentials, std::map<Color, size_t>& color_to_column_map)
+ {
+ using namespace boost;
+ potentials = Matrix::Zero (num_vertices (g_), colors_.size ());
+ // Copy over rows from X
+ for (int i = 0; i < X.rows (); ++i)
+ potentials.row (L_vertex_bimap.left.at (i)).head (X.cols ()) = X.row (i);
+ // In rows that correspond to seeds put ones in proper columns
+ for (size_t i = 0; i < seeds_.size (); ++i)
+ {
+ VertexDescriptor v = seeds_[i];
+ insertInBimap (B_color_bimap, color_map_[v]);
+ potentials (seeds_[i], B_color_bimap.right.at (color_map_[seeds_[i]])) = 1;
+ }
+ // Fill in a map that associates colors with columns in potentials matrix
+ color_to_column_map.clear ();
+ for (int i = 0; i < potentials.cols (); ++i)
+ color_to_column_map[B_color_bimap.left.at (i)] = i;
+ }
+
+ template <typename T> static inline size_t
+ insertInBimap (boost::bimap<size_t, T>& bimap, T value)
+ {
+ if (bimap.right.count (value) != 0)
+ {
+ return bimap.right.at (value);
+ }
+ else
+ {
+ size_t s = bimap.size ();
+ bimap.insert (typename boost::bimap<size_t, T>::value_type (s, value));
+ return s;
+ }
+ }
+
+ Graph& g_;
+ EdgeWeightMap weight_map_;
+ VertexColorMap color_map_;
+ VertexIndexMap index_map_;
+
+ std::vector<VertexDescriptor> seeds_;
+ std::set<Color> colors_;
+
+ std::vector<Weight> degree_storage_;
+ VertexDegreeMap degree_map_;
+ SparseMatrix L;
+ SparseMatrix B;
+ Matrix X;
+
+ // Map vertex identifiers to the rows/columns of L and vice versa
+ boost::bimap<size_t, VertexDescriptor> L_vertex_bimap;
+ // Map colors to the columns of B and vice versa
+ boost::bimap<size_t, Color> B_color_bimap;
+
+ };
+
+ }
+
+ template <class Graph> bool
+ randomWalker (Graph& graph)
+ {
+ return randomWalker (graph,
+ boost::get (boost::edge_weight, graph),
+ boost::get (boost::vertex_color, graph));
+ }
+
+ template <class Graph, class EdgeWeightMap, class VertexColorMap> bool
+ randomWalker (Graph& graph,
+ EdgeWeightMap weights,
+ VertexColorMap colors)
+ {
+ using namespace boost;
+
+ typedef typename graph_traits<Graph>::edge_descriptor EdgeDescriptor;
+ typedef typename graph_traits<Graph>::vertex_descriptor VertexDescriptor;
+
+ BOOST_CONCEPT_ASSERT ((VertexListGraphConcept<Graph>)); // to have vertices(), num_vertices()
+ BOOST_CONCEPT_ASSERT ((EdgeListGraphConcept<Graph>)); // to have edges()
+ BOOST_CONCEPT_ASSERT ((IncidenceGraphConcept<Graph>)); // to have source(), target() and out_edges()
+ BOOST_CONCEPT_ASSERT ((ReadablePropertyMapConcept<EdgeWeightMap, EdgeDescriptor>)); // read weight-values from edges
+ BOOST_CONCEPT_ASSERT ((ReadWritePropertyMapConcept<VertexColorMap, VertexDescriptor>)); // read and write color-values from vertices
+
+ ::pcl::segmentation::detail::RandomWalker
+ <
+ Graph,
+ EdgeWeightMap,
+ VertexColorMap
+ >
+ rw (graph, weights, colors);
+ return rw.segment ();
+ }
+
+ template <class Graph, class EdgeWeightMap, class VertexColorMap> bool
+ randomWalker (Graph& graph,
+ EdgeWeightMap weights,
+ VertexColorMap colors,
+ Eigen::Matrix<typename boost::property_traits<EdgeWeightMap>::value_type, Eigen::Dynamic, Eigen::Dynamic>& potentials,
+ std::map<typename boost::property_traits<VertexColorMap>::value_type, size_t>& colors_to_columns_map)
+ {
+ using namespace boost;
+
+ typedef typename graph_traits<Graph>::edge_descriptor EdgeDescriptor;
+ typedef typename graph_traits<Graph>::vertex_descriptor VertexDescriptor;
+
+ BOOST_CONCEPT_ASSERT ((VertexListGraphConcept<Graph>)); // to have vertices(), num_vertices()
+ BOOST_CONCEPT_ASSERT ((EdgeListGraphConcept<Graph>)); // to have edges()
+ BOOST_CONCEPT_ASSERT ((IncidenceGraphConcept<Graph>)); // to have source(), target() and out_edges()
+ BOOST_CONCEPT_ASSERT ((ReadablePropertyMapConcept<EdgeWeightMap, EdgeDescriptor>)); // read weight-values from edges
+ BOOST_CONCEPT_ASSERT ((ReadWritePropertyMapConcept<VertexColorMap, VertexDescriptor>)); // read and write color-values from vertices
+
+ ::pcl::segmentation::detail::RandomWalker
+ <
+ Graph,
+ EdgeWeightMap,
+ VertexColorMap
+ >
+ rw (graph, weights, colors);
+ bool result = rw.segment ();
+ rw.getPotentials (potentials, colors_to_columns_map);
+ return result;
+ }
+
+ }
+
+}
+
+#endif /* PCL_SEGMENTATION_IMPL_RANDOM_WALKER_HPP */
+
}
// check the residual if needed
- data[0] = input_->points[nghbr].data[0];
- data[1] = input_->points[nghbr].data[1];
- data[2] = input_->points[nghbr].data[2];
- data[3] = input_->points[nghbr].data[3];
- Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data));
+ float data_1[4];
+
+ data_1[0] = input_->points[nghbr].data[0];
+ data_1[1] = input_->points[nghbr].data[1];
+ data_1[2] = input_->points[nghbr].data[2];
+ data_1[3] = input_->points[nghbr].data[3];
+ Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data_1));
float residual = fabsf (initial_normal.dot (initial_point - nghbr_point));
if (residual_flag_ && residual > residual_threshold_)
is_a_seed = false;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT>
-pcl::SupervoxelClustering<PointT>::SupervoxelClustering (float voxel_resolution, float seed_resolution, bool use_single_camera_transform) :
+pcl::SupervoxelClustering<PointT>::SupervoxelClustering (float voxel_resolution, float seed_resolution) :
resolution_ (voxel_resolution),
seed_resolution_ (seed_resolution),
adjacency_octree_ (),
voxel_centroid_cloud_ (),
- color_importance_(0.1f),
- spatial_importance_(0.4f),
- normal_importance_(1.0f),
- label_colors_ (0)
+ color_importance_ (0.1f),
+ spatial_importance_ (0.4f),
+ normal_importance_ (1.0f),
+ use_default_transform_behaviour_ (true)
+{
+ adjacency_octree_.reset (new OctreeAdjacencyT (resolution_));
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT>
+pcl::SupervoxelClustering<PointT>::SupervoxelClustering (float voxel_resolution, float seed_resolution, bool) :
+ resolution_ (voxel_resolution),
+ seed_resolution_ (seed_resolution),
+ adjacency_octree_ (),
+ voxel_centroid_cloud_ (),
+ color_importance_ (0.1f),
+ spatial_importance_ (0.4f),
+ normal_importance_ (1.0f),
+ use_default_transform_behaviour_ (true)
{
adjacency_octree_.reset (new OctreeAdjacencyT (resolution_));
- if (use_single_camera_transform)
- adjacency_octree_->setTransformFunction (boost::bind (&SupervoxelClustering::transformFunction, this, _1));
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//double t_prep = timer_.getTime ();
//std::cout << "Placing Seeds" << std::endl;
- std::vector<PointT, Eigen::aligned_allocator<PointT> > seed_points;
- selectInitialSupervoxelSeeds (seed_points);
+ std::vector<int> seed_indices;
+ selectInitialSupervoxelSeeds (seed_indices);
//std::cout << "Creating helpers "<<std::endl;
- createSupervoxelHelpers (seed_points);
+ createSupervoxelHelpers (seed_indices);
//double t_seeds = timer_.getTime ();
//Add the new cloud of data to the octree
//std::cout << "Populating adjacency octree with new cloud \n";
//double prep_start = timer_.getTime ();
+ if ( (use_default_transform_behaviour_ && input_->isOrganized ())
+ || (!use_default_transform_behaviour_ && use_single_camera_transform_))
+ adjacency_octree_->setTransformFunction (boost::bind (&SupervoxelClustering::transformFunction, this, _1));
+
adjacency_octree_->addPointsFromInputCloud ();
//double prep_end = timer_.getTime ();
//std::cout<<"Time elapsed populating octree with next frame ="<<prep_end-prep_start<<" ms\n";
indices.reserve (81);
//Push this point
indices.push_back (new_voxel_data.idx_);
- for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
+ for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
{
VoxelData& neighb_voxel_data = (*neighb_itr)->getData ();
//Push neighbor index
indices.push_back (neighb_voxel_data.idx_);
//Get neighbors neighbors, push onto cloud
- for (typename LeafContainerT::iterator neighb_neighb_itr=(*neighb_itr)->begin (); neighb_neighb_itr!=(*neighb_itr)->end (); ++neighb_neighb_itr)
+ for (typename LeafContainerT::const_iterator neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr)
{
VoxelData& neighb2_voxel_data = (*neighb_neighb_itr)->getData ();
indices.push_back (neighb2_voxel_data.idx_);
sv_itr->getVoxels (supervoxel_clusters[label]->voxels_);
sv_itr->getNormals (supervoxel_clusters[label]->normals_);
}
- //Make sure that color vector is big enough
- initializeLabelColors ();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
-pcl::SupervoxelClustering<PointT>::createSupervoxelHelpers (std::vector<PointT, Eigen::aligned_allocator<PointT> > &seed_points)
+pcl::SupervoxelClustering<PointT>::createSupervoxelHelpers (std::vector<int> &seed_indices)
{
supervoxel_helpers_.clear ();
- for (size_t i = 0; i < seed_points.size (); ++i)
+ for (size_t i = 0; i < seed_indices.size (); ++i)
{
supervoxel_helpers_.push_back (new SupervoxelHelper(i+1,this));
//Find which leaf corresponds to this seed index
- LeafContainerT* seed_leaf = adjacency_octree_->getLeafContainerAtPoint (seed_points[i]);
+ LeafContainerT* seed_leaf = adjacency_octree_->at(seed_indices[i]);//adjacency_octree_->getLeafContainerAtPoint (seed_points[i]);
if (seed_leaf)
{
supervoxel_helpers_.back ().addLeaf (seed_leaf);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
-pcl::SupervoxelClustering<PointT>::selectInitialSupervoxelSeeds (std::vector<PointT, Eigen::aligned_allocator<PointT> > &seed_points)
+pcl::SupervoxelClustering<PointT>::selectInitialSupervoxelSeeds (std::vector<int> &seed_indices)
{
//TODO THIS IS BAD - SEEDING SHOULD BE BETTER
//TODO Switch to assigning leaves! Don't use Octree!
std::vector<int> seed_indices_orig;
seed_indices_orig.resize (num_seeds, 0);
- seed_points.clear ();
+ seed_indices.clear ();
std::vector<int> closest_index;
std::vector<float> distance;
closest_index.resize(1,0);
std::vector<int> neighbors;
std::vector<float> sqr_distances;
- seed_points.reserve (seed_indices_orig.size ());
+ seed_indices.reserve (seed_indices_orig.size ());
float search_radius = 0.5f*seed_resolution_;
// This is number of voxels which fit in a planar slice through search volume
// Area of planar slice / area of voxel side
int min_index = seed_indices_orig[i];
if ( num > min_points)
{
- seed_points.push_back (voxel_centroid_cloud_->points[min_index]);
+ seed_indices.push_back (min_index);
}
}
sv_itr->getXYZ (point.x, point.y, point.z);
voxel_kdtree_->nearestKSearch (point, 1, closest_index, distance);
- LeafContainerT* seed_leaf = adjacency_octree_->getLeafContainerAtPoint (voxel_centroid_cloud_->points[closest_index[0]]);
+ LeafContainerT* seed_leaf = adjacency_octree_->at (closest_index[0]);
if (seed_leaf)
{
sv_itr->addLeaf (seed_leaf);
}
+ else
+ {
+ PCL_WARN ("Could not find leaf in pcl::SupervoxelClustering<PointT>::reseedSupervoxels - supervoxel will be deleted \n");
+ }
}
}
//if (neighbor_labels.size () == 0)
// std::cout << label<<"(size="<<sv_itr->size () << ") has "<<neighbor_labels.size () << "\n";
}
-
-}
-
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
-pcl::SupervoxelClustering<PointT>::getColoredCloud () const
-{
- pcl::PointCloud<pcl::PointXYZRGBA>::Ptr colored_cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
- pcl::copyPointCloud (*input_,*colored_cloud);
-
- pcl::PointCloud <pcl::PointXYZRGBA>::iterator i_colored;
- typename pcl::PointCloud <PointT>::const_iterator i_input = input_->begin ();
- std::vector <int> indices;
- std::vector <float> sqr_distances;
- for (i_colored = colored_cloud->begin (); i_colored != colored_cloud->end (); ++i_colored,++i_input)
- {
- if ( !pcl::isFinite<PointT> (*i_input))
- i_colored->rgb = 0;
- else
- {
- i_colored->rgb = 0;
- LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input);
- VoxelData& voxel_data = leaf->getData ();
- if (voxel_data.owner_)
- i_colored->rgba = label_colors_[voxel_data.owner_->getLabel ()];
-
- }
-
- }
-
- return (colored_cloud);
-}
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
-pcl::SupervoxelClustering<PointT>::getColoredVoxelCloud () const
-{
- pcl::PointCloud<pcl::PointXYZRGBA>::Ptr colored_cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
- for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
- {
- typename PointCloudT::Ptr voxels;
- sv_itr->getVoxels (voxels);
- pcl::PointCloud<pcl::PointXYZRGBA> rgb_copy;
- copyPointCloud (*voxels, rgb_copy);
-
- pcl::PointCloud<pcl::PointXYZRGBA>::iterator rgb_copy_itr = rgb_copy.begin ();
- for ( ; rgb_copy_itr != rgb_copy.end (); ++rgb_copy_itr)
- rgb_copy_itr->rgba = label_colors_ [sv_itr->getLabel ()];
-
- *colored_cloud += rgb_copy;
- }
-
- return colored_cloud;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> float
pcl::SupervoxelClustering<PointT>::getSeedResolution () const
{
- return (resolution_);
+ return (seed_resolution_);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
normal_importance_ = val;
}
-
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
-pcl::SupervoxelClustering<PointT>::initializeLabelColors ()
+pcl::SupervoxelClustering<PointT>::setUseSingleCameraTransform (bool val)
{
- uint32_t max_label = static_cast<uint32_t> (getMaxLabel ());
- //If we already have enough colors, return
- if (label_colors_.size () > max_label)
- return;
-
- //Otherwise, generate new colors until we have enough
- label_colors_.reserve (max_label + 1);
- srand (static_cast<unsigned int> (time (0)));
- while (label_colors_.size () <= max_label )
- {
- uint8_t r = static_cast<uint8_t>( (rand () % 256));
- uint8_t g = static_cast<uint8_t>( (rand () % 256));
- uint8_t b = static_cast<uint8_t>( (rand () % 256));
- label_colors_.push_back (static_cast<uint32_t>(r) << 16 | static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
- }
+ use_default_transform_behaviour_ = false;
+ use_single_camera_transform_ = val;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
namespace pcl
{
-
namespace octree
{
//Explicit overloads for RGB types
template<>
void
- pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGB,pcl::SupervoxelClustering<pcl::PointXYZRGB>::VoxelData>::addPoint (const pcl::PointXYZRGB &new_point)
- {
- ++num_points_;
- //Same as before here
- data_.xyz_[0] += new_point.x;
- data_.xyz_[1] += new_point.y;
- data_.xyz_[2] += new_point.z;
- //Separate sums for r,g,b since we cant sum in uchars
- data_.rgb_[0] += static_cast<float> (new_point.r);
- data_.rgb_[1] += static_cast<float> (new_point.g);
- data_.rgb_[2] += static_cast<float> (new_point.b);
- }
+ pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGB,pcl::SupervoxelClustering<pcl::PointXYZRGB>::VoxelData>::addPoint (const pcl::PointXYZRGB &new_point);
template<>
void
- pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGBA,pcl::SupervoxelClustering<pcl::PointXYZRGBA>::VoxelData>::addPoint (const pcl::PointXYZRGBA &new_point)
- {
- ++num_points_;
- //Same as before here
- data_.xyz_[0] += new_point.x;
- data_.xyz_[1] += new_point.y;
- data_.xyz_[2] += new_point.z;
- //Separate sums for r,g,b since we cant sum in uchars
- data_.rgb_[0] += static_cast<float> (new_point.r);
- data_.rgb_[1] += static_cast<float> (new_point.g);
- data_.rgb_[2] += static_cast<float> (new_point.b);
- }
-
-
+ pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGBA,pcl::SupervoxelClustering<pcl::PointXYZRGBA>::VoxelData>::addPoint (const pcl::PointXYZRGBA &new_point);
//Explicit overloads for RGB types
template<> void
- pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGB,pcl::SupervoxelClustering<pcl::PointXYZRGB>::VoxelData>::computeData ()
- {
- data_.rgb_[0] /= (static_cast<float> (num_points_));
- data_.rgb_[1] /= (static_cast<float> (num_points_));
- data_.rgb_[2] /= (static_cast<float> (num_points_));
- data_.xyz_[0] /= (static_cast<float> (num_points_));
- data_.xyz_[1] /= (static_cast<float> (num_points_));
- data_.xyz_[2] /= (static_cast<float> (num_points_));
- }
+ pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGB,pcl::SupervoxelClustering<pcl::PointXYZRGB>::VoxelData>::computeData ();
template<> void
- pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGBA,pcl::SupervoxelClustering<pcl::PointXYZRGBA>::VoxelData>::computeData ()
- {
- data_.rgb_[0] /= (static_cast<float> (num_points_));
- data_.rgb_[1] /= (static_cast<float> (num_points_));
- data_.rgb_[2] /= (static_cast<float> (num_points_));
- data_.xyz_[0] /= (static_cast<float> (num_points_));
- data_.xyz_[1] /= (static_cast<float> (num_points_));
- data_.xyz_[2] /= (static_cast<float> (num_points_));
- }
-
+ pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGBA,pcl::SupervoxelClustering<pcl::PointXYZRGBA>::VoxelData>::computeData ();
+
//Explicit overloads for XYZ types
template<>
void
- pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZ,pcl::SupervoxelClustering<pcl::PointXYZ>::VoxelData>::addPoint (const pcl::PointXYZ &new_point)
- {
- ++num_points_;
- //Same as before here
- data_.xyz_[0] += new_point.x;
- data_.xyz_[1] += new_point.y;
- data_.xyz_[2] += new_point.z;
- }
-
+ pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZ,pcl::SupervoxelClustering<pcl::PointXYZ>::VoxelData>::addPoint (const pcl::PointXYZ &new_point);
+
template<> void
- pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZ,pcl::SupervoxelClustering<pcl::PointXYZ>::VoxelData>::computeData ()
- {
- data_.xyz_[0] /= (static_cast<float> (num_points_));
- data_.xyz_[1] /= (static_cast<float> (num_points_));
- data_.xyz_[2] /= (static_cast<float> (num_points_));
- }
-
+ pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZ,pcl::SupervoxelClustering<pcl::PointXYZ>::VoxelData>::computeData ();
}
}
{
template<> void
- pcl::SupervoxelClustering<pcl::PointXYZRGB>::VoxelData::getPoint (pcl::PointXYZRGB &point_arg) const
- {
- point_arg.rgba = static_cast<uint32_t>(rgb_[0]) << 16 |
- static_cast<uint32_t>(rgb_[1]) << 8 |
- static_cast<uint32_t>(rgb_[2]);
- point_arg.x = xyz_[0];
- point_arg.y = xyz_[1];
- point_arg.z = xyz_[2];
- }
+ pcl::SupervoxelClustering<pcl::PointXYZRGB>::VoxelData::getPoint (pcl::PointXYZRGB &point_arg) const;
template<> void
- pcl::SupervoxelClustering<pcl::PointXYZRGBA>::VoxelData::getPoint (pcl::PointXYZRGBA &point_arg ) const
- {
- point_arg.rgba = static_cast<uint32_t>(rgb_[0]) << 16 |
- static_cast<uint32_t>(rgb_[1]) << 8 |
- static_cast<uint32_t>(rgb_[2]);
- point_arg.x = xyz_[0];
- point_arg.y = xyz_[1];
- point_arg.z = xyz_[2];
- }
+ pcl::SupervoxelClustering<pcl::PointXYZRGBA>::VoxelData::getPoint (pcl::PointXYZRGBA &point_arg ) const;
template<typename PointT> void
pcl::SupervoxelClustering<PointT>::VoxelData::getPoint (PointT &point_arg ) const
normal_arg.curvature = curvature_;
}
}
-
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SupervoxelClustering<PointT>::SupervoxelHelper::removeAllLeaves ()
{
- typename std::set<LeafContainerT*>::iterator leaf_itr;
+ typename SupervoxelHelper::iterator leaf_itr;
for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
{
VoxelData& voxel = ((*leaf_itr)->getData ());
std::vector<LeafContainerT*> new_owned;
new_owned.reserve (leaves_.size () * 9);
//For each leaf belonging to this supervoxel
- typename std::set<LeafContainerT*>::iterator leaf_itr;
+ typename SupervoxelHelper::iterator leaf_itr;
for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
{
//for each neighbor of the leaf
- for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
+ for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
{
//Get a reference to the data contained in the leaf
VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
template <typename PointT> void
pcl::SupervoxelClustering<PointT>::SupervoxelHelper::refineNormals ()
{
- typename std::set<LeafContainerT*>::iterator leaf_itr;
+ typename SupervoxelHelper::iterator leaf_itr;
//For each leaf belonging to this supervoxel, get its neighbors, build an index vector, compute normal
for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
{
indices.reserve (81);
//Push this point
indices.push_back (voxel_data.idx_);
- for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
+ for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
{
//Get a reference to the data contained in the leaf
VoxelData& neighbor_voxel_data = ((*neighb_itr)->getData ());
{
indices.push_back (neighbor_voxel_data.idx_);
//Also check its neighbors
- for (typename LeafContainerT::iterator neighb_neighb_itr=(*neighb_itr)->begin (); neighb_neighb_itr!=(*neighb_itr)->end (); ++neighb_neighb_itr)
+ for (typename LeafContainerT::const_iterator neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr)
{
VoxelData& neighb_neighb_voxel_data = (*neighb_neighb_itr)->getData ();
if (neighb_neighb_voxel_data.owner_ == this)
centroid_.normal_ = Eigen::Vector4f::Zero ();
centroid_.xyz_ = Eigen::Vector3f::Zero ();
centroid_.rgb_ = Eigen::Vector3f::Zero ();
- typename std::set<LeafContainerT*>::iterator leaf_itr = leaves_.begin ();
+ typename SupervoxelHelper::iterator leaf_itr = leaves_.begin ();
for ( ; leaf_itr!= leaves_.end (); ++leaf_itr)
{
const VoxelData& leaf_data = (*leaf_itr)->getData ();
voxels->clear ();
voxels->resize (leaves_.size ());
typename pcl::PointCloud<PointT>::iterator voxel_itr = voxels->begin ();
- //typename std::set<LeafContainerT*>::iterator leaf_itr;
- for (typename std::set<LeafContainerT*>::const_iterator leaf_itr = leaves_.begin ();
- leaf_itr != leaves_.end ();
- ++leaf_itr, ++voxel_itr)
+ typename SupervoxelHelper::const_iterator leaf_itr;
+ for ( leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++voxel_itr)
{
const VoxelData& leaf_data = (*leaf_itr)->getData ();
leaf_data.getPoint (*voxel_itr);
normals.reset (new pcl::PointCloud<Normal>);
normals->clear ();
normals->resize (leaves_.size ());
- typename std::set<LeafContainerT*>::const_iterator leaf_itr;
+ typename SupervoxelHelper::const_iterator leaf_itr;
typename pcl::PointCloud<Normal>::iterator normal_itr = normals->begin ();
for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++normal_itr)
{
{
neighbor_labels.clear ();
//For each leaf belonging to this supervoxel
- typename std::set<LeafContainerT*>::const_iterator leaf_itr;
+ typename SupervoxelHelper::const_iterator leaf_itr;
for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
{
//for each neighbor of the leaf
- for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
+ for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
{
//Get a reference to the data contained in the leaf
VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author : Christian Potthast
+ * Email : potthast@usc.edu
+ *
+ */
+
+#ifndef PCL_UNARY_CLASSIFIER_HPP_
+#define PCL_UNARY_CLASSIFIER_HPP_
+
+#include <Eigen/Core>
+#include <pcl/segmentation/unary_classifier.h>
+#include <pcl/common/io.h>
+#include <pcl/kdtree/flann.h>
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT>
+pcl::UnaryClassifier<PointT>::UnaryClassifier () :
+ input_cloud_ (new pcl::PointCloud<PointT>),
+ label_field_ (false),
+ normal_radius_search_ (0.01f),
+ fpfh_radius_search_ (0.05f),
+ feature_threshold_ (5.0)
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT>
+pcl::UnaryClassifier<PointT>::~UnaryClassifier ()
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::UnaryClassifier<PointT>::setInputCloud (typename pcl::PointCloud<PointT>::Ptr input_cloud)
+{
+ if (input_cloud_ != NULL)
+ input_cloud_.reset ();
+
+ input_cloud_ = input_cloud;
+
+ pcl::PointCloud <PointT> point;
+ std::vector<pcl::PCLPointField> fields;
+
+ int label_index = -1;
+ label_index = pcl::getFieldIndex (point, "label", fields);
+
+ if (label_index != -1)
+ label_field_ = true;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::UnaryClassifier<PointT>::convertCloud (typename pcl::PointCloud<PointT>::Ptr in,
+ pcl::PointCloud<pcl::PointXYZ>::Ptr out)
+{
+ // resize points of output cloud
+ out->points.resize (in->points.size ());
+ out->width = static_cast<int> (out->points.size ());
+ out->height = 1;
+ out->is_dense = false;
+
+ for (size_t i = 0; i < in->points.size (); i++)
+ {
+ pcl::PointXYZ point;
+ // fill X Y Z
+ point.x = in->points[i].x;
+ point.y = in->points[i].y;
+ point.z = in->points[i].z;
+ out->points[i] = point;
+ }
+}
+
+template <typename PointT> void
+pcl::UnaryClassifier<PointT>::convertCloud (typename pcl::PointCloud<PointT>::Ptr in,
+ pcl::PointCloud<pcl::PointXYZRGBL>::Ptr out)
+{
+ // TODO:: check if input cloud has RGBA information and insert into the cloud
+
+ // resize points of output cloud
+ out->points.resize (in->points.size ());
+ out->width = static_cast<int> (out->points.size ());
+ out->height = 1;
+ out->is_dense = false;
+
+ for (size_t i = 0; i < in->points.size (); i++)
+ {
+ pcl::PointXYZRGBL point;
+ // X Y Z R G B L
+ point.x = in->points[i].x;
+ point.y = in->points[i].y;
+ point.z = in->points[i].z;
+ //point.rgba = in->points[i].rgba;
+ point.label = 1;
+ out->points[i] = point;
+ }
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::UnaryClassifier<PointT>::findClusters (typename pcl::PointCloud<PointT>::Ptr in,
+ std::vector<int> &cluster_numbers)
+{
+ // find the 'label' field index
+ std::vector <pcl::PCLPointField> fields;
+ int label_idx = -1;
+ pcl::PointCloud <PointT> point;
+ label_idx = pcl::getFieldIndex (point, "label", fields);
+
+ if (label_idx != -1)
+ {
+ for (size_t i = 0; i < in->points.size (); i++)
+ {
+ // get the 'label' field
+ uint32_t label;
+ memcpy (&label, reinterpret_cast<char*> (&in->points[i]) + fields[label_idx].offset, sizeof(uint32_t));
+
+ // check if label exist
+ bool exist = false;
+ for (size_t j = 0; j < cluster_numbers.size (); j++)
+ {
+ if (static_cast<uint32_t> (cluster_numbers[j]) == label)
+ {
+ exist = true;
+ break;
+ }
+ }
+ if (!exist)
+ cluster_numbers.push_back (label);
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::UnaryClassifier<PointT>::getCloudWithLabel (typename pcl::PointCloud<PointT>::Ptr in,
+ pcl::PointCloud<pcl::PointXYZ>::Ptr out,
+ int label_num)
+{
+ // find the 'label' field index
+ std::vector <pcl::PCLPointField> fields;
+ int label_idx = -1;
+ pcl::PointCloud <PointT> point;
+ label_idx = pcl::getFieldIndex (point, "label", fields);
+
+ if (label_idx != -1)
+ {
+ for (size_t i = 0; i < in->points.size (); i++)
+ {
+ // get the 'label' field
+ uint32_t label;
+ memcpy (&label, reinterpret_cast<char*> (&in->points[i]) + fields[label_idx].offset, sizeof(uint32_t));
+
+ if (static_cast<int> (label) == label_num)
+ {
+ pcl::PointXYZ point;
+ // X Y Z
+ point.x = in->points[i].x;
+ point.y = in->points[i].y;
+ point.z = in->points[i].z;
+ out->points.push_back (point);
+ }
+ }
+ out->width = static_cast<int> (out->points.size ());
+ out->height = 1;
+ out->is_dense = false;
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::UnaryClassifier<PointT>::computeFPFH (pcl::PointCloud<pcl::PointXYZ>::Ptr in,
+ pcl::PointCloud<pcl::FPFHSignature33>::Ptr out,
+ float normal_radius_search,
+ float fpfh_radius_search)
+{
+ pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal> ());
+ pcl::search::KdTree<pcl::PointXYZ>::Ptr normals_tree (new pcl::search::KdTree<pcl::PointXYZ>);
+ pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n3d;
+
+ n3d.setRadiusSearch (normal_radius_search);
+ n3d.setSearchMethod (normals_tree);
+ // ---[ Estimate the point normals
+ n3d.setInputCloud (in);
+ n3d.compute (*normals);
+
+ // Create the FPFH estimation class, and pass the input dataset+normals to it
+ pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;
+ fpfh.setInputCloud (in);
+ fpfh.setInputNormals (normals);
+
+ pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
+ fpfh.setSearchMethod (tree);
+ fpfh.setRadiusSearch (fpfh_radius_search);
+ // Compute the features
+ fpfh.compute (*out);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::UnaryClassifier<PointT>::kmeansClustering (pcl::PointCloud<pcl::FPFHSignature33>::Ptr in,
+ pcl::PointCloud<pcl::FPFHSignature33>::Ptr out,
+ int k)
+{
+ pcl::Kmeans kmeans (static_cast<int> (in->points.size ()), 33);
+ kmeans.setClusterSize (k);
+
+ // add points to the clustering
+ for (size_t i = 0; i < in->points.size (); i++)
+ {
+ std::vector<float> data (33);
+ for (int idx = 0; idx < 33; idx++)
+ data[idx] = in->points[i].histogram[idx];
+ kmeans.addDataPoint (data);
+ }
+
+ // k-means clustering
+ kmeans.kMeans ();
+
+ // get the cluster centroids
+ pcl::Kmeans::Centroids centroids = kmeans.get_centroids ();
+
+ // initialize output cloud
+ out->width = static_cast<int> (centroids.size ());
+ out->height = 1;
+ out->is_dense = false;
+ out->points.resize (static_cast<int> (centroids.size ()));
+ // copy cluster centroids into feature cloud
+ for (size_t i = 0; i < centroids.size (); i++)
+ {
+ pcl::FPFHSignature33 point;
+ for (int idx = 0; idx < 33; idx++)
+ point.histogram[idx] = centroids[i][idx];
+ out->points[i] = point;
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::UnaryClassifier<PointT>::queryFeatureDistances (std::vector<pcl::PointCloud<pcl::FPFHSignature33>::Ptr> &trained_features,
+ pcl::PointCloud<pcl::FPFHSignature33>::Ptr query_features,
+ std::vector<int> &indi,
+ std::vector<float> &dist)
+{
+ // estimate the total number of row's needed
+ int n_row = 0;
+ for (size_t i = 0; i < trained_features.size (); i++)
+ n_row += static_cast<int> (trained_features[i]->points.size ());
+
+ // Convert data into FLANN format
+ int n_col = 33;
+ flann::Matrix<float> data (new float[n_row * n_col], n_row, n_col);
+ for (size_t k = 0; k < trained_features.size (); k++)
+ {
+ pcl::PointCloud<pcl::FPFHSignature33>::Ptr hist = trained_features[k];
+ size_t c = hist->points.size ();
+ for (size_t i = 0; i < c; ++i)
+ for (size_t j = 0; j < data.cols; ++j)
+ data[(k * c) + i][j] = hist->points[i].histogram[j];
+ }
+
+ // build kd-tree given the training features
+ flann::Index<flann::ChiSquareDistance<float> > *index;
+ index = new flann::Index<flann::ChiSquareDistance<float> > (data, flann::LinearIndexParams ());
+ //flann::Index<flann::ChiSquareDistance<float> > index (data, flann::LinearIndexParams ());
+ //flann::Index<flann::ChiSquareDistance<float> > index (data, flann::KMeansIndexParams (5, -1));
+ //flann::Index<flann::ChiSquareDistance<float> > index (data, flann::KDTreeIndexParams (4));
+ index->buildIndex ();
+
+ int k = 1;
+ indi.resize (query_features->points.size ());
+ dist.resize (query_features->points.size ());
+ // Query all points
+ for (size_t i = 0; i < query_features->points.size (); i++)
+ {
+ // Query point
+ flann::Matrix<float> p = flann::Matrix<float>(new float[n_col], 1, n_col);
+ memcpy (&p.ptr ()[0], query_features->points[i].histogram, p.cols * p.rows * sizeof (float));
+
+ flann::Matrix<int> indices (new int[k], 1, k);
+ flann::Matrix<float> distances (new float[k], 1, k);
+ index->knnSearch (p, indices, distances, k, flann::SearchParams (512));
+
+ indi[i] = indices[0][0];
+ dist[i] = distances[0][0];
+
+ delete[] p.ptr ();
+ }
+
+ //std::cout << "kdtree size: " << index->size () << std::endl;
+
+ delete[] data.ptr ();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::UnaryClassifier<PointT>::assignLabels (std::vector<int> &indi,
+ std::vector<float> &dist,
+ int n_feature_means,
+ float feature_threshold,
+ pcl::PointCloud<pcl::PointXYZRGBL>::Ptr out)
+
+{
+ float nfm = static_cast<float> (n_feature_means);
+ for (size_t i = 0; i < out->points.size (); i++)
+ {
+ if (dist[i] < feature_threshold)
+ {
+ float l = static_cast<float> (indi[i]) / nfm;
+ float intpart;
+ //float fractpart = modf (l , &intpart);
+ std::modf (l , &intpart);
+ int label = static_cast<int> (intpart);
+
+ out->points[i].label = label+2;
+ }
+ }
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::UnaryClassifier<PointT>::train (pcl::PointCloud<pcl::FPFHSignature33>::Ptr &output)
+{
+ // convert cloud into cloud with XYZ
+ pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ convertCloud (input_cloud_, tmp_cloud);
+
+ // compute FPFH feature histograms for all point of the input point cloud
+ pcl::PointCloud<pcl::FPFHSignature33>::Ptr feature (new pcl::PointCloud<pcl::FPFHSignature33>);
+ computeFPFH (tmp_cloud, feature, normal_radius_search_, fpfh_radius_search_);
+
+ //PCL_INFO ("Number of input cloud features: %d\n", static_cast<int> (feature->points.size ()));
+
+ // use k-means to cluster the features
+ kmeansClustering (feature, output, cluster_size_);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::UnaryClassifier<PointT>::trainWithLabel (
+ std::vector<pcl::PointCloud<pcl::FPFHSignature33>, Eigen::aligned_allocator<pcl::PointCloud<pcl::FPFHSignature33> > > &output)
+{
+ // find clusters
+ std::vector<int> cluster_numbers;
+ findClusters (input_cloud_, cluster_numbers);
+ std::cout << "cluster numbers: ";
+ for (size_t i = 0; i < cluster_numbers.size (); i++)
+ std::cout << cluster_numbers[i] << " ";
+ std::cout << std::endl;
+
+ for (size_t i = 0; i < cluster_numbers.size (); i++)
+ {
+ // extract all points with the same label number
+ pcl::PointCloud<pcl::PointXYZ>::Ptr label_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ getCloudWithLabel (input_cloud_, label_cloud, cluster_numbers[i]);
+
+ // compute FPFH feature histograms for all point of the input point cloud
+ pcl::PointCloud<pcl::FPFHSignature33>::Ptr feature (new pcl::PointCloud<pcl::FPFHSignature33>);
+ computeFPFH (label_cloud, feature, normal_radius_search_, fpfh_radius_search_);
+
+ // use k-means to cluster the features
+ pcl::PointCloud<pcl::FPFHSignature33>::Ptr kmeans_feature (new pcl::PointCloud<pcl::FPFHSignature33>);
+ kmeansClustering (feature, kmeans_feature, cluster_size_);
+
+ output.push_back (*kmeans_feature);
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::UnaryClassifier<PointT>::segment (pcl::PointCloud<pcl::PointXYZRGBL>::Ptr &out)
+{
+ if (trained_features_.size () > 0)
+ {
+ // convert cloud into cloud with XYZ
+ pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ convertCloud (input_cloud_, tmp_cloud);
+
+ // compute FPFH feature histograms for all point of the input point cloud
+ pcl::PointCloud<pcl::FPFHSignature33>::Ptr input_cloud_features (new pcl::PointCloud<pcl::FPFHSignature33>);
+ computeFPFH (tmp_cloud, input_cloud_features, normal_radius_search_, fpfh_radius_search_);
+
+ // query the distances from the input data features to all trained features
+ std::vector<int> indices;
+ std::vector<float> distance;
+ queryFeatureDistances (trained_features_, input_cloud_features, indices, distance);
+
+ // assign a label to each point of the input point cloud
+ int n_feature_means = static_cast<int> (trained_features_[0]->points.size ());
+ convertCloud (input_cloud_, out);
+ assignLabels (indices, distance, n_feature_means, feature_threshold_, out);
+ //std::cout << "Assign labels - DONE" << std::endl;
+ }
+ else
+ PCL_ERROR ("no training features set \n");
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+#define PCL_INSTANTIATE_UnaryClassifier(T) template class pcl::UnaryClassifier<T>;
+
+#endif // PCL_UNARY_CLASSIFIER_HPP_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_SEGMENTATION_LCCP_SEGMENTATION_H_
+#define PCL_SEGMENTATION_LCCP_SEGMENTATION_H_
+
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/segmentation/supervoxel_clustering.h>
+
+#define PCL_INSTANTIATE_LCCPSegmentation(T) template class PCL_EXPORTS pcl::LCCPSegmentation<T>;
+
+namespace pcl
+{
+ /** \brief A simple segmentation algorithm partitioning a supervoxel graph into groups of locally convex connected supervoxels separated by concave borders.
+ * \note If you use this in a scientific work please cite the following paper:
+ * S. C. Stein, M. Schoeler, J. Papon, F. Woergoetter
+ * Object Partitioning using Local Convexity
+ * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2014
+ * \author Simon Christoph Stein and Markus Schoeler (mschoeler@gwdg.de)
+ * \ingroup segmentation
+ */
+ template <typename PointT>
+ class LCCPSegmentation
+ {
+ /** \brief Edge Properties stored in the adjacency graph.*/
+ struct EdgeProperties
+ {
+ /** \brief Desribes the difference of normals of the two supervoxels being connected*/
+ float normal_difference;
+
+ /** \brief Desribes if a connection is convex or concave*/
+ bool is_convex;
+
+ /** \brief Describes if a connection is valid for the segment growing. Usually convex connections are and concave connection are not. Due to k-concavity a convex connection can be invalidated*/
+ bool is_valid;
+
+ /** \brief Additional member used for the CPC algorithm. If edge has already induced a cut, it should be ignored for further cutting.*/
+ bool used_for_cutting;
+
+ EdgeProperties () :
+ normal_difference (0), is_convex (false), is_valid (false), used_for_cutting (false)
+ {
+ }
+ };
+
+ public:
+
+ // Adjacency list with nodes holding labels (uint32_t) and edges holding EdgeProperties.
+ typedef typename boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, uint32_t, EdgeProperties> SupervoxelAdjacencyList;
+ typedef typename boost::graph_traits<SupervoxelAdjacencyList>::vertex_iterator VertexIterator;
+ typedef typename boost::graph_traits<SupervoxelAdjacencyList>::adjacency_iterator AdjacencyIterator;
+
+ typedef typename boost::graph_traits<SupervoxelAdjacencyList>::vertex_descriptor VertexID;
+ typedef typename boost::graph_traits<SupervoxelAdjacencyList>::edge_iterator EdgeIterator;
+ typedef typename boost::graph_traits<SupervoxelAdjacencyList>::out_edge_iterator OutEdgeIterator;
+ typedef typename boost::graph_traits<SupervoxelAdjacencyList>::edge_descriptor EdgeID;
+
+ LCCPSegmentation ();
+ virtual
+ ~LCCPSegmentation ();
+
+ /** \brief Reset internal memory. */
+ void
+ reset ();
+
+
+ /** \brief Set the supervoxel clusters as well as the adjacency graph for the segmentation.Those parameters are generated by using the \ref SupervoxelClustering class. To retrieve the output use the \ref segment method.
+ * \param[in] supervoxel_clusters_arg Map of < supervoxel labels, supervoxels >
+ * \param[in] label_adjacency_arg The graph defining the supervoxel adjacency relations
+ * \note Implicitly calls \ref reset */
+ inline void
+ setInputSupervoxels (const std::map<uint32_t, typename pcl::Supervoxel<PointT>::Ptr> &supervoxel_clusters_arg,
+ const std::multimap<uint32_t, uint32_t> &label_adjacency_arg)
+ {
+ // Initialization
+ prepareSegmentation (supervoxel_clusters_arg, label_adjacency_arg); // after this, sv_adjacency_list_ can be used to access adjacency list
+ supervoxels_set_ = true;
+ }
+
+ /** \brief Merge supervoxels using local convexity. The input parameters are generated by using the \ref SupervoxelClustering class. To retrieve the output use the \ref relabelCloud method.
+ * \note There are three ways to retrieve the segmentation afterwards: \ref relabelCloud, \ref getSegmentSupervoxelMap and \ref getSupervoxelSegmentMap*/
+ void
+ segment ();
+
+ /** \brief Relabels cloud with supervoxel labels with the computed segment labels. labeled_cloud_arg should be created using the \ref getLabeledCloud method of the \ref SupervoxelClustering class.
+ * \param[in,out] labeled_cloud_arg Cloud to relabel */
+ void
+ relabelCloud (pcl::PointCloud<pcl::PointXYZL> &labeled_cloud_arg);
+
+ /** \brief Get map<SegmentID, std::set<SuperVoxel IDs> >
+ * \param[out] segment_supervoxel_map_arg The output container. On error the map is empty. */
+ inline void
+ getSegmentToSupervoxelMap (std::map<uint32_t, std::set<uint32_t> >& segment_supervoxel_map_arg) const
+ {
+ if (grouping_data_valid_)
+ {
+ segment_supervoxel_map_arg = seg_label_to_sv_list_map_;
+ }
+ else
+ {
+ PCL_WARN ("[pcl::LCCPSegmentation::getSegmentMap] WARNING: Call function segment first. Nothing has been done. \n");
+ segment_supervoxel_map_arg = std::map<uint32_t, std::set<uint32_t> > ();
+ }
+ }
+
+ /** \brief Get map<Supervoxel_ID, Segment_ID>
+ * \param[out] supervoxel_segment_map_arg The output container. On error the map is empty. */
+ inline void
+ getSupervoxelToSegmentMap (std::map<uint32_t, uint32_t> supervoxel_segment_map_arg) const
+ {
+ if (grouping_data_valid_)
+ {
+ supervoxel_segment_map_arg = sv_label_to_seg_label_map_;
+ }
+ else
+ {
+ PCL_WARN ("[pcl::LCCPSegmentation::getSegmentMap] WARNING: Call function segment first. Nothing has been done. \n");
+ supervoxel_segment_map_arg = std::map<uint32_t, uint32_t> ();
+ }
+ }
+
+ /** \brief Get map <SegmentID, std::set<Neighboring SegmentIDs> >
+ * \param[out] segment_adjacency_map_arg map < SegmentID, std::set< Neighboring SegmentIDs> >. On error the map is empty. */
+ inline void
+ getSegmentAdjacencyMap (std::map<uint32_t, std::set<uint32_t> >& segment_adjacency_map_arg)
+ {
+ if (grouping_data_valid_)
+ {
+ if (seg_label_to_neighbor_set_map_.empty ())
+ computeSegmentAdjacency ();
+ segment_adjacency_map_arg = seg_label_to_neighbor_set_map_;
+ }
+ else
+ {
+ PCL_WARN ("[pcl::LCCPSegmentation::getSegmentAdjacencyMap] WARNING: Call function segment first. Nothing has been done. \n");
+ segment_adjacency_map_arg = std::map<uint32_t, std::set<uint32_t> > ();
+ }
+ }
+
+ /** \brief Get normal threshold
+ * \return The concavity tolerance angle in [deg] that is currently set */
+ inline float
+ getConcavityToleranceThreshold () const
+ {
+ return (concavity_tolerance_threshold_);
+ }
+
+ /** \brief Get the supervoxel adjacency graph with classified edges (boost::adjacency_list).
+ * \param[out] adjacency_list_arg The supervoxel adjacency list with classified (convex/concave) edges. On error the list is empty. */
+ inline void
+ getSVAdjacencyList (SupervoxelAdjacencyList& adjacency_list_arg) const
+ {
+ if (grouping_data_valid_)
+ {
+ adjacency_list_arg = sv_adjacency_list_;
+ }
+ else
+ {
+ PCL_WARN ("[pcl::LCCPSegmentation::getSVAdjacencyList] WARNING: Call function segment first. Nothing has been done. \n");
+ adjacency_list_arg = pcl::LCCPSegmentation<PointT>::SupervoxelAdjacencyList ();
+ }
+ }
+
+ /** \brief Set normal threshold
+ * \param[in] concavity_tolerance_threshold_arg the concavity tolerance angle in [deg] to set */
+ inline void
+ setConcavityToleranceThreshold (float concavity_tolerance_threshold_arg)
+ {
+ concavity_tolerance_threshold_ = concavity_tolerance_threshold_arg;
+ }
+
+ /** \brief Determines if a smoothness check is done during segmentation, trying to invalidate edges of non-smooth connected edges (steps). Two supervoxels are unsmooth if their plane-to-plane distance DIST > (expected_distance + smoothness_threshold_*voxel_resolution_). For parallel supervoxels, the expected_distance is zero.
+ * \param[in] use_smoothness_check_arg Determines if the smoothness check is used
+ * \param[in] voxel_res_arg The voxel resolution used for the supervoxels that are segmented
+ * \param[in] seed_res_arg The seed resolution used for the supervoxels that are segmented
+ * \param[in] smoothness_threshold_arg Threshold (/fudging factor) for smoothness constraint according to the above formula. */
+ inline void
+ setSmoothnessCheck (bool use_smoothness_check_arg,
+ float voxel_res_arg,
+ float seed_res_arg,
+ float smoothness_threshold_arg = 0.1)
+ {
+ use_smoothness_check_ = use_smoothness_check_arg;
+ voxel_resolution_ = voxel_res_arg;
+ seed_resolution_ = seed_res_arg;
+ smoothness_threshold_ = smoothness_threshold_arg;
+ }
+
+ /** \brief Determines if we want to use the sanity criterion to invalidate singular connected patches
+ * \param[in] use_sanity_criterion_arg Determines if the sanity check is performed */
+ inline void
+ setSanityCheck (const bool use_sanity_criterion_arg)
+ {
+ use_sanity_check_ = use_sanity_criterion_arg;
+ }
+
+ /** \brief Set the value used for k convexity. For k>0 convex connections between p_i and p_j require k common neighbors of these patches that have a convex connection to both.
+ * \param[in] k_factor_arg factor used for extended convexity check */
+ inline void
+ setKFactor (const uint32_t k_factor_arg)
+ {
+ k_factor_ = k_factor_arg;
+ }
+
+ /** \brief Set the value \ref min_segment_size_ used in \ref mergeSmallSegments
+ * \param[in] min_segment_size_arg Segments smaller than this size will be merged */
+ inline void
+ setMinSegmentSize (const uint32_t min_segment_size_arg)
+ {
+ min_segment_size_ = min_segment_size_arg;
+ }
+
+ protected:
+
+ /** \brief Segments smaller than \ref min_segment_size_ are merged to the label of largest neighbor */
+ void
+ mergeSmallSegments ();
+
+ /** \brief Compute the adjacency of the segments */
+ void
+ computeSegmentAdjacency ();
+
+ /** \brief Is called within \ref setInputSupervoxels mainly to reserve required memory.
+ * \param[in] supervoxel_clusters_arg map of < supervoxel labels, supervoxels >
+ * \param[in] label_adjacency_arg The graph defining the supervoxel adjacency relations */
+ void
+ prepareSegmentation (const std::map<uint32_t, typename pcl::Supervoxel<PointT>::Ptr> &supervoxel_clusters_arg,
+ const std::multimap<uint32_t, uint32_t> &label_adjacency_arg);
+
+
+ /** Perform depth search on the graph and recursively group all supervoxels with convex connections
+ * \note The vertices in the supervoxel adjacency list are the supervoxel centroids */
+ void
+ doGrouping ();
+
+ /** \brief Assigns neighbors of the query point to the same group as the query point. Recursive part of \ref doGrouping. Grouping is done by a depth-search of nodes in the adjacency-graph.
+ * \param[in] queryPointID ID of point whose neighbors will be considered for grouping
+ * \param[in] group_label ID of the group/segment the queried point belongs to */
+ void
+ recursiveSegmentGrowing (const VertexID &queryPointID,
+ const unsigned int group_label);
+
+ /** \brief Calculates convexity of edges and saves this to the adjacency graph.
+ * \param[in,out] adjacency_list_arg The supervoxel adjacency list*/
+ void
+ calculateConvexConnections (SupervoxelAdjacencyList& adjacency_list_arg);
+
+ /** \brief Connections are only convex if this is true for at least k_arg common neighbors of the two patches. Call \ref setKFactor before \ref segment to use this.
+ * \param[in] k_arg Factor used for extended convexity check */
+ void
+ applyKconvexity (const unsigned int k_arg);
+
+ /** \brief Returns true if the connection between source and target is convex.
+ * \param[in] source_label_arg Label of one supervoxel connected to the edge that should be checked
+ * \param[in] target_label_arg Label of the other supervoxel connected to the edge that should be checked
+ * \param[out] normal_angle The angle between source and target
+ * \return True if connection is convex */
+ bool
+ connIsConvex (const uint32_t source_label_arg,
+ const uint32_t target_label_arg,
+ float &normal_angle);
+
+ /// *** Parameters *** ///
+
+ /** \brief Normal Threshold in degrees [0,180] used for merging */
+ float concavity_tolerance_threshold_;
+
+ /** \brief Marks if valid grouping data (\ref sv_adjacency_list_, \ref sv_label_to_seg_label_map_, \ref processed_) is avaiable */
+ bool grouping_data_valid_;
+
+ /** \brief Marks if supervoxels have been set by calling \ref setInputSupervoxels */
+ bool supervoxels_set_;
+
+ /** \brief Determines if the smoothness check is used during segmentation*/
+ bool use_smoothness_check_;
+
+ /** \brief Two supervoxels are unsmooth if their plane-to-plane distance DIST > (expected_distance + smoothness_threshold_*voxel_resolution_). For parallel supervoxels, the expected_distance is zero. */
+ float smoothness_threshold_;
+
+ /** \brief Determines if we use the sanity check which tries to find and invalidate singular connected patches*/
+ bool use_sanity_check_;
+
+ /** \brief Seed resolution of the supervoxels (used only for smoothness check) */
+ float seed_resolution_;
+
+ /** \brief Voxel resolution used to build the supervoxels (used only for smoothness check)*/
+ float voxel_resolution_;
+
+ /** \brief Factor used for k-convexity */
+ uint32_t k_factor_;
+
+ /** \brief Minimum segment size */
+ uint32_t min_segment_size_;
+
+ /** \brief Stores which supervoxel labels were already visited during recursive grouping.
+ * \note processed_[sv_Label] = false (default)/true (already processed) */
+ std::map<uint32_t, bool> processed_;
+
+ /** \brief Adjacency graph with the supervoxel labels as nodes and edges between adjacent supervoxels */
+ SupervoxelAdjacencyList sv_adjacency_list_;
+
+ /** \brief map from the supervoxel labels to the supervoxel objects */
+ std::map<uint32_t, typename pcl::Supervoxel<PointT>::Ptr> sv_label_to_supervoxel_map_;
+
+ /** \brief Storing relation between original SuperVoxel Labels and new segmantion labels.
+ * \note sv_label_to_seg_label_map_[old_labelID] = new_labelID */
+ std::map<uint32_t, uint32_t> sv_label_to_seg_label_map_;
+
+ /** \brief map <Segment Label, std::set <SuperVoxel Labels> > */
+ std::map<uint32_t, std::set<uint32_t> > seg_label_to_sv_list_map_;
+
+ /** \brief map < SegmentID, std::set< Neighboring segment labels> > */
+ std::map<uint32_t, std::set<uint32_t> > seg_label_to_neighbor_set_map_;
+
+ };
+}
+
+#ifdef PCL_NO_PRECOMPILE
+#include <pcl/segmentation/impl/lccp_segmentation.hpp>
+#endif
+
+#endif // PCL_SEGMENTATION_LCCP_SEGMENTATION_H_
inline unsigned
findRoot (const std::vector<unsigned>& runs, unsigned index) const
{
- register unsigned idx = index;
+ unsigned idx = index;
while (runs[idx] != idx)
idx = runs[idx];
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_SEGMENTATION_RANDOM_WALKER_H
+#define PCL_SEGMENTATION_RANDOM_WALKER_H
+
+#include <boost/graph/adjacency_list.hpp>
+#include <boost/graph/graph_concepts.hpp>
+#include <boost/concept/assert.hpp>
+
+#include <Eigen/Dense>
+
+namespace pcl
+{
+
+ namespace segmentation
+ {
+
+ /** \brief Multilabel graph segmentation using random walks.
+ *
+ * This is an implementation of the algorithm described in "Random Walks
+ * for Image Segmentation" by Leo Grady.
+ *
+ * Given a weighted undirected graph and a small number of user-defined
+ * labels this algorithm analytically determines the probability that a
+ * random walker starting at each unlabeled vertex will first reach one
+ * of the prelabeled vertices. The unlabeled vertices are then assigned
+ * to the label for which the greatest probability is calculated.
+ *
+ * The input is a BGL graph, a property map that associates a weight to
+ * each edge of the graph, and a property map that contains initial
+ * vertex colors (the term "color" is used interchangeably with "label").
+ *
+ * \note The colors of unlabeled vertices should be set to 0, the colors
+ * of labeled vetices could be any positive numbers.
+ *
+ * \note This is the responsibility of the user to make sure that every
+ * connected component of the graph has at least one colored vertex. If
+ * the user failed to do so, then the behavior of the algorithm is
+ * undefined, i.e. it may or may not succeed, and also may or may not
+ * report failure.
+ *
+ * The output of the algorithm (i.e. label assignment) is written back
+ * to the color map.
+ *
+ * \param[in] graph an undirected graph with internal edge weight and
+ * vertex color property maps
+ *
+ * Several overloads of randomWalker() function are provided for
+ * convenience.
+ *
+ * \sa randomWalker(Graph&, EdgeWeightMap, VertexColorMap)
+ * \sa randomWalker(Graph&, EdgeWeightMap, VertexColorMap, Eigen::Matrix <typename boost::property_traits<EdgeWeightMap>::value_type, Eigen::Dynamic, Eigen::Dynamic>&, std::map<typename boost::property_traits <VertexColorMap>::value_type, size_t>&)
+ *
+ * \author Sergey Alexandrov
+ * \ingroup segmentation
+ */
+
+ template <class Graph> bool
+ randomWalker (Graph& graph);
+
+ /** \brief Multilabel graph segmentation using random walks.
+ *
+ * This is an overloaded function provided for convenience. See the
+ * documentation for randomWalker().
+ *
+ * \param[in] graph an undirected graph
+ * \param[in] weights an external edge weight property map
+ * \param[in,out] colors an external vertex color property map
+ *
+ * \author Sergey Alexandrov
+ * \ingroup segmentation
+ */
+ template <class Graph, class EdgeWeightMap, class VertexColorMap> bool
+ randomWalker (Graph& graph,
+ EdgeWeightMap weights,
+ VertexColorMap colors);
+
+ /** \brief Multilabel graph segmentation using random walks.
+ *
+ * This is an overloaded function provided for convenience. See the
+ * documentation for randomWalker().
+ *
+ * \param[in] graph an undirected graph
+ * \param[in] weights an external edge weight property map
+ * \param[in,out] colors an external vertex color property map
+ * \param[out] potentials a matrix with calculated probabilities,
+ * where rows correspond to vertices, and columns
+ * correspond to colors
+ * \param[out] colors_to_columns_map a mapping between colors and
+ * columns in \a potentials matrix
+ *
+ * \author Sergey Alexandrov
+ * \ingroup segmentation
+ */
+ template <class Graph, class EdgeWeightMap, class VertexColorMap> bool
+ randomWalker (Graph& graph,
+ EdgeWeightMap weights,
+ VertexColorMap colors,
+ Eigen::Matrix<typename boost::property_traits<EdgeWeightMap>::value_type, Eigen::Dynamic, Eigen::Dynamic>& potentials,
+ std::map<typename boost::property_traits<VertexColorMap>::value_type, size_t>& colors_to_columns_map);
+
+ }
+
+}
+
+#include <pcl/segmentation/impl/random_walker.hpp>
+
+#endif /* PCL_SEGMENTATION_RANDOM_WALKER_H */
+
getEpsAngle () const { return (eps_angle_); }
/** \brief Base method for segmentation of a model in a PointCloud given by <setInputCloud (), setIndices ()>
- * \param[in] inliers the resultant point indices that support the model found (inliers)
+ * \param[out] inliers the resultant point indices that support the model found (inliers)
* \param[out] model_coefficients the resultant model coefficients
*/
virtual void
voxels_ (new pcl::PointCloud<PointT> ()),
normals_ (new pcl::PointCloud<Normal> ())
{ }
-
+
typedef boost::shared_ptr<Supervoxel<PointT> > Ptr;
typedef boost::shared_ptr<const Supervoxel<PointT> > ConstPtr;
{
centroid_arg = centroid_;
}
-
+
/** \brief Gets the point normal for the supervoxel
* \param[out] normal_arg Point normal of the supervoxel
* \note This isn't an average, it is a normal computed using all of the voxels in the supervoxel as support
normal_arg.normal_z = normal_.normal_z;
normal_arg.curvature = normal_.curvature;
}
-
+
/** \brief The normal calculated for the voxels contained in the supervoxel */
pcl::Normal normal_;
/** \brief The centroid of the supervoxel - average voxel */
typename pcl::PointCloud<PointT>::Ptr voxels_;
/** \brief A Pointcloud of the normals for the points in the supervoxel */
typename pcl::PointCloud<Normal>::Ptr normals_;
-
+
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
* \note Usually, color isn't needed (and can be detrimental)- spatial structure is mainly used
* - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter
* Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds
- * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013
+ * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013
+ * \ingroup segmentation
* \author Jeremie Papon (jpapon@gmail.com)
*/
template <typename PointT>
curvature_ (0.0f),
owner_ (0)
{}
-
+
/** \brief Gets the data of in the form of a point
* \param[out] point_arg Will contain the point value of the voxeldata
*/
void
getPoint (PointT &point_arg) const;
-
+
/** \brief Gets the data of in the form of a normal
* \param[out] normal_arg Will contain the normal value of the voxeldata
*/
void
getNormal (Normal &normal_arg) const;
-
+
Eigen::Vector3f xyz_;
Eigen::Vector3f rgb_;
Eigen::Vector4f normal_;
float distance_;
int idx_;
SupervoxelHelper* owner_;
-
+
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-
+
typedef pcl::octree::OctreePointCloudAdjacencyContainer<PointT, VoxelData> LeafContainerT;
typedef std::vector <LeafContainerT*> LeafVectorT;
-
+
typedef typename pcl::PointCloud<PointT> PointCloudT;
typedef typename pcl::PointCloud<Normal> NormalCloudT;
typedef typename pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT> OctreeAdjacencyT;
typedef typename pcl::octree::OctreePointCloudSearch <PointT> OctreeSearchT;
typedef typename pcl::search::KdTree<PointT> KdTreeT;
typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
-
+
using PCLBase <PointT>::initCompute;
using PCLBase <PointT>::deinitCompute;
using PCLBase <PointT>::input_;
-
+
typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, uint32_t, float> VoxelAdjacencyList;
typedef VoxelAdjacencyList::vertex_descriptor VoxelID;
typedef VoxelAdjacencyList::edge_descriptor EdgeID;
-
-
+
public:
/** \brief Constructor that sets default values for member variables.
* \param[in] voxel_resolution The resolution (in meters) of voxels used
* \param[in] seed_resolution The average size (in meters) of resulting supervoxels
- * \param[in] use_single_camera_transform Set to true if point density in cloud falls off with distance from origin (such as with a cloud coming from one stationary camera), set false if input cloud is from multiple captures from multiple locations.
*/
- SupervoxelClustering (float voxel_resolution, float seed_resolution, bool use_single_camera_transform = true);
+ SupervoxelClustering (float voxel_resolution, float seed_resolution);
+
+ PCL_DEPRECATED ("SupervoxelClustering constructor with flag for using the single camera transform is deprecated. Default behavior is now to use the transform for organized clouds, and not use it for unorganized. To force use/disuse of the transform, use the setUseSingleCameraTransform(bool) function.")
+ SupervoxelClustering (float voxel_resolution, float seed_resolution, bool);
/** \brief This destructor destroys the cloud, normals and search method used for
* finding neighbors. In other words it frees memory.
/** \brief Set the resolution of the octree voxels */
void
setVoxelResolution (float resolution);
-
+
/** \brief Get the resolution of the octree voxels */
float
getVoxelResolution () const;
-
+
/** \brief Set the resolution of the octree seed voxels */
void
setSeedResolution (float seed_resolution);
-
+
/** \brief Get the resolution of the octree seed voxels */
float
getSeedResolution () const;
-
+
/** \brief Set the importance of color for supervoxels */
void
setColorImportance (float val);
-
+
/** \brief Set the importance of spatial distance for supervoxels */
void
setSpatialImportance (float val);
-
+
/** \brief Set the importance of scalar normal product for supervoxels */
void
setNormalImportance (float val);
+
+ /** \brief Set whether or not to use the single camera transform
+ * \note By default it will be used for organized clouds, but not for unorganized - this parameter will override that behavior
+ * The single camera transform scales bin size so that it increases exponentially with depth (z dimension).
+ * This is done to account for the decreasing point density found with depth when using an RGB-D camera.
+ * Without the transform, beyond a certain depth adjacency of voxels breaks down unless the voxel size is set to a large value.
+ * Using the transform allows preserving detail up close, while allowing adjacency at distance.
+ * The specific transform used here is:
+ * x /= z; y /= z; z = ln(z);
+ * This transform is applied when calculating the octree bins in OctreePointCloudAdjacency
+ */
+ void
+ setUseSingleCameraTransform (bool val);
/** \brief This method launches the segmentation algorithm and returns the supervoxels that were
* obtained during the segmentation.
*/
virtual void
setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr& cloud);
-
+
/** \brief This method sets the normals to be used for supervoxels (should be same size as input cloud)
* \param[in] normal_cloud The input normals
*/
virtual void
setNormalCloud (typename NormalCloudT::ConstPtr normal_cloud);
-
+
/** \brief This method refines the calculated supervoxels - may only be called after extract
* \param[in] num_itr The number of iterations of refinement to be done (2 or 3 is usually sufficient)
* \param[out] supervoxel_clusters The resulting refined supervoxels
*/
virtual void
refineSupervoxels (int num_itr, std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
-
+
////////////////////////////////////////////////////////////
/** \brief Returns an RGB colorized cloud showing superpixels
* Otherwise it returns an empty pointer.
* color(it's random). Points that are unlabeled will be black
* \note This will expand the label_colors_ vector so that it can accomodate all labels
*/
+ PCL_DEPRECATED ("SupervoxelClustering::getColoredCloud is deprecated. Use the getLabeledCloud function instead. examples/segmentation/example_supervoxels.cpp shows how to use this to display and save with colorized labels.")
typename pcl::PointCloud<PointXYZRGBA>::Ptr
- getColoredCloud () const;
-
+ getColoredCloud () const
+ {
+ return boost::make_shared<pcl::PointCloud<PointXYZRGBA> > ();
+ }
+
/** \brief Returns a deep copy of the voxel centroid cloud */
typename pcl::PointCloud<PointT>::Ptr
getVoxelCentroidCloud () const;
-
+
/** \brief Returns labeled cloud
* Points that belong to the same supervoxel have the same label.
* Labels for segments start from 1, unlabled points have label 0
*/
typename pcl::PointCloud<PointXYZL>::Ptr
getLabeledCloud () const;
-
+
/** \brief Returns an RGB colorized voxelized cloud showing superpixels
* Otherwise it returns an empty pointer.
* Points that belong to the same supervoxel have the same color.
* color(it's random). Points that are unlabeled will be black
* \note This will expand the label_colors_ vector so that it can accomodate all labels
*/
+ PCL_DEPRECATED ("SupervoxelClustering::getColoredVoxelCloud is deprecated. Use the getLabeledVoxelCloud function instead. examples/segmentation/example_supervoxels.cpp shows how to use this to display and save with colorized labels.")
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
- getColoredVoxelCloud () const;
-
+ getColoredVoxelCloud () const
+ {
+ return boost::make_shared<pcl::PointCloud<PointXYZRGBA> > ();
+ }
+
/** \brief Returns labeled voxelized cloud
* Points that belong to the same supervoxel have the same label.
* Labels for segments start from 1, unlabled points have label 0
*/
void
getSupervoxelAdjacencyList (VoxelAdjacencyList &adjacency_list_arg) const;
-
+
/** \brief Get a multimap which gives supervoxel adjacency
* \param[out] label_adjacency Multi-Map which maps a supervoxel label to all adjacent supervoxel labels
*/
void
getSupervoxelAdjacency (std::multimap<uint32_t, uint32_t> &label_adjacency) const;
-
+
/** \brief Static helper function which returns a pointcloud of normals for the input supervoxels
* \param[in] supervoxel_clusters Supervoxel cluster map coming from this class
* \returns Cloud of PointNormals of the supervoxels
*/
static pcl::PointCloud<pcl::PointNormal>::Ptr
makeSupervoxelNormalCloud (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
-
+
/** \brief Returns the current maximum (highest) label */
int
getMaxLabel () const;
-
+
private:
-
- /** \brief This method initializes the label_colors_ vector (assigns random colors to labels)
- * \note Checks to see if it is already big enough - if so, does not reinitialize it
- */
- void
- initializeLabelColors ();
-
/** \brief This method simply checks if it is possible to execute the segmentation algorithm with
* the current settings. If it is possible then it returns true.
*/
prepareForSegmentation ();
/** \brief This selects points to use as initial supervoxel centroids
- * \param[out] seed_points The selected points
+ * \param[out] seed_indices The selected leaf indices
*/
void
- selectInitialSupervoxelSeeds (std::vector<PointT, Eigen::aligned_allocator<PointT> > &seed_points);
-
+ selectInitialSupervoxelSeeds (std::vector<int> &seed_indices);
+
/** \brief This method creates the internal supervoxel helpers based on the provided seed points
- * \param[in] seed_points The selected points
+ * \param[in] seed_indices Indices of the leaves to use as seeds
*/
void
- createSupervoxelHelpers (std::vector<PointT, Eigen::aligned_allocator<PointT> > &seed_points);
-
+ createSupervoxelHelpers (std::vector<int> &seed_indices);
+
/** \brief This performs the superpixel evolution */
void
expandSupervoxels (int depth);
/** \brief This sets the data of the voxels in the tree */
void
computeVoxelData ();
-
+
/** \brief Reseeds the supervoxels by finding the voxel closest to current centroid */
void
reseedSupervoxels ();
-
+
/** \brief Constructs the map of supervoxel clusters from the internal supervoxel helpers */
void
makeSupervoxels (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
-
+
/** \brief Stores the resolution used in the octree */
float resolution_;
-
+
/** \brief Stores the resolution used to seed the superpixels */
float seed_resolution_;
-
+
/** \brief Distance function used for comparing voxelDatas */
float
voxelDataDistance (const VoxelData &v1, const VoxelData &v2) const;
-
+
/** \brief Transform function used to normalize voxel density versus distance from camera */
void
transformFunction (PointT &p);
-
+
/** \brief Contains a KDtree for the voxelized cloud */
typename pcl::search::KdTree<PointT>::Ptr voxel_kdtree_;
-
+
/** \brief Octree Adjacency structure with leaves at voxel resolution */
typename OctreeAdjacencyT::Ptr adjacency_octree_;
-
+
/** \brief Contains the Voxelized centroid Cloud */
typename PointCloudT::Ptr voxel_centroid_cloud_;
-
+
/** \brief Contains the Voxelized centroid Cloud */
typename NormalCloudT::ConstPtr input_normals_;
-
+
/** \brief Importance of color in clustering */
float color_importance_;
/** \brief Importance of distance from seed center in clustering */
/** \brief Importance of similarity in normals for clustering */
float normal_importance_;
- /** \brief Stores the colors used for the superpixel labels*/
- std::vector<uint32_t> label_colors_;
+ /** \brief Whether or not to use the transform compressing depth in Z
+ * This is only checked if it has been manually set by the user.
+ * The default behavior is to use the transform for organized, and not for unorganized.
+ */
+ bool use_single_camera_transform_;
+ /** \brief Whether to use default transform behavior or not */
+ bool use_default_transform_behaviour_;
/** \brief Internal storage class for supervoxels
* \note Stores pointers to leaves of clustering internal octree,
class SupervoxelHelper
{
public:
+ /** \brief Comparator for LeafContainerT pointers - used for sorting set of leaves
+ * \note Compares by index in the overall leaf_vector. Order isn't important, so long as it is fixed.
+ */
+ struct compareLeaves
+ {
+ bool operator() (LeafContainerT* const &left, LeafContainerT* const &right) const
+ {
+ const VoxelData& leaf_data_left = left->getData ();
+ const VoxelData& leaf_data_right = right->getData ();
+ return leaf_data_left.idx_ < leaf_data_right.idx_;
+ }
+ };
+ typedef std::set<LeafContainerT*, typename SupervoxelHelper::compareLeaves> LeafSetT;
+ typedef typename LeafSetT::iterator iterator;
+ typedef typename LeafSetT::const_iterator const_iterator;
+
SupervoxelHelper (uint32_t label, SupervoxelClustering* parent_arg):
label_ (label),
parent_ (parent_arg)
{ }
-
+
void
addLeaf (LeafContainerT* leaf_arg);
-
+
void
removeLeaf (LeafContainerT* leaf_arg);
-
+
void
removeAllLeaves ();
-
+
void
expand ();
-
+
void
refineNormals ();
-
+
void
updateCentroid ();
-
+
void
getVoxels (typename pcl::PointCloud<PointT>::Ptr &voxels) const;
-
+
void
getNormals (typename pcl::PointCloud<Normal>::Ptr &normals) const;
-
+
typedef float (SupervoxelClustering::*DistFuncPtr)(const VoxelData &v1, const VoxelData &v2);
-
+
uint32_t
getLabel () const
{ return label_; }
-
+
Eigen::Vector4f
getNormal () const
{ return centroid_.normal_; }
-
+
Eigen::Vector3f
getRGB () const
{ return centroid_.rgb_; }
-
+
Eigen::Vector3f
getXYZ () const
{ return centroid_.xyz_;}
-
+
void
getXYZ (float &x, float &y, float &z) const
{ x=centroid_.xyz_[0]; y=centroid_.xyz_[1]; z=centroid_.xyz_[2]; }
-
+
void
getRGB (uint32_t &rgba) const
{
static_cast<uint32_t>(centroid_.rgb_[1]) << 8 |
static_cast<uint32_t>(centroid_.rgb_[2]);
}
-
+
void
getNormal (pcl::Normal &normal_arg) const
{
normal_arg.normal_z = centroid_.normal_[2];
normal_arg.curvature = centroid_.curvature_;
}
-
+
void
getNeighborLabels (std::set<uint32_t> &neighbor_labels) const;
-
+
VoxelData
getCentroid () const
{ return centroid_; }
-
-
+
size_t
size () const { return leaves_.size (); }
private:
//Stores leaves
- std::set<LeafContainerT*> leaves_;
+ LeafSetT leaves_;
uint32_t label_;
VoxelData centroid_;
SupervoxelClustering* parent_;
//Type VoxelData may have fixed-size Eigen objects inside
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-
+
//Make boost::ptr_list can access the private class SupervoxelHelper
friend void boost::checked_delete<> (const typename pcl::SupervoxelClustering<PointT>::SupervoxelHelper *);
-
+
typedef boost::ptr_list<SupervoxelHelper> HelperListT;
HelperListT supervoxel_helpers_;
-
+
//TODO DEBUG REMOVE
StopWatch timer_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-
};
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author : Christian Potthast
+ * Email : potthast@usc.edu
+ *
+ */
+
+#ifndef PCL_UNARY_CLASSIFIER_H_
+#define PCL_UNARY_CLASSIFIER_H_
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <pcl/features/fpfh.h>
+#include <pcl/features/normal_3d.h>
+
+#include <pcl/filters/filter_indices.h>
+#include <pcl/segmentation/extract_clusters.h>
+
+#include <pcl/ml/kmeans.h>
+
+namespace pcl
+{
+ /** \brief
+ *
+ */
+ template <typename PointT>
+ class PCL_EXPORTS UnaryClassifier
+ {
+ public:
+
+ /** \brief Constructor that sets default values for member variables. */
+ UnaryClassifier ();
+
+ /** \brief This destructor destroys the cloud...
+ *
+ */
+ ~UnaryClassifier ();
+
+ /** \brief This method sets the input cloud.
+ * \param[in] input_cloud input point cloud
+ */
+ void
+ setInputCloud (typename pcl::PointCloud<PointT>::Ptr input_cloud);
+
+ void
+ train (pcl::PointCloud<pcl::FPFHSignature33>::Ptr &output);
+
+ void
+ trainWithLabel (std::vector<pcl::PointCloud<pcl::FPFHSignature33>, Eigen::aligned_allocator<pcl::PointCloud<pcl::FPFHSignature33> > > &output);
+
+ void
+ segment (pcl::PointCloud<pcl::PointXYZRGBL>::Ptr &out);
+
+ void
+ queryFeatureDistances (std::vector<pcl::PointCloud<pcl::FPFHSignature33>::Ptr> &trained_features,
+ pcl::PointCloud<pcl::FPFHSignature33>::Ptr query_features,
+ std::vector<int> &indi,
+ std::vector<float> &dist);
+
+ void
+ assignLabels (std::vector<int> &indi,
+ std::vector<float> &dist,
+ int n_feature_means,
+ float feature_threshold,
+ pcl::PointCloud<pcl::PointXYZRGBL>::Ptr out);
+
+ void
+ setClusterSize (unsigned int k){cluster_size_ = k;};
+
+ void
+ setNormalRadiusSearch (float param){normal_radius_search_ = param;};
+
+ void
+ setFPFHRadiusSearch (float param){fpfh_radius_search_ = param;};
+
+ void
+ setLabelField (bool l){label_field_ = l;};
+
+ void
+ setTrainedFeatures (std::vector<pcl::PointCloud<pcl::FPFHSignature33>::Ptr> &features){trained_features_ = features;};
+
+ void
+ setFeatureThreshold (float threshold){feature_threshold_ = threshold;};
+
+ protected:
+
+ void
+ convertCloud (typename pcl::PointCloud<PointT>::Ptr in,
+ pcl::PointCloud<pcl::PointXYZ>::Ptr out);
+
+ void
+ convertCloud (typename pcl::PointCloud<PointT>::Ptr in,
+ pcl::PointCloud<pcl::PointXYZRGBL>::Ptr out);
+
+ void
+ findClusters (typename pcl::PointCloud<PointT>::Ptr in,
+ std::vector<int> &cluster_numbers);
+
+ void
+ getCloudWithLabel (typename pcl::PointCloud<PointT>::Ptr in,
+ pcl::PointCloud<pcl::PointXYZ>::Ptr out,
+ int label_num);
+
+ void
+ computeFPFH (pcl::PointCloud<pcl::PointXYZ>::Ptr in,
+ pcl::PointCloud<pcl::FPFHSignature33>::Ptr out,
+ float normal_radius_search,
+ float fpfh_radius_search);
+
+ void
+ kmeansClustering (pcl::PointCloud<pcl::FPFHSignature33>::Ptr in,
+ pcl::PointCloud<pcl::FPFHSignature33>::Ptr out,
+ int k);
+
+
+
+ /** \brief Contains the input cloud */
+ typename pcl::PointCloud<PointT>::Ptr input_cloud_;
+
+ bool label_field_;
+
+ unsigned int cluster_size_;
+
+ float normal_radius_search_;
+ float fpfh_radius_search_;
+ float feature_threshold_;
+
+
+ std::vector<pcl::PointCloud<pcl::FPFHSignature33>::Ptr> trained_features_;
+
+ /** \brief Contains normals of the points that will be segmented. */
+ //typename pcl::PointCloud<pcl::Normal>::Ptr normals_;
+
+ /** \brief Stores the cloud that will be segmented. */
+ //typename pcl::PointCloud<PointT>::Ptr cloud_for_segmentation_;
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+}
+
+#ifdef PCL_NO_PRECOMPILE
+#include <pcl/segmentation/impl/unary_classifier.hpp>
+#endif
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/point_types.h>
+#include <pcl/impl/instantiate.hpp>
+#include <pcl/segmentation/cpc_segmentation.h>
+#include <pcl/segmentation/impl/cpc_segmentation.hpp>
+
+PCL_INSTANTIATE(CPCSegmentation, (pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)(pcl::PointXYZRGBNormal))
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author : Christian Potthast
+ * Email : potthast@usc.edu
+ *
+ */
+
+#include <pcl/point_types.h>
+#include <pcl/impl/instantiate.hpp>
+#include <pcl/segmentation/crf_segmentation.h>
+#include <pcl/segmentation/impl/crf_segmentation.hpp>
+
+// Instantiations of specific point types
+template class pcl::CrfSegmentation<pcl::PointXYZRGB>;
+template class pcl::CrfSegmentation<pcl::PointXYZRGBA>;
+template class pcl::CrfSegmentation<pcl::PointXYZRGBNormal>;
#include <map>
#include <algorithm>
+const int pcl::segmentation::grabcut::BoykovKolmogorov::TERMINAL = -1;
+
pcl::segmentation::grabcut::BoykovKolmogorov::BoykovKolmogorov (std::size_t max_nodes)
: flow_value_(0.0)
{
if (source_edges_[u] == 0.0) continue;
// augment s-u-v-t paths
- for (std::map<int, double>::iterator it = nodes_[u].begin (); it != nodes_[u].end (); it++)
+ for (std::map<int, double>::iterator it = nodes_[u].begin (); it != nodes_[u].end (); ++it)
{
const int v = it->first;
if ((it->second == 0.0) || (target_edges_[v] == 0.0)) continue;
std::fill (target_edges_.begin (), target_edges_.end (), 0.0);
for (int u = 0; u < (int)nodes_.size (); u++)
{
- for (capacitated_edge::iterator it = nodes_[u].begin (); it != nodes_[u].end (); it++)
+ for (capacitated_edge::iterator it = nodes_[u].begin (); it != nodes_[u].end (); ++it)
{
it->second = 0.0;
}
const int u = active_head_;
if (cut_[u] == SOURCE) {
- for (capacitated_edge::iterator it = nodes_[u].begin (); it != nodes_[u].end (); it++)
+ for (capacitated_edge::iterator it = nodes_[u].begin (); it != nodes_[u].end (); ++it)
{
if (it->second > 0.0)
{
}
else
{
- for (capacitated_edge::iterator it = nodes_[u].begin (); it != nodes_[u].end (); it++)
+ for (capacitated_edge::iterator it = nodes_[u].begin (); it != nodes_[u].end (); ++it)
{
if (cut_[it->first] == TARGET) continue;
if (nodes_[it->first][u] > 0.0)
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/point_types.h>
+#include <pcl/impl/instantiate.hpp>
+#include <pcl/segmentation/lccp_segmentation.h>
+#include <pcl/segmentation/impl/lccp_segmentation.hpp>
+
+PCL_INSTANTIATE(LCCPSegmentation, (pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)(pcl::PointXYZRGBNormal))
\ No newline at end of file
#include <pcl/point_types.h>
#include <pcl/impl/instantiate.hpp>
#include <pcl/segmentation/impl/supervoxel_clustering.hpp>
-
#include <pcl/octree/impl/octree_pointcloud_adjacency.hpp>
-#include <pcl/octree/impl/octree_pointcloud.hpp>
-#include <pcl/octree/impl/octree_base.hpp>
-#include <pcl/octree/impl/octree_iterator.hpp>
-template class pcl::SupervoxelClustering<pcl::PointXYZRGBA>;
-template class pcl::SupervoxelClustering<pcl::PointXYZRGB>;
-template class pcl::SupervoxelClustering<pcl::PointXYZ>;
+namespace pcl
+{
+ namespace octree
+ {
+ //Explicit overloads for RGB types
+ template<>
+ void
+ pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGB,pcl::SupervoxelClustering<pcl::PointXYZRGB>::VoxelData>::addPoint (const pcl::PointXYZRGB &new_point)
+ {
+ ++num_points_;
+ //Same as before here
+ data_.xyz_[0] += new_point.x;
+ data_.xyz_[1] += new_point.y;
+ data_.xyz_[2] += new_point.z;
+ //Separate sums for r,g,b since we cant sum in uchars
+ data_.rgb_[0] += static_cast<float> (new_point.r);
+ data_.rgb_[1] += static_cast<float> (new_point.g);
+ data_.rgb_[2] += static_cast<float> (new_point.b);
+ }
+
+ template<>
+ void
+ pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGBA,pcl::SupervoxelClustering<pcl::PointXYZRGBA>::VoxelData>::addPoint (const pcl::PointXYZRGBA &new_point)
+ {
+ ++num_points_;
+ //Same as before here
+ data_.xyz_[0] += new_point.x;
+ data_.xyz_[1] += new_point.y;
+ data_.xyz_[2] += new_point.z;
+ //Separate sums for r,g,b since we cant sum in uchars
+ data_.rgb_[0] += static_cast<float> (new_point.r);
+ data_.rgb_[1] += static_cast<float> (new_point.g);
+ data_.rgb_[2] += static_cast<float> (new_point.b);
+ }
+
+
+
+ //Explicit overloads for RGB types
+ template<> void
+ pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGB,pcl::SupervoxelClustering<pcl::PointXYZRGB>::VoxelData>::computeData ()
+ {
+ data_.rgb_[0] /= (static_cast<float> (num_points_));
+ data_.rgb_[1] /= (static_cast<float> (num_points_));
+ data_.rgb_[2] /= (static_cast<float> (num_points_));
+ data_.xyz_[0] /= (static_cast<float> (num_points_));
+ data_.xyz_[1] /= (static_cast<float> (num_points_));
+ data_.xyz_[2] /= (static_cast<float> (num_points_));
+ }
+
+ template<> void
+ pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGBA,pcl::SupervoxelClustering<pcl::PointXYZRGBA>::VoxelData>::computeData ()
+ {
+ data_.rgb_[0] /= (static_cast<float> (num_points_));
+ data_.rgb_[1] /= (static_cast<float> (num_points_));
+ data_.rgb_[2] /= (static_cast<float> (num_points_));
+ data_.xyz_[0] /= (static_cast<float> (num_points_));
+ data_.xyz_[1] /= (static_cast<float> (num_points_));
+ data_.xyz_[2] /= (static_cast<float> (num_points_));
+ }
+
+ //Explicit overloads for XYZ types
+ template<>
+ void
+ pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZ,pcl::SupervoxelClustering<pcl::PointXYZ>::VoxelData>::addPoint (const pcl::PointXYZ &new_point)
+ {
+ ++num_points_;
+ //Same as before here
+ data_.xyz_[0] += new_point.x;
+ data_.xyz_[1] += new_point.y;
+ data_.xyz_[2] += new_point.z;
+ }
+
+ template<> void
+ pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZ,pcl::SupervoxelClustering<pcl::PointXYZ>::VoxelData>::computeData ()
+ {
+ data_.xyz_[0] /= (static_cast<float> (num_points_));
+ data_.xyz_[1] /= (static_cast<float> (num_points_));
+ data_.xyz_[2] /= (static_cast<float> (num_points_));
+ }
+
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+namespace pcl
+{
+ template<> void
+ pcl::SupervoxelClustering<pcl::PointXYZRGB>::VoxelData::getPoint (pcl::PointXYZRGB &point_arg) const
+ {
+ point_arg.rgba = static_cast<uint32_t>(rgb_[0]) << 16 |
+ static_cast<uint32_t>(rgb_[1]) << 8 |
+ static_cast<uint32_t>(rgb_[2]);
+ point_arg.x = xyz_[0];
+ point_arg.y = xyz_[1];
+ point_arg.z = xyz_[2];
+ }
+
+ template<> void
+ pcl::SupervoxelClustering<pcl::PointXYZRGBA>::VoxelData::getPoint (pcl::PointXYZRGBA &point_arg ) const
+ {
+ point_arg.rgba = static_cast<uint32_t>(rgb_[0]) << 16 |
+ static_cast<uint32_t>(rgb_[1]) << 8 |
+ static_cast<uint32_t>(rgb_[2]);
+ point_arg.x = xyz_[0];
+ point_arg.y = xyz_[1];
+ point_arg.z = xyz_[2];
+ }
+}
typedef pcl::SupervoxelClustering<pcl::PointXYZ>::VoxelData VoxelDataT;
typedef pcl::SupervoxelClustering<pcl::PointXYZRGB>::VoxelData VoxelDataRGBT;
typedef pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGB, VoxelDataRGBT> AdjacencyContainerRGBT;
typedef pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGBA, VoxelDataRGBAT> AdjacencyContainerRGBAT;
+template class pcl::SupervoxelClustering<pcl::PointXYZ>;
+template class pcl::SupervoxelClustering<pcl::PointXYZRGB>;
+template class pcl::SupervoxelClustering<pcl::PointXYZRGBA>;
+
template class pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZ, VoxelDataT>;
template class pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGB, VoxelDataRGBT>;
template class pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGBA, VoxelDataRGBAT>;
template class pcl::octree::OctreePointCloudAdjacency<pcl::PointXYZ, AdjacencyContainerT>;
template class pcl::octree::OctreePointCloudAdjacency<pcl::PointXYZRGB, AdjacencyContainerRGBT>;
-template class pcl::octree::OctreePointCloudAdjacency<pcl::PointXYZRGBA, AdjacencyContainerRGBAT>;
-
-//template class pcl::octree::OctreeBase<AdjacencyContainerRGBT, pcl::octree::OctreeContainerEmpty>;
-//template class pcl::octree::OctreeBase<AdjacencyContainerRGBAT, pcl::octree::OctreeContainerEmpty>;
-
-//template class pcl::octree::OctreeBreadthFirstIterator<pcl::octree::OctreeBase<AdjacencyContainerRGBT, pcl::octree::OctreeContainerEmpty> >;
-//template class pcl::octree::OctreeBreadthFirstIterator<pcl::octree::OctreeBase<AdjacencyContainerRGBAT, pcl::octree::OctreeContainerEmpty> >;
-
+template class pcl::octree::OctreePointCloudAdjacency<pcl::PointXYZRGBA, AdjacencyContainerRGBAT>;
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author : Christian Potthast
+ * Email : potthast@usc.edu
+ *
+ */
+
+#include <pcl/point_types.h>
+#include <pcl/impl/instantiate.hpp>
+#include <pcl/segmentation/unary_classifier.h>
+#include <pcl/segmentation/impl/unary_classifier.hpp>
+
+// Instantiations of specific point types
+PCL_INSTANTIATE(UnaryClassifier, PCL_XYZ_POINT_TYPES)
--- /dev/null
+set(SUBSYS_NAME simulation)
+set(SUBSYS_DESC "Point Cloud Library Simulation")
+set(SUBSYS_DEPS common io surface kdtree features search octree visualization filters geometry)
+
+set(build FALSE)
+find_package(OpenGL)
+find_package(GLEW)
+
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" OFF)
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS opengl glew)
+
+PCL_ADD_DOC("${SUBSYS_NAME}")
+
+if(build)
+ set(srcs
+ src/camera.cpp
+ src/model.cpp
+ src/range_likelihood.cpp
+ src/scene.cpp
+ src/glsl_shader.cpp
+ src/sum_reduce.cpp
+ )
+
+ set(incs
+ "include/pcl/${SUBSYS_NAME}/camera.h"
+ "include/pcl/${SUBSYS_NAME}/model.h"
+ "include/pcl/${SUBSYS_NAME}/range_likelihood.h"
+ "include/pcl/${SUBSYS_NAME}/scene.h"
+ "include/pcl/${SUBSYS_NAME}/glsl_shader.h"
+ "include/pcl/${SUBSYS_NAME}/sum_reduce.h"
+ )
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include"
+ "${GLEW_INCLUDE_DIR}")
+
+ PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs})
+
+ target_link_libraries("${LIB_NAME}" pcl_common pcl_io
+ ${OPENGL_LIBRARIES} ${GLEW_LIBRARIES})
+
+ set(EXT_DEPS eigen3)
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}"
+ "${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "")
+
+ # Install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs})
+ #PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs})
+
+ add_subdirectory(tools)
+
+endif(build)
--- /dev/null
+#ifndef PCL_SIMULATION_CAMERA_HPP_
+#define PCL_SIMULATION_CAMERA_HPP_
+
+#include <boost/shared_ptr.hpp>
+#include <Eigen/Dense>
+#include <Eigen/StdVector>
+
+#include <pcl/pcl_macros.h>
+
+namespace pcl
+{
+ namespace simulation
+ {
+ class PCL_EXPORTS Camera
+ {
+ public:
+ typedef boost::shared_ptr<Camera> Ptr;
+ typedef boost::shared_ptr<const Camera> ConstPtr;
+
+ Camera () : x_ (0), y_ (0), z_ (0), roll_ (0), pitch_ (0), yaw_ (0)
+ {
+ updatePose ();
+ initializeCameraParameters ();
+ }
+
+ Camera (double x, double y, double z,
+ double roll, double pitch, double yaw) : x_ (x),
+ y_ (y),
+ z_ (z),
+ roll_ (roll),
+ pitch_ (pitch),
+ yaw_ (yaw)
+ {
+ updatePose ();
+ initializeCameraParameters ();
+ }
+
+ void
+ setParameters (int width, int height,
+ float fx, float fy,
+ float cx, float cy,
+ float z_near, float z_far);
+
+ Eigen::Matrix4f
+ getProjectionMatrix () { return projection_matrix_; }
+
+ double getX () const { return x_; }
+ void setX (double x) { x_ = x; updatePose (); }
+
+ double getY () const { return y_; }
+ void setY (double y) { y_ = y; updatePose(); }
+
+ double getZ () const { return z_; }
+ void setZ (double z) { z_ = z; updatePose(); }
+
+ double
+ getRoll () const { return roll_; }
+ void
+ setRoll (double roll) { roll_ = roll; updatePose (); }
+
+ double
+ getPitch() const {return pitch_;}
+ void
+ setPitch(double pitch) { pitch_ = pitch; updatePose (); }
+
+ double
+ getYaw () const { return yaw_; }
+ void
+ setYaw (double yaw) { yaw_ = yaw; updatePose (); }
+
+ Eigen::Isometry3d
+ getPose () const { return pose_; }
+
+ void set (double x, double y, double z, double roll, double pitch, double yaw)
+ {
+ x_ = x; y_ = y; z_ = z;
+ roll_ = roll; pitch_ = pitch; yaw_ = yaw;
+ updatePose();
+ }
+
+ void move (double vx, double vy, double vz);
+
+ // Return the pose of the camera:
+ Eigen::Vector3d getYPR ()
+ {
+ return Eigen::Vector3d (yaw_, pitch_, roll_);
+ }
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ private:
+ void
+ updatePose ();
+
+ void
+ initializeCameraParameters ();
+
+ void
+ updateProjectionMatrix ();
+
+ double x_,y_,z_;
+ double roll_,pitch_,yaw_;
+
+ Eigen::Isometry3d pose_;
+
+ // Camera Intrinsic Parameters
+ int width_;
+ int height_;
+ float fx_;
+ float fy_;
+ float cx_;
+ float cy_;
+
+ // min and max range of the camera
+ float z_near_;
+ float z_far_;
+
+ Eigen::Matrix4f projection_matrix_;
+ };
+ } // namespace - simulation
+} // namespace - pcl
+
+#endif /* PCL_SIMULATION_CAMERA_HPP_ */
--- /dev/null
+/*
+ * glsl_shader.h
+ *
+ * Created on: Nov 27, 2011
+ * Author: Hordur Johannsson
+ */
+
+#ifndef PCL_SIMULATION_GLSL_SHADER
+#define PCL_SIMULATION_GLSL_SHADER
+
+#include <pcl/pcl_exports.h>
+
+#include <GL/glew.h>
+#include <Eigen/Core>
+#include <boost/shared_ptr.hpp>
+
+namespace pcl
+{
+ namespace simulation
+ {
+ namespace gllib
+ {
+ enum ShaderType { VERTEX = GL_VERTEX_SHADER,
+ FRAGMENT = GL_FRAGMENT_SHADER,
+ GEOMETRY = GL_GEOMETRY_SHADER,
+ TESS_CONTROL = GL_TESS_CONTROL_SHADER,
+ TESS_EVALUATION = GL_TESS_EVALUATION_SHADER };
+
+ /**
+ * A GLSL shader program.
+ */
+ class PCL_EXPORTS Program
+ {
+ public:
+ typedef boost::shared_ptr<Program> Ptr;
+ typedef boost::shared_ptr<const Program> ConstPtr;
+
+ /**
+ * Construct an empty shader program.
+ */
+ Program ();
+
+ /**
+ * Destruct the shader program.
+ */
+ ~Program ();
+
+ /**
+ * Add a new shader object to the program.
+ */
+ bool addShaderText (const std::string& text, ShaderType shader_type);
+
+ /**
+ * Add a new shader object to the program.
+ */
+ bool addShaderFile (const std::string& text, ShaderType shader_type);
+
+ /**
+ * Link the program.
+ */
+ bool link ();
+
+ /**
+ * Return true if the program is linked.
+ */
+ bool isLinked ();
+
+ /**
+ * Use the program.
+ */
+ void use ();
+
+ // Set uniforms
+ void setUniform (const std::string& name, const Eigen::Vector2f& v);
+ void setUniform (const std::string& name, const Eigen::Vector3f& v);
+ void setUniform (const std::string& name, const Eigen::Vector4f& v);
+ void setUniform (const std::string& name, const Eigen::Matrix2f& v);
+ void setUniform (const std::string& name, const Eigen::Matrix3f& v);
+ void setUniform (const std::string& name, const Eigen::Matrix4f& v);
+ void setUniform (const std::string& name, float v);
+ void setUniform (const std::string& name, int v);
+ void setUniform (const std::string& name, bool v);
+
+ int getUniformLocation (const std::string& name);
+
+ void printActiveUniforms ();
+ void printActiveAttribs ();
+
+ GLuint programId () { return program_id_; }
+
+ static Ptr loadProgramFromFile (const std::string& vertex_shader_file, const std::string& fragment_shader_file);
+ static Ptr loadProgramFromText (const std::string& vertex_shader_text, const std::string& fragment_shader_text);
+
+ private:
+ GLuint program_id_;
+ };
+
+ GLenum PCL_EXPORTS getGLError ();
+ void PCL_EXPORTS printShaderInfoLog (GLuint shader);
+ void PCL_EXPORTS printProgramInfoLog (GLuint program);
+
+ } // namespace - gllib
+ } // namespace - simulation
+} // namespace - pcl
+
+#endif /* PCL_SIMULATION_GLSL_SHADER_HPP_ */
--- /dev/null
+#ifndef PCL_MODEL_HPP_
+#define PCL_MODEL_HPP_
+
+#if defined(_WIN32) && !defined(APIENTRY) && !defined(__CYGWIN__)
+# define WIN32_LEAN_AND_MEAN 1
+# include <windows.h>
+#endif
+
+#include <GL/glew.h>
+
+#include <pcl/pcl_config.h>
+#ifdef OPENGL_IS_A_FRAMEWORK
+# include <OpenGL/gl.h>
+# include <OpenGL/glu.h>
+#else
+# include <GL/gl.h>
+# include <GL/glu.h>
+#endif
+
+#include <boost/shared_ptr.hpp>
+#include <pcl/pcl_macros.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/PolygonMesh.h>
+#include <pcl/simulation/glsl_shader.h>
+
+namespace pcl
+{
+ namespace simulation
+ {
+ typedef struct _SinglePoly
+ {
+ float* vertices_;
+ float* colors_;
+ GLenum mode_;
+ GLuint nvertices_;
+ } SinglePoly;
+
+ struct Vertex
+ {
+ Vertex () {}
+ //Vertex(Eigen::Vector3f pos, Eigen::Vector3f norm) : pos(pos), norm(norm) {}
+ Vertex (Eigen::Vector3f pos, Eigen::Vector3f rgb) : pos (pos), rgb (rgb) {}
+ Eigen::Vector3f pos;
+ Eigen::Vector3f rgb;
+ //Eigen::Vector3f norm;
+ //Eigen::Vector2f tex;
+ };
+
+ struct Face
+ {
+ // Index int to the index list
+ unsigned int index_offset;
+ // Number of vertices on face
+ unsigned int count;
+ // Normal of face
+ Eigen::Vector3f norm;
+ };
+
+ typedef std::vector<Vertex> Vertices;
+ typedef std::vector<size_t> Indices;
+
+ class Model
+ {
+ public:
+ virtual void draw () = 0;
+
+ typedef boost::shared_ptr<Model> Ptr;
+ typedef boost::shared_ptr<const Model> ConstPtr;
+ };
+
+ class PCL_EXPORTS TriangleMeshModel : public Model
+ {
+ public:
+ typedef boost::shared_ptr<TriangleMeshModel> Ptr;
+ typedef boost::shared_ptr<const TriangleMeshModel> ConstPtr;
+
+ TriangleMeshModel (pcl::PolygonMesh::Ptr plg);
+
+ virtual
+ ~TriangleMeshModel ();
+
+ virtual void
+ draw ();
+
+ private:
+ GLuint vbo_;
+ GLuint ibo_;
+ GLuint size_;
+ //Vertices vertices_;
+ //Indices indices_;
+ };
+
+ class PCL_EXPORTS PolygonMeshModel : public Model
+ {
+ public:
+ PolygonMeshModel(GLenum mode, pcl::PolygonMesh::Ptr plg);
+ virtual ~PolygonMeshModel();
+ virtual void draw();
+
+ typedef boost::shared_ptr<PolygonMeshModel> Ptr;
+ typedef boost::shared_ptr<const PolygonMeshModel> ConstPtr;
+ private:
+ std::vector<SinglePoly> polygons;
+
+ /*
+ GL_POINTS;
+ GL_LINE_STRIP;
+ GL_LINE_LOOP;
+ GL_LINES;
+ GL_TRIANGLE_STRIP;
+ GL_TRIANGLE_FAN;
+ GL_TRIANGLES;
+ GL_QUAD_STRIP;
+ GL_QUADS;
+ GL_POLYGON;
+ */
+ GLenum mode_;
+ };
+
+ class PCL_EXPORTS PointCloudModel : public Model
+ {
+ public:
+ typedef boost::shared_ptr<PointCloudModel> Ptr;
+ typedef boost::shared_ptr<const PointCloudModel> ConstPtr;
+
+ PointCloudModel (GLenum mode, pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc);
+
+ virtual
+ ~PointCloudModel ();
+
+ virtual void
+ draw();
+
+ private:
+ float* vertices_;
+ float* colors_;
+
+ /*
+ GL_POINTS;
+ GL_LINE_STRIP;
+ GL_LINE_LOOP;
+ GL_LINES;
+ GL_TRIANGLE_STRIP;
+ GL_TRIANGLE_FAN;
+ GL_TRIANGLES;
+ GL_QUAD_STRIP;
+ GL_QUADS;
+ GL_POLYGON;
+ */
+ GLenum mode_;
+ size_t nvertices_;
+ };
+
+ /**
+ * Renders a single quad providing position (-1,-1,0) - (1,1,0) and
+ * texture coordinates (0,0) - (1,1) to each vertex.
+ * Coordinates are (lower left) - (upper right).
+ * Position is set as vertex attribute 0 and the texture coordinate
+ * as vertex attribute 1.
+ */
+ class PCL_EXPORTS Quad
+ {
+ public:
+ /**
+ * Setup the vbo for the quad.
+ */
+ Quad ();
+
+ /**
+ * Release any resources.
+ */
+
+ ~Quad ();
+
+ /**
+ * Render the quad.
+ */
+ void
+ render ();
+
+ private:
+ GLuint quad_vbo_;
+ };
+
+ class PCL_EXPORTS TexturedQuad
+ {
+ public:
+ typedef boost::shared_ptr<TexturedQuad> Ptr;
+ typedef boost::shared_ptr<const TexturedQuad> ConstPtr;
+
+ TexturedQuad (int width, int height);
+ ~TexturedQuad ();
+
+ void
+ setTexture (const uint8_t* data);
+
+ void
+ render ();
+
+ private:
+ int width_;
+ int height_;
+ Quad quad_;
+ GLuint texture_;
+ gllib::Program::Ptr program_;
+ };
+ } // namespace - simulation
+} // namespace - pcl
+
+#endif /* PCL_SIMULATION_MODEL_HPP_ */
--- /dev/null
+#ifndef PCL_RANGE_LIKELIHOOD
+#define PCL_RANGE_LIKELIHOOD
+
+#include <GL/glew.h>
+
+#include <pcl/pcl_config.h>
+#ifdef OPENGL_IS_A_FRAMEWORK
+# include <OpenGL/gl.h>
+# include <OpenGL/glu.h>
+#else
+# include <GL/gl.h>
+# include <GL/glu.h>
+#endif
+
+#include <boost/random/linear_congruential.hpp>
+#include <boost/random/normal_distribution.hpp>
+#include <boost/random/variate_generator.hpp>
+#include <boost/math/distributions/normal.hpp> // for normal_distribution
+
+//#include <math.h>
+#include <Eigen/StdVector>
+
+#include <pcl/pcl_macros.h>
+//#include <pcl/win32_macros.h>
+#include <pcl/range_image/range_image_planar.h>
+#include <pcl/common/transforms.h>
+#include <pcl/simulation/camera.h>
+#include <pcl/simulation/scene.h>
+#include <pcl/simulation/glsl_shader.h>
+#include <pcl/simulation/sum_reduce.h>
+
+namespace pcl
+{
+ namespace simulation
+ {
+ class PCL_EXPORTS RangeLikelihood
+ {
+ public:
+ typedef boost::shared_ptr<RangeLikelihood> Ptr;
+ typedef boost::shared_ptr<const RangeLikelihood> ConstPtr;
+
+ public:
+ /**
+ * Create a new object to compute range image likelihoods.
+ *
+ * OpenGL is used to render the images. It is assumed that
+ * render buffers have already been setup. The area used is
+ * from 0,0 to cols*col_width,rows*row_height.
+ *
+ * @param rows - number of rows to use in the render buffer.
+ * @param cols - number of columns to use in the render buffer.
+ * @param row_height - height of the image for a single particle.
+ * @param col_width - width of the image for a single particle.
+ * @param scene - a pointer to the scene that should be rendered when
+ * computing likelihoods.
+ *
+ */
+ RangeLikelihood (int rows,
+ int cols,
+ int row_height,
+ int col_width,
+ Scene::Ptr scene);
+
+ /**
+ * Destroy the RangeLikelihood object and release any memory allocated.
+ */
+ ~RangeLikelihood ();
+
+ /**
+ * Computes the likelihood of reference for each of the provided poses.
+ *
+ * @param reference is a depth image.
+ * @param poses is a vector of the poses to test.
+ * @param scores is an output argument. The resulting log likelihoods will be written in score.
+ *
+ */
+ void
+ computeLikelihoods (float* reference,
+ std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses,
+ std::vector<float> & scores);
+
+ /**
+ * Set the basic camera intrinsic parameters
+ */
+ void
+ setCameraIntrinsicsParameters (int camera_width_in,
+ int camera_height_in,
+ float camera_fx_in,
+ float camera_fy_in,
+ float camera_cx_in,
+ float camera_cy_in)
+ {
+ camera_width_ = camera_width_in;
+ camera_height_ = camera_height_in;
+ camera_fx_ = camera_fx_in;
+ camera_fy_ = camera_fy_in;
+ camera_cx_ = camera_cx_in;
+ camera_cy_ = camera_cy_in;
+ }
+
+ /**
+ * Set the cost function to be used - one of 4 hard coded currently
+ */
+ void setCostFunction (int which_cost_function_in){ which_cost_function_ = which_cost_function_in;}
+ void setSigma (double sigma_in){ sigma_ = sigma_in; }
+ void setFloorProportion (double floor_proportion_in){ floor_proportion_ = floor_proportion_in;}
+
+ int getRows () {return rows_;}
+ int getCols () {return cols_;}
+ int getRowHeight () {return row_height_;}
+ int getColWidth () {return col_width_;}
+ int getWidth () {return width_;}
+ int getHeight () {return height_;}
+
+ // Convenience function to return simulated RGB-D PointCloud
+ // Two modes:
+ // global=false - PointCloud is as would be captured by an RGB-D camera [default]
+ // global=true - PointCloud is transformed into the model/world frame using the camera pose
+ void getPointCloud (pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc,
+ bool make_global, const Eigen::Isometry3d & pose);
+
+ // Convenience function to return RangeImagePlanar containing
+ // simulated RGB-D:
+ void getRangeImagePlanar (pcl::RangeImagePlanar &rip);
+
+ // Add various types of noise to simulated RGB-D data
+ void addNoise ();
+ double sampleNormal (double sigma = 1.0);
+
+ void
+ setComputeOnCPU(bool compute_on_cpu) { compute_likelihood_on_cpu_ = compute_on_cpu; }
+
+ void
+ setSumOnCPU(bool sum_on_cpu) { aggregate_on_cpu_ = sum_on_cpu; }
+
+ void
+ setUseColor(bool use_color) { use_color_ = use_color; }
+
+ const uint8_t*
+ getColorBuffer ();
+
+ const float*
+ getDepthBuffer ();
+
+ const float*
+ getScoreBuffer ();
+
+ private:
+ /**
+ * Evaluate the likelihood/score for a set of particles
+ *
+ * @param[in] reference - input Measurement depth image (raw data)
+ * @param[out] scores - output score
+ */
+ void
+ computeScores (float* reference,
+ std::vector<float> & scores);
+
+ void
+ computeScoresShader (float* reference);
+
+ void
+ render (const std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > & poses);
+
+ void
+ drawParticles (std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses);
+
+ void
+ applyCameraTransform (const Eigen::Isometry3d & pose);
+
+ void
+ applyCameraTransform (const Camera & camera);
+
+ void
+ setupProjectionMatrix ();
+
+ Scene::Ptr scene_;
+ int rows_;
+ int cols_;
+ int row_height_;
+ int col_width_;
+ int width_;
+ int height_;
+ float* depth_buffer_;
+ uint8_t* color_buffer_;
+
+ // Camera Intrinsic Parameters
+ int camera_width_;
+ int camera_height_;
+ float camera_fx_;
+ float camera_fy_;
+ float camera_cx_;
+ float camera_cy_;
+
+ // min and max range of the rgbd sensor
+ // everything outside this doesnt appear in depth images
+ float z_near_;
+ float z_far_;
+
+ bool depth_buffer_dirty_;
+ bool color_buffer_dirty_;
+ bool score_buffer_dirty_;
+
+ int which_cost_function_;
+ double floor_proportion_;
+ double sigma_;
+
+ GLuint fbo_;
+ GLuint score_fbo_;
+
+ GLuint depth_render_buffer_;
+ GLuint color_render_buffer_;
+ GLuint color_texture_;
+ GLuint depth_texture_;
+ GLuint score_texture_;
+ GLuint score_summarized_texture_;
+ GLuint sensor_texture_;
+ GLuint likelihood_texture_;
+
+ bool compute_likelihood_on_cpu_;
+ bool aggregate_on_cpu_;
+ bool use_instancing_;
+ bool generate_color_image_;
+ bool use_color_;
+
+ gllib::Program::Ptr likelihood_program_;
+ GLuint quad_vbo_;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vertices_;
+ float* score_buffer_;
+ Quad quad_;
+ SumReduce sum_reduce_;
+ };
+
+ template<class T> T
+ sqr(T val) { return val*val; }
+
+ } // namespace - simulation
+
+} // namespace - pcl
+
+#endif
--- /dev/null
+/*
+ * scene.hpp
+ *
+ * Created on: Aug 16, 2011
+ * Author: Hordur Johannsson
+ */
+
+#ifndef PCL_SIMULATION_SCENE_HPP_
+#define PCL_SIMULATION_SCENE_HPP_
+
+#include <boost/shared_ptr.hpp>
+
+#include <pcl/pcl_macros.h>
+//#include <pcl/win32_macros.h>
+
+#include <pcl/simulation/camera.h>
+#include <pcl/simulation/model.h>
+
+namespace pcl
+{
+ namespace simulation
+ {
+ class PCL_EXPORTS Scene
+ {
+ public:
+ typedef boost::shared_ptr<Scene> Ptr;
+ typedef boost::shared_ptr<Scene> ConstPtr;
+
+ void
+ draw ();
+
+ void
+ add (Model::Ptr model);
+
+ void
+ addCompleteModel (std::vector<Model::Ptr> model);
+
+ void
+ clear ();
+
+ private:
+ std::vector<Model::Ptr> models_;
+ };
+
+ } // namespace - simulation
+} // namespace - pcl
+
+#endif
--- /dev/null
+/*
+ * sum_reduce.hpp
+ *
+ * Author: Hordur Johannsson
+ *
+ */
+
+#ifndef PCL_SIMULATION_SUM_REDUCE
+#define PCL_SIMULATION_SUM_REDUCE
+
+#include <GL/glew.h>
+
+#include <pcl/pcl_config.h>
+#ifdef OPENGL_IS_A_FRAMEWORK
+# include <OpenGL/gl.h>
+#else
+# include <GL/gl.h>
+#endif
+
+#include <pcl/simulation/glsl_shader.h>
+#include <pcl/simulation/model.h>
+
+namespace pcl
+{
+ namespace simulation
+ {
+ /** \brief Implements a parallel summation of float arrays using GLSL.
+ * The input array is provided as a float texture and the summation
+ * is performed over set number of levels, where each level halfs each
+ * dimension.
+ *
+ * \author Hordur Johannsson
+ * \ingroup simulation
+ */
+ class PCL_EXPORTS SumReduce
+ {
+ public:
+ /** \brief Construct a new summation object for an array of given size.
+ * \param width[in] the width of the input array.
+ * \param width[in] the height of the input array.
+ * \param levels[in] the number of levels to carry out the reduction.
+ */
+ SumReduce (int width, int height, int levels);
+
+ /** \brief Release any allocated resources. */
+ ~SumReduce ();
+
+ /** \brief Reduce the array with summation over set number of levels.
+ * \param[in] input_array name of the input texture.
+ * \param[out] a pointer to an array that can store the summation result.
+ */
+ void sum (GLuint input_array, float* output_array);
+
+ private:
+ GLuint fbo_;
+ GLuint* arrays_;
+ Quad quad_;
+ int levels_;
+ int width_;
+ int height_;
+ gllib::Program::Ptr sum_program_;
+ };
+ } // namespace - simulation
+} // namespace - pcl
+
+#endif /* PCL_SIMULATION_SUM_REDUCE */
--- /dev/null
+#include <iostream>
+#include <pcl/simulation/camera.h>
+
+using namespace Eigen;
+using namespace pcl::simulation;
+
+void
+pcl::simulation::Camera::move (double vx, double vy, double vz)
+{
+ Vector3d v;
+ v << vx, vy, vz;
+ pose_.pretranslate (pose_.rotation ()*v);
+ x_ = pose_.translation ().x ();
+ y_ = pose_.translation ().y ();
+ z_ = pose_.translation ().z ();
+}
+
+void
+pcl::simulation::Camera::updatePose ()
+{
+ Matrix3d m;
+ m = AngleAxisd (yaw_, Vector3d::UnitZ ())
+ * AngleAxisd (pitch_, Vector3d::UnitY ())
+ * AngleAxisd (roll_, Vector3d::UnitX ());
+
+ pose_.setIdentity ();
+ pose_ *= m;
+
+ Vector3d v;
+ v << x_, y_, z_;
+ pose_.translation () = v;
+}
+
+void
+pcl::simulation::Camera::setParameters (int width, int height,
+ float fx, float fy,
+ float cx, float cy,
+ float z_near, float z_far)
+{
+ width_ = width;
+ height_ = height;
+ fx_ = fx;
+ fy_ = fy;
+ cx_ = cx;
+ cy_ = cy;
+ z_near_ = z_near;
+ z_far_ = z_far;
+
+ float z_nf = (z_near_-z_far_);
+ projection_matrix_ << 2.0f*fx_/width_, 0, 1.0f-(2.0f*cx_/width_), 0,
+ 0, 2.0f*fy_/height_, 1.0f-(2.0f*cy_/height_), 0,
+ 0, 0, (z_far_+z_near_)/z_nf, 2.0f*z_near_*z_far_/z_nf,
+ 0, 0, -1.0f, 0;
+}
+
+void
+pcl::simulation::Camera::initializeCameraParameters ()
+{
+ setParameters (640, 480,
+ 576.09757860f, 576.09757860f,
+ 321.06398107f, 242.97676897f,
+ 0.7f, 20.0f);
+}
--- /dev/null
+#version 330
+in vec2 TexCoord0;
+
+layout(location = 0) out vec4 FragColor;
+
+uniform sampler2D DepthSampler;
+uniform sampler2D ReferenceSampler;
+uniform sampler2D CostSampler;
+
+uniform int cols;
+uniform int rows;
+uniform float near;
+uniform float far;
+
+void main()
+{
+ float n = near;
+ float f = far;
+
+ float reference_depth_val = texture2D(ReferenceSampler, vec2(TexCoord0.s*cols, TexCoord0.t*rows)).r;
+ float depth_val = texture2D(DepthSampler, TexCoord0.st).r;
+ float ref_meters = 1.0 / (reference_depth_val * (1.0/f - 1.0/n) + 1.0/n);
+ float depth_meters = 1.0 / (depth_val * (1.0/f - 1.0/n) + 1.0/n);
+ float min_dist = abs(ref_meters - depth_meters);
+
+ float likelihood = texture2D(CostSampler, vec2(clamp(min_dist/3.0, 0.0, 1.0),0.0)).r;
+ float ratio = 0.99;
+ float r_min = 0.0; // meters
+ float r_max = 3.0; // meters
+
+ likelihood = log(ratio/(r_max - r_min) + (1.0 - ratio)*likelihood);
+ if (reference_depth_val < 0.0) likelihood = 0.0;
+
+ FragColor = vec4(likelihood, 0, 0, 0);
+}
--- /dev/null
+#version 330
+
+layout (location = 0) in vec3 Position;
+layout (location = 1) in vec2 TexCoord;
+
+out vec2 TexCoord0;
+
+void main()
+{
+ TexCoord0 = TexCoord;
+ gl_Position = vec4(Position, 1.0);
+};
--- /dev/null
+/*
+ * glsl_shader.cpp
+ *
+ * Created on: Nov 27, 2011
+ * Author: hordurj
+ */
+
+#include <pcl/simulation/glsl_shader.h>
+#include <iostream>
+#include <fstream>
+
+using namespace pcl::simulation::gllib;
+
+char*
+readTextFile (const char* filename)
+{
+ using namespace std;
+ char* buf = NULL;
+ ifstream file;
+ file.open (filename, ios::in|ios::binary|ios::ate);
+ if (file.is_open ())
+ {
+ ifstream::pos_type size;
+ size = file.tellg();
+ buf = new char[size + static_cast<ifstream::pos_type> (1)];
+ file.seekg (0, ios::beg);
+ file.read (buf, size);
+ file.close ();
+ buf[size] = 0;
+ }
+ return buf;
+}
+
+pcl::simulation::gllib::Program::Program ()
+{
+ program_id_ = glCreateProgram ();
+}
+
+pcl::simulation::gllib::Program::~Program ()
+{
+}
+
+int
+pcl::simulation::gllib::Program::getUniformLocation (const std::string& name)
+{
+ return glGetUniformLocation (program_id_, name.c_str ());
+}
+
+void
+pcl::simulation::gllib::Program::setUniform (const std::string& name, const Eigen::Vector2f& v)
+{
+ GLuint loc = getUniformLocation (name.c_str ());
+ glUniform2f(loc, v (0), v (1));
+}
+
+void
+pcl::simulation::gllib::Program::setUniform (const std::string& name, const Eigen::Vector3f& v)
+{
+ GLuint loc = getUniformLocation (name.c_str ());
+ glUniform3f(loc, v (0), v (1), v (2));
+}
+
+void
+pcl::simulation::gllib::Program::setUniform(const std::string& name, const Eigen::Vector4f& v)
+{
+ GLuint loc = getUniformLocation (name.c_str ());
+ glUniform4f (loc, v (0), v (1), v (2), v (4));
+}
+
+void
+pcl::simulation::gllib::Program::setUniform (const std::string& name, const Eigen::Matrix3f& v)
+{
+ GLuint loc = getUniformLocation (name.c_str ());
+ glUniformMatrix3fv (loc, 1, false, v.data ());
+}
+
+void
+pcl::simulation::gllib::Program::setUniform (const std::string& name, const Eigen::Matrix4f& v)
+{
+ GLuint loc = getUniformLocation (name.c_str ());
+ glUniformMatrix4fv (loc, 1, false, v.data ());
+}
+
+void
+pcl::simulation::gllib::Program::setUniform (const std::string& name, float v)
+{
+ GLuint loc = getUniformLocation (name.c_str ());
+ glUniform1f (loc, v);
+}
+
+void
+pcl::simulation::gllib::Program::setUniform (const std::string& name, int v)
+{
+ GLuint loc = getUniformLocation (name.c_str ());
+ glUniform1i (loc, v);
+}
+
+void
+pcl::simulation::gllib::Program::setUniform (const std::string& name, bool v)
+{
+ GLuint loc = getUniformLocation (name.c_str ());
+ glUniform1i(loc, (v?1:0));
+}
+
+bool
+pcl::simulation::gllib::Program::addShaderText (const std::string& text, ShaderType shader_type)
+{
+ GLuint id;
+ GLint compile_status;
+ id = glCreateShader (shader_type);
+ const char* source_list = text.c_str ();
+
+ glShaderSource (id, 1, &source_list, NULL);
+
+ glCompileShader (id);
+ printShaderInfoLog (id);
+ glGetShaderiv (id, GL_COMPILE_STATUS, &compile_status);
+ if (compile_status == GL_FALSE) return false;
+
+ if (getGLError () != GL_NO_ERROR) return false;
+
+ glAttachShader (program_id_, id);
+ return true;
+}
+
+bool
+pcl::simulation::gllib::Program::addShaderFile (const std::string& filename, ShaderType shader_type)
+{
+ char* text = readTextFile (filename.c_str ());
+ if(text == NULL) return (false);
+
+ std::string source(text);
+
+ bool rval = addShaderText (text, shader_type);
+ delete [] text;
+ return rval;
+}
+
+bool
+pcl::simulation::gllib::Program::link ()
+{
+ glLinkProgram (program_id_);
+ printProgramInfoLog (program_id_);
+
+ if (getGLError () != GL_NO_ERROR) return false;
+ return true;
+}
+
+void pcl::simulation::gllib::Program::use ()
+{
+ glUseProgram (program_id_);
+}
+
+GLenum pcl::simulation::gllib::getGLError ()
+{
+ GLenum last_error = GL_NO_ERROR;
+ GLenum error = glGetError ();
+ while (error != GL_NO_ERROR)
+ {
+ last_error = error;
+ std::cout << "Error: OpenGL: " << gluErrorString(error) << std::endl;
+ error = glGetError ();
+ }
+ return last_error;
+}
+
+void
+pcl::simulation::gllib::printShaderInfoLog (GLuint shader)
+{
+ GLsizei max_length;
+ GLsizei length;
+ GLchar* info_log;
+
+ glGetShaderiv (shader, GL_INFO_LOG_LENGTH, &length);
+ max_length = length;
+ info_log = new GLchar[length+1];
+
+ glGetShaderInfoLog (shader, max_length, &length, info_log);
+
+ info_log[max_length] = 0;
+
+ std::cout << "Shader info log: " << std::endl << info_log << std::endl;
+
+ delete [] info_log;
+}
+
+void
+pcl::simulation::gllib::printProgramInfoLog (GLuint program)
+{
+ GLsizei max_length;
+ GLsizei length;
+ GLchar* info_log;
+
+ glGetProgramiv (program, GL_INFO_LOG_LENGTH, &length);
+ max_length = length;
+ info_log = new GLchar[length+1];
+
+ glGetProgramInfoLog (program, max_length, &length, info_log);
+
+ info_log[max_length] = 0;
+
+ std::cout << "Program info log: " << std::endl << info_log << std::endl;
+
+ delete [] info_log;
+}
+
+Program::Ptr
+pcl::simulation::gllib::Program::loadProgramFromText (const std::string& vertex_shader_text, const std::string& fragment_shader_text)
+{
+ // Load shader
+ Program::Ptr program = gllib::Program::Ptr (new gllib::Program ());
+ if (!program->addShaderText (vertex_shader_text, gllib::VERTEX))
+ {
+ std::cerr << "Failed loading vertex shader" << std::endl;
+ }
+
+ // TODO: to remove file dependency include the shader source in the binary
+ if (!program->addShaderFile (fragment_shader_text, gllib::FRAGMENT))
+ {
+ std::cerr << "Failed loading fragment shader" << std::endl;
+ }
+
+ program->link ();
+
+ return program;
+}
+
+Program::Ptr
+pcl::simulation::gllib::Program::loadProgramFromFile (const std::string& vertex_shader_file, const std::string& fragment_shader_file)
+{
+ // Load shader
+ Program::Ptr program = gllib::Program::Ptr (new gllib::Program ());
+ if (!program->addShaderFile (vertex_shader_file, gllib::VERTEX))
+ {
+ std::cerr << "Failed loading vertex shader" << std::endl;
+ }
+
+ // TODO: to remove file dependency include the shader source in the binary
+ if (!program->addShaderFile (fragment_shader_file, gllib::FRAGMENT))
+ {
+ std::cerr << "Failed loading fragment shader" << std::endl;
+ }
+
+ program->link ();
+
+ return program;
+}
--- /dev/null
+#include <pcl/simulation/model.h>
+using namespace pcl::simulation;
+
+pcl::simulation::TriangleMeshModel::TriangleMeshModel (pcl::PolygonMesh::Ptr plg)
+{
+ Vertices vertices;
+ Indices indices;
+
+ bool found_rgb = false;
+ for (size_t i=0; i < plg->cloud.fields.size () ; ++i)
+ if (plg->cloud.fields[i].name.compare ("rgb") == 0)
+ found_rgb = true;
+
+ if (found_rgb)
+ {
+ pcl::PointCloud<pcl::PointXYZRGB> newcloud;
+ pcl::fromPCLPointCloud2 (plg->cloud, newcloud);
+
+ PCL_DEBUG("RGB Triangle mesh: ");
+ PCL_DEBUG("Mesh polygons: %ld", plg->polygons.size ());
+ PCL_DEBUG("Mesh points: %ld", newcloud.points.size ());
+
+ Eigen::Vector4f tmp;
+ for(size_t i=0; i< plg->polygons.size (); ++i)
+ { // each triangle/polygon
+ pcl::Vertices apoly_in = plg->polygons[i];
+ for(size_t j = 0; j < apoly_in.vertices.size (); ++j)
+ { // each point
+ uint32_t pt = apoly_in.vertices[j];
+ tmp = newcloud.points[pt].getVector4fMap();
+ vertices.push_back (Vertex (Eigen::Vector3f (tmp (0), tmp (1), tmp (2)),
+ Eigen::Vector3f (newcloud.points[pt].r/255.0f,
+ newcloud.points[pt].g/255.0f,
+ newcloud.points[pt].b/255.0f)));
+ indices.push_back (indices.size ());
+ }
+ }
+ }
+ else
+ {
+ pcl::PointCloud<pcl::PointXYZ> newcloud;
+ pcl::fromPCLPointCloud2 (plg->cloud, newcloud);
+ Eigen::Vector4f tmp;
+ for(size_t i=0; i< plg->polygons.size (); i++)
+ { // each triangle/polygon
+ pcl::Vertices apoly_in = plg->polygons[i];
+ for(size_t j=0; j< apoly_in.vertices.size (); j++)
+ { // each point
+ uint32_t pt = apoly_in.vertices[j];
+ tmp = newcloud.points[pt].getVector4fMap();
+ vertices.push_back (Vertex (Eigen::Vector3f (tmp (0), tmp (1), tmp (2)),
+ Eigen::Vector3f (1.0, 1.0, 1.0)));
+ indices.push_back (indices.size ());
+ }
+ }
+ }
+
+ PCL_DEBUG("Vertices: %ld", vertices.size ());
+ PCL_DEBUG("Indices: %ld", indices.size ());
+
+ glGenBuffers (1, &vbo_);
+ glBindBuffer (GL_ARRAY_BUFFER, vbo_);
+ glBufferData (GL_ARRAY_BUFFER, vertices.size () * sizeof (vertices[0]), &(vertices[0]), GL_STATIC_DRAW);
+ glBindBuffer (GL_ARRAY_BUFFER, 0);
+
+ glGenBuffers (1, &ibo_);
+ glBindBuffer (GL_ELEMENT_ARRAY_BUFFER, ibo_);
+ glBufferData (GL_ELEMENT_ARRAY_BUFFER, indices.size () * sizeof (indices[0]), &(indices[0]), GL_STATIC_DRAW);
+ glBindBuffer (GL_ELEMENT_ARRAY_BUFFER, 0);
+
+ if (indices.size () > std::numeric_limits<GLuint>::max ())
+ PCL_THROW_EXCEPTION(PCLException, "Too many vertices");
+
+ size_ = static_cast<GLuint>(indices.size ());
+}
+
+void
+pcl::simulation::TriangleMeshModel::draw ()
+{
+ glEnable (GL_DEPTH_TEST);
+
+ glEnableClientState (GL_VERTEX_ARRAY);
+ glEnableClientState (GL_COLOR_ARRAY);
+ //glEnableClientState(GL_NORMAL_ARRAY);
+ glBindBuffer (GL_ARRAY_BUFFER, vbo_);
+ glBindBuffer (GL_ELEMENT_ARRAY_BUFFER, ibo_);
+
+ glVertexPointer (3, GL_FLOAT, sizeof (Vertex), 0);
+ glColorPointer (3, GL_FLOAT, sizeof (Vertex), reinterpret_cast<GLvoid*> (12));
+
+ // glNormalPointer(GL_FLOAT, sizeof(Vertex), (GLvoid*)((char*)&(vertices_[0].norm)-(char*)&(vertices_[0].pos)));
+ // glDrawElements(GL_TRIANGLES, indices_.size(), GL_UNSIGNED_INT, 0);
+ glDrawElements (GL_TRIANGLES, size_, GL_UNSIGNED_INT, 0);
+ glDisableClientState (GL_VERTEX_ARRAY);
+ glDisableClientState (GL_COLOR_ARRAY);
+ //glDisableClientState(GL_NORMAL_ARRAY);
+ glVertexPointer (3, GL_FLOAT, 0, 0);
+ glColorPointer (3, GL_FLOAT, 0, 0);
+ //glNormalPointer(GL_FLOAT, 0, 0);
+
+ glBindBuffer (GL_ARRAY_BUFFER, 0);
+ glBindBuffer (GL_ELEMENT_ARRAY_BUFFER, 0);
+}
+
+pcl::simulation::TriangleMeshModel::~TriangleMeshModel ()
+{
+ if (glIsBuffer (vbo_) == GL_TRUE)
+ glDeleteBuffers (1, &vbo_);
+
+ if (glIsBuffer (ibo_) == GL_TRUE)
+ glDeleteBuffers (1, &ibo_);
+}
+
+// Create a PolygonMeshModel by converting the PolygonMesh to our format
+pcl::simulation::PolygonMeshModel::PolygonMeshModel (GLenum mode, pcl::PolygonMesh::Ptr plg) : mode_ (mode)
+{
+ bool found_rgb=false;
+ for (size_t i=0; i<plg->cloud.fields.size () ;i++)
+ if (plg->cloud.fields[i].name.compare ("rgb") == 0)
+ found_rgb = true;
+
+ if (found_rgb)
+ {
+ pcl::PointCloud<pcl::PointXYZRGB> newcloud;
+ pcl::fromPCLPointCloud2 (plg->cloud, newcloud);
+ Eigen::Vector4f tmp;
+ for(size_t i = 0; i< plg->polygons.size (); i++)
+ { // each triangle/polygon
+ pcl::Vertices apoly_in = plg->polygons[i];
+ SinglePoly apoly;
+ apoly.nvertices_ = apoly_in.vertices.size ();
+ apoly.vertices_ = new float[3*apoly_in.vertices.size ()];
+ apoly.colors_ = new float[4*apoly_in.vertices.size ()];
+
+ for(size_t j=0; j< apoly_in.vertices.size (); j++)
+ { // each point
+ uint32_t pt = apoly_in.vertices[j];
+ tmp = newcloud.points[pt].getVector4fMap ();
+ // x,y,z
+ apoly.vertices_[3*j + 0] = tmp (0);
+ apoly.vertices_[3*j + 1] = tmp (1);
+ apoly.vertices_[3*j + 2] = tmp (2);
+ // r,g,b: input is ints 0->255, opengl wants floats 0->1
+ apoly.colors_[4*j + 0] = newcloud.points[pt].r/255.0f; // Red
+ apoly.colors_[4*j + 1] = newcloud.points[pt].g/255.0f; // Green
+ apoly.colors_[4*j + 2] = newcloud.points[pt].b/255.0f; // Blue
+ apoly.colors_[4*j + 3] = 1.0f; // transparancy? unnecessary?
+ }
+ polygons.push_back (apoly);
+ }
+ }
+ else
+ {
+ pcl::PointCloud<pcl::PointXYZ> newcloud;
+ pcl::fromPCLPointCloud2 (plg->cloud, newcloud);
+ Eigen::Vector4f tmp;
+ for(size_t i=0; i< plg->polygons.size (); i++)
+ { // each triangle/polygon
+ pcl::Vertices apoly_in = plg->polygons[i];
+ SinglePoly apoly;
+ apoly.nvertices_ = apoly_in.vertices.size ();
+ apoly.vertices_ = new float [3*apoly_in.vertices.size ()];
+ apoly.colors_ = new float [4*apoly_in.vertices.size ()];
+
+ for(size_t j=0; j< apoly_in.vertices.size (); j++)
+ { // each point
+ uint32_t pt = apoly_in.vertices[j];
+ tmp = newcloud.points[pt].getVector4fMap ();
+ // x,y,z
+ apoly.vertices_[3*j + 0] = tmp (0);
+ apoly.vertices_[3*j + 1] = tmp (1);
+ apoly.vertices_[3*j + 2] = tmp (2);
+ // r,g,b: input is ints 0->255, opengl wants floats 0->1
+ apoly.colors_[4*j + 0] = 1.0f; // Red
+ apoly.colors_[4*j + 1] = 0.0f; // Green
+ apoly.colors_[4*j + 2] = 0.0f; // Blue
+ apoly.colors_[4*j + 3] = 1.0;
+ }
+ polygons.push_back (apoly);
+ }
+ }
+}
+
+pcl::simulation::PolygonMeshModel::~PolygonMeshModel ()
+{
+ // TODO: memory management!
+ for (size_t i = 0; i < polygons.size (); ++i)
+ {
+ delete [] polygons[i].vertices_;
+ delete [] polygons[i].colors_;
+ }
+}
+
+void
+pcl::simulation::PolygonMeshModel::draw ()
+{
+ // This might be a little quicker than drawing using individual polygons
+ // TODO: test by how much
+ glEnable (GL_DEPTH_TEST);
+ glEnableClientState (GL_VERTEX_ARRAY);
+ glEnableClientState (GL_COLOR_ARRAY);
+
+ for (size_t i = 0; i < polygons.size (); i++)
+ {
+ glVertexPointer (3, GL_FLOAT, 0, polygons[i].vertices_);
+ glColorPointer (4, GL_FLOAT, 0, polygons[i].colors_);
+ glDrawArrays (mode_, 0, polygons[i].nvertices_);
+ }
+ glDisableClientState (GL_COLOR_ARRAY);
+ glDisableClientState (GL_VERTEX_ARRAY);
+}
+
+pcl::simulation::PointCloudModel::PointCloudModel (GLenum mode, pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc) : mode_ (mode)
+{
+ nvertices_ = pc->points.size ();
+ vertices_ = new float[3*nvertices_];
+ colors_ = new float[4*nvertices_];
+
+ for (size_t i = 0; i < pc->points.size (); ++i)
+ {
+ vertices_[3*i + 0] = pc->points[i].x;
+ vertices_[3*i + 1] = pc->points[i].y;
+ vertices_[3*i + 2] = pc->points[i].z;
+
+ colors_[4*i + 0] = pc->points[i].r / 255.0f;
+ colors_[4*i + 1] = pc->points[i].g / 255.0f;
+ colors_[4*i + 2] = pc->points[i].b / 255.0f;
+ colors_[4*i + 3] = 1.0;
+ }
+}
+
+pcl::simulation::PointCloudModel::~PointCloudModel ()
+{
+ delete vertices_;
+ delete colors_;
+}
+
+void
+pcl::simulation::PointCloudModel::draw ()
+{
+ glEnable (GL_DEPTH_TEST);
+
+ glEnableClientState (GL_VERTEX_ARRAY);
+ glEnableClientState (GL_COLOR_ARRAY);
+
+ float att[3] = {0.0f, 0.25f, 0.0f};
+ glPointParameterf(GL_POINT_SIZE_MIN, 1.0f);
+ glPointParameterf(GL_POINT_SIZE_MAX, 500.0f);
+ glPointParameterfv(GL_POINT_DISTANCE_ATTENUATION, att);
+ glEnable(GL_POINT_SPRITE);
+
+ glVertexPointer (3, GL_FLOAT, 0, vertices_);
+ glColorPointer (4, GL_FLOAT, 0, colors_);
+
+ glDrawArrays (mode_, 0, nvertices_);
+
+ glDisableClientState (GL_COLOR_ARRAY);
+ glDisableClientState (GL_VERTEX_ARRAY);
+
+ glDisable(GL_POINT_SPRITE);
+}
+
+pcl::simulation::Quad::Quad ()
+{
+ // vertex pos: xyz , texture coord: uv
+ const static float vertices[20] = {-1.0, -1.0, 0.0, 0.0, 0.0,
+ -1.0, 1.0, 0.0, 0.0, 1.0,
+ 1.0, 1.0, 0.0, 1.0, 1.0,
+ 1.0, -1.0, 0.0, 1.0, 0.0 };
+
+ glGenBuffers (1, &quad_vbo_);
+ glBindBuffer (GL_ARRAY_BUFFER, quad_vbo_);
+ glBufferData (GL_ARRAY_BUFFER, sizeof (vertices), vertices, GL_STATIC_DRAW);
+ glBindBuffer (GL_ARRAY_BUFFER, 0);
+}
+
+pcl::simulation::Quad::~Quad ()
+{
+ glDeleteBuffers (1, &quad_vbo_);
+}
+
+void
+pcl::simulation::Quad::render ()
+{
+ glBindBuffer (GL_ARRAY_BUFFER, quad_vbo_);
+ glEnableVertexAttribArray (0);
+ glEnableVertexAttribArray (1);
+ glVertexAttribPointer (0, 3, GL_FLOAT, GL_FALSE, sizeof (float)*5, 0);
+ glVertexAttribPointer (1, 2, GL_FLOAT, GL_FALSE, sizeof (float)*5, (const GLvoid*)12);
+
+ glDrawArrays (GL_QUADS, 0, 4);
+
+ glDisableVertexAttribArray (1);
+ glDisableVertexAttribArray (0);
+ glBindBuffer (GL_ARRAY_BUFFER, 0);
+}
+
+pcl::simulation::TexturedQuad::TexturedQuad (int width, int height) : width_ (width), height_ (height)
+{
+ program_ = gllib::Program::loadProgramFromFile ("single_texture.vert", "single_texture.frag");
+ program_->use ();
+ Eigen::Matrix<float, 4, 4> MVP;
+ MVP.setIdentity();
+ program_->setUniform ("MVP", MVP);
+ glUseProgram (0);
+
+ glGenTextures (1, &texture_);
+ glBindTexture (GL_TEXTURE_2D, texture_);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
+ glTexImage2D (GL_TEXTURE_2D, 0, GL_RGB, width_, height_, 0, GL_RGB, GL_UNSIGNED_BYTE, NULL);
+ glBindTexture (GL_TEXTURE_2D, 0);
+}
+
+pcl::simulation::TexturedQuad::~TexturedQuad ()
+{
+ glDeleteTextures (1, &texture_);
+}
+
+void
+pcl::simulation::TexturedQuad::setTexture (const uint8_t* data)
+{
+ glBindTexture (GL_TEXTURE_2D, texture_);
+ glTexImage2D (GL_TEXTURE_2D, 0, GL_RGB, width_, height_, 0, GL_RGB, GL_UNSIGNED_BYTE, data);
+ glBindTexture (GL_TEXTURE_2D, 0);
+}
+
+void
+pcl::simulation::TexturedQuad::render ()
+{
+ program_->use ();
+ glActiveTexture (GL_TEXTURE0);
+ glBindTexture (GL_TEXTURE_2D, texture_);
+ quad_.render ();
+ glBindTexture (GL_TEXTURE_2D, 0);
+ glUseProgram (0);
+}
--- /dev/null
+#include <GL/glew.h>
+#include <time.h>
+
+#include <pcl/pcl_config.h>
+#ifdef OPENGL_IS_A_FRAMEWORK
+# include <OpenGL/gl.h>
+# include <OpenGL/glu.h>
+#else
+# include <GL/gl.h>
+# include <GL/glu.h>
+#endif
+
+#include <pcl/common/time.h>
+#include <pcl/simulation/range_likelihood.h>
+
+// For adding noise:
+static boost::minstd_rand generator (static_cast<unsigned int> (std::time (0))); // seed
+
+//#define SIMULATION_DEBUG 1
+#define DO_TIMING_PROFILE 0
+
+using namespace std;
+
+// 301 values, 0.0 uniform 1.0 normal. properly truncated/normalized
+float normal_sigma0x5_normal1x0_range0to3_step0x01[] = {1.59576912f, 1.59545000f, 1.59449302f, 1.59289932f, 1.59067083f,
+ 1.58781019f, 1.58432085f, 1.58020697f, 1.57547345f, 1.57012594f,
+ 1.56417078f, 1.55761504f, 1.55046646f, 1.54273348f, 1.53442517f,
+ 1.52555126f, 1.51612211f, 1.50614865f, 1.49564242f, 1.48461552f,
+ 1.47308056f, 1.46105069f, 1.44853953f, 1.43556117f, 1.42213012f,
+ 1.40826131f, 1.39397005f, 1.37927201f, 1.36418316f, 1.34871978f,
+ 1.33289841f, 1.31673585f, 1.30024906f, 1.28345522f, 1.26637163f,
+ 1.24901574f, 1.23140504f, 1.21355714f, 1.19548963f, 1.17722012f,
+ 1.15876621f, 1.14014544f, 1.12137525f, 1.10247299f, 1.08345589f,
+ 1.06434100f, 1.04514521f, 1.02588518f, 1.00657737f, 0.98723796f,
+ 0.96788290f, 0.94852781f, 0.92918802f, 0.90987853f, 0.89061400f,
+ 0.87140871f, 0.85227659f, 0.83323116f, 0.81428555f, 0.79545248f,
+ 0.77674422f, 0.75817263f, 0.73974913f, 0.72148465f, 0.70338972f,
+ 0.68547437f, 0.66774817f, 0.65022022f, 0.63289916f, 0.61579315f,
+ 0.59890986f, 0.58225652f, 0.56583986f, 0.54966616f, 0.53374122f,
+ 0.51807038f, 0.50265855f, 0.48751015f, 0.47262918f, 0.45801920f,
+ 0.44368334f, 0.42962430f, 0.41584438f, 0.40234547f, 0.38912908f,
+ 0.37619631f, 0.36354792f, 0.35118428f, 0.33910545f, 0.32731110f,
+ 0.31580063f, 0.30457310f, 0.29362725f, 0.28296157f, 0.27257426f,
+ 0.26246326f, 0.25262625f, 0.24306068f, 0.23376378f, 0.22473257f,
+ 0.21596387f, 0.20745431f, 0.19920035f, 0.19119830f, 0.18344431f,
+ 0.17593438f, 0.16866443f, 0.16163022f, 0.15482742f, 0.14825164f,
+ 0.14189837f, 0.13576305f, 0.12984106f, 0.12412773f, 0.11861834f,
+ 0.11330815f, 0.10819240f, 0.10326630f, 0.09852508f, 0.09396394f,
+ 0.08957812f, 0.08536286f, 0.08131342f, 0.07742511f, 0.07369324f,
+ 0.07011320f, 0.06668040f, 0.06339032f, 0.06023847f, 0.05722044f,
+ 0.05433188f, 0.05156850f, 0.04892611f, 0.04640054f, 0.04398775f,
+ 0.04168374f, 0.03948462f, 0.03738655f, 0.03538582f, 0.03347876f,
+ 0.03166181f, 0.02993149f, 0.02828442f, 0.02671730f, 0.02522691f,
+ 0.02381013f, 0.02246393f, 0.02118538f, 0.01997160f, 0.01881983f,
+ 0.01772739f, 0.01669169f, 0.01571021f, 0.01478053f, 0.01390031f,
+ 0.01306728f, 0.01227925f, 0.01153414f, 0.01082990f, 0.01016460f,
+ 0.00953635f, 0.00894336f, 0.00838388f, 0.00785626f, 0.00735890f,
+ 0.00689028f, 0.00644891f, 0.00603340f, 0.00564241f, 0.00527464f,
+ 0.00492888f, 0.00460393f, 0.00429869f, 0.00401209f, 0.00374309f,
+ 0.00349073f, 0.00325408f, 0.00303227f, 0.00282444f, 0.00262981f,
+ 0.00244761f, 0.00227712f, 0.00211766f, 0.00196858f, 0.00182926f,
+ 0.00169912f, 0.00157761f, 0.00146420f, 0.00135840f, 0.00125975f,
+ 0.00116779f, 0.00108211f, 0.00100231f, 0.00092803f, 0.00085891f,
+ 0.00079462f, 0.00073485f, 0.00067930f, 0.00062770f, 0.00057979f,
+ 0.00053532f, 0.00049406f, 0.00045581f, 0.00042034f, 0.00038748f,
+ 0.00035705f, 0.00032887f, 0.00030280f, 0.00027868f, 0.00025638f,
+ 0.00023577f, 0.00021673f, 0.00019915f, 0.00018292f, 0.00016795f,
+ 0.00015414f, 0.00014141f, 0.00012968f, 0.00011887f, 0.00010893f,
+ 0.00009977f, 0.00009135f, 0.00008360f, 0.00007648f, 0.00006994f,
+ 0.00006393f, 0.00005842f, 0.00005336f, 0.00004872f, 0.00004446f,
+ 0.00004056f, 0.00003699f, 0.00003372f, 0.00003072f, 0.00002798f,
+ 0.00002548f, 0.00002319f, 0.00002110f, 0.00001918f, 0.00001744f,
+ 0.00001585f, 0.00001439f, 0.00001307f, 0.00001186f, 0.00001076f,
+ 0.00000976f, 0.00000884f, 0.00000801f, 0.00000726f, 0.00000657f,
+ 0.00000595f, 0.00000538f, 0.00000486f, 0.00000440f, 0.00000397f,
+ 0.00000359f, 0.00000324f, 0.00000292f, 0.00000264f, 0.00000238f,
+ 0.00000214f, 0.00000193f, 0.00000174f, 0.00000157f, 0.00000141f,
+ 0.00000127f, 0.00000114f, 0.00000103f, 0.00000092f, 0.00000083f,
+ 0.00000074f, 0.00000067f, 0.00000060f, 0.00000054f, 0.00000048f,
+ 0.00000043f, 0.00000039f, 0.00000035f, 0.00000031f, 0.00000028f,
+ 0.00000025f, 0.00000022f, 0.00000020f, 0.00000018f, 0.00000016f,
+ 0.00000014f, 0.00000013f, 0.00000011f, 0.00000010f, 0.00000009f,
+ 0.00000008f, 0.00000007f, 0.00000006f, 0.00000006f, 0.00000005f,
+ 0.00000004f, 0.00000004f, 0.00000003f, 0.00000003f, 0.00000003f,
+ 0.00000002f};
+
+// Where the above if lhoodf, this a hard coded/optimized version:
+//ratio = 0.99; r_min =0; r_max = 3;
+//lhood = ratio/(r_max -r_min) + (1-ratio)*lhood ; hard_coded_log_lhood=log(lhood)
+float hard_coded_log_lhood[] = {-1.0614388f, -1.0614480f, -1.0614757f, -1.0615217f, -1.0615862f, -1.0616689f, -1.0617698f, -1.0618887f, -1.0620256f, -1.0621803f, -1.0623526f, -1.0625423f, -1.0627491f, -1.0629730f, -1.0632135f, -1.0634705f, -1.0637437f, -1.0640327f, -1.0643372f, -1.0646569f, -1.0649914f, -1.0653405f, -1.0657036f, -1.0660804f, -1.0664705f, -1.0668735f, -1.0672889f, -1.0677164f, -1.0681554f, -1.0686054f, -1.0690662f, -1.0695370f, -1.0700176f, -1.0705073f, -1.0710057f, -1.0715124f, -1.0720267f, -1.0725482f, -1.0730764f, -1.0736108f, -1.0741509f, -1.0746962f, -1.0752462f, -1.0758003f, -1.0763581f, -1.0769191f, -1.0774827f, -1.0780486f, -1.0786162f, -1.0791851f, -1.0797547f, -1.0803247f, -1.0808945f, -1.0814638f, -1.0820321f, -1.0825989f, -1.0831639f, -1.0837267f, -1.0842868f, -1.0848439f, -1.0853977f, -1.0859476f, -1.0864935f, -1.0870350f, -1.0875718f, -1.0881035f, -1.0886298f, -1.0891506f, -1.0896655f, -1.0901742f, -1.0906766f, -1.0911723f, -1.0916613f, -1.0921433f, -1.0926181f, -1.0930855f, -1.0935454f, -1.0939976f, -1.0944421f, -1.0948787f, -1.0953073f, -1.0957277f, -1.0961400f, -1.0965441f, -1.0969398f, -1.0973272f, -1.0977063f, -1.0980769f, -1.0984391f, -1.0987930f, -1.0991384f, -1.0994755f, -1.0998042f, -1.1001246f, -1.1004367f, -1.1007407f, -1.1010364f, -1.1013241f, -1.1016038f, -1.1018756f, -1.1021396f, -1.1023958f, -1.1026444f, -1.1028855f, -1.1031191f, -1.1033454f, -1.1035646f, -1.1037767f, -1.1039819f, -1.1041802f, -1.1043719f, -1.1045570f, -1.1047358f, -1.1049082f, -1.1050746f, -1.1052349f, -1.1053894f, -1.1055382f, -1.1056815f, -1.1058193f, -1.1059518f, -1.1060792f, -1.1062016f, -1.1063192f, -1.1064320f, -1.1065402f, -1.1066440f, -1.1067435f, -1.1068389f, -1.1069302f, -1.1070176f, -1.1071012f, -1.1071811f, -1.1072575f, -1.1073306f, -1.1074003f, -1.1074668f, -1.1075303f, -1.1075909f, -1.1076486f, -1.1077036f, -1.1077560f, -1.1078059f, -1.1078533f, -1.1078985f, -1.1079414f, -1.1079821f, -1.1080208f, -1.1080576f, -1.1080925f, -1.1081256f, -1.1081569f, -1.1081867f, -1.1082148f, -1.1082415f, -1.1082667f, -1.1082906f, -1.1083132f, -1.1083345f, -1.1083547f, -1.1083737f, -1.1083917f, -1.1084086f, -1.1084246f, -1.1084397f, -1.1084538f, -1.1084672f, -1.1084798f, -1.1084917f, -1.1085028f, -1.1085133f, -1.1085231f, -1.1085324f, -1.1085411f, -1.1085492f, -1.1085569f, -1.1085640f, -1.1085707f, -1.1085770f, -1.1085829f, -1.1085885f, -1.1085936f, -1.1085985f, -1.1086030f, -1.1086072f, -1.1086111f, -1.1086148f, -1.1086183f, -1.1086215f, -1.1086245f, -1.1086272f, -1.1086298f, -1.1086323f, -1.1086345f, -1.1086366f, -1.1086385f, -1.1086404f, -1.1086420f, -1.1086436f, -1.1086451f, -1.1086464f, -1.1086477f, -1.1086488f, -1.1086499f, -1.1086509f, -1.1086518f, -1.1086527f, -1.1086534f, -1.1086542f, -1.1086549f, -1.1086555f, -1.1086561f, -1.1086566f, -1.1086571f, -1.1086575f, -1.1086580f, -1.1086583f, -1.1086587f, -1.1086590f, -1.1086593f, -1.1086596f, -1.1086599f, -1.1086601f, -1.1086603f, -1.1086605f, -1.1086607f, -1.1086609f, -1.1086610f, -1.1086611f, -1.1086613f, -1.1086614f, -1.1086615f, -1.1086616f, -1.1086617f, -1.1086618f, -1.1086619f, -1.1086619f, -1.1086620f, -1.1086620f, -1.1086621f, -1.1086621f, -1.1086622f, -1.1086622f, -1.1086623f, -1.1086623f, -1.1086623f, -1.1086624f, -1.1086624f, -1.1086624f, -1.1086624f, -1.1086624f, -1.1086625f, -1.1086625f, -1.1086625f, -1.1086625f, -1.1086625f, -1.1086625f, -1.1086625f, -1.1086625f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f};
+
+// Disparity:
+// sigma 0.025
+double top_lookup[]={15.9577, 15.8165, 15.4003, 14.7308, 13.8422, 12.7779, 11.5877, 10.3231, 9.0345, 7.7674, 6.5604, 5.4433, 4.4368, 3.5527, 2.7947, 2.1596, 1.6395, 1.2227, 0.89578, 0.64471, 0.45584, 0.31662, 0.21604, 0.14482, 0.095364, 0.061691, 0.039205, 0.024476, 0.015011, 0.0090444, 0.0053532, 0.0031126, 0.001778, 0.0009977, 0.00054999, 0.00029784, 0.00015845, 8.2811e-05, 4.2517e-05, 2.1444e-05, 1.0625e-05, 5.1718e-06, 2.473e-06, 1.1617e-06, 5.361e-07, 2.4304e-07, 1.0824e-07, 4.7354e-08, 2.0353e-08, 8.5933e-09, 3.5644e-09, 1.4524e-09, 5.8138e-10, 2.2862e-10, 8.832e-11, 3.3518e-11, 1.2496e-11, 4.5766e-12, 1.6466e-12, 5.8201e-13, 2.0209e-13, 6.8935e-14, 2.31e-14, 7.6043e-15, 2.4592e-15, 7.8127e-16, 2.4383e-16, 7.4758e-17, 2.2517e-17, 6.6624e-18, 1.9366e-18, 5.5299e-19, 1.5512e-19, 4.2749e-20, 1.1573e-20, 3.0778e-21, 8.0413e-22, 2.0639e-22, 5.2038e-23, 1.289e-23, 3.1365e-24, 7.4975e-25, 1.7606e-25, 4.0617e-26, 9.2049e-27, 2.0493e-27, 4.4821e-28, 9.6302e-29, 2.0327e-29, 4.2148e-30, 8.5855e-31, 1.718e-31, 3.3774e-32, 6.5224e-33, 1.2374e-33, 2.3062e-34, 4.2225e-35, 7.5947e-36, 1.3419e-36, 2.3294e-37, 3.9721e-38, 6.6539e-39, 1.095e-39, 1.7703e-40, 2.8115e-41, 4.3864e-42, 6.7231e-43, 1.0123e-43, 1.4973e-44, 2.1758e-45, 3.1059e-46, 4.3555e-47, 6.0003e-48, 8.1205e-49, 1.0796e-49, 1.4101e-50, 1.8092e-51, 2.2804e-52, 2.8237e-53, 3.4349e-54, 4.1047e-55, 4.8186e-56, 5.5571e-57, 6.2958e-58, 7.007e-59, 7.6611e-60, 8.2287e-61, 8.6827e-62, 9.0002e-63, 9.165e-64, 9.1683e-65, 9.01e-66, 8.6984e-67, 8.2497e-68, 7.6862e-69, 7.035e-70, 6.3255e-71, 5.5874e-72, 4.8484e-73, 4.133e-74, 3.4611e-75, 2.8474e-76, 2.3012e-77, 1.827e-78, 1.425e-79, 1.0918e-80, 8.2183e-82, 6.077e-83, 4.4144e-84, 3.1502e-85, 2.2084e-86, 1.5209e-87, 1.029e-88, 6.8387e-90, 4.4651e-91, 2.864e-92, 1.8046e-93, 1.1171e-94, 6.793e-96, 4.058e-97, 2.3815e-98, 1.373e-99, 7.7759e-101, 4.3264e-102, 2.3647e-103, 1.2697e-104, 6.6975e-106, 3.4706e-107, 1.7667e-108, 8.8352e-110, 4.3405e-111, 2.0948e-112, 9.9319e-114, 4.6259e-115, 2.1166e-116, 9.514e-118, 4.2011e-119, 1.8224e-120, 7.7661e-122, 3.2512e-123, 1.3371e-124, 5.402e-126, 2.144e-127, 8.3597e-129, 3.202e-130, 1.2049e-131, 4.4538e-133, 1.6173e-134, 5.7697e-136, 2.022e-137, 6.9614e-139, 2.3544e-140, 7.8227e-142, 2.5533e-143, 8.1871e-145, 2.5789e-146, 7.9803e-148, 2.426e-149, 7.2448e-151, 2.1255e-152, 6.1257e-154, 1.7343e-155, 4.8239e-157, 1.3181e-158, 3.538e-160, 9.3294e-162, 2.4167e-163, 6.1502e-165, 1.5375e-166, 3.7761e-168, 9.1103e-170, 2.1593e-171, 5.0276e-173, 1.15e-174, 2.5841e-176, 5.7042e-178, 1.237e-179, 2.6352e-181, 5.5149e-183, 1.1338e-184, 2.29e-186, 4.5436e-188, 8.8561e-190, 1.6958e-191, 3.1899e-193, 5.8946e-195, 1.0701e-196, 1.9083e-198, 3.3433e-200, 5.7541e-202, 9.7287e-204, 1.6159e-205, 2.6366e-207, 4.2263e-209, 6.6552e-211, 1.0295e-212, 1.5645e-214, 2.3357e-216, 3.4256e-218, 4.9354e-220, 6.9855e-222, 9.7128e-224, 1.3267e-225, 1.7803e-227, 2.3468e-229, 3.039e-231, 3.8662e-233, 4.8318e-235, 5.9321e-237, 7.1548e-239, 8.4773e-241, 9.8673e-243, 1.1283e-244, 1.2674e-246, 1.3986e-248, 1.5162e-250, 1.6147e-252, 1.6893e-254, 1.7362e-256, 1.753e-258, 1.7388e-260, 1.6942e-262, 1.6218e-264, 1.525e-266, 1.4088e-268, 1.2785e-270, 1.1398e-272, 9.9826e-275, 8.5888e-277, 7.2594e-279, 6.0276e-281, 4.9167e-283, 3.9398e-285, 3.1014e-287, 2.3984e-289, 1.8221e-291, 1.3598e-293, 9.9699e-296, 7.1808e-298, 5.0808e-300, 3.5316e-302, 2.4115e-304, 1.6177e-306, 1.066e-308, 6.9011e-311, 4.3889e-313, 2.742e-315, 1.6829e-317, 1.0147e-319, 6.324e-322, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+float bottom_lookup[]={0.5f, 0.55304f, 0.60514f, 0.65542f, 0.7031f, 0.74751f, 0.78814f, 0.82468f, 0.85694f, 0.88493f, 0.90879f, 0.92877f, 0.9452f, 0.95848f, 0.96903f, 0.97725f, 0.98355f, 0.98829f, 0.9918f, 0.99435f, 0.99617f, 0.99744f, 0.99832f, 0.99892f, 0.99931f, 0.99957f, 0.99974f, 0.99984f, 0.99991f, 0.99994f, 0.99997f, 0.99998f, 0.99999f, 0.99999f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.99999f, 0.99999f, 0.99998f, 0.99997f, 0.99994f, 0.99991f, 0.99984f, 0.99974f, 0.99957f, 0.99931f, 0.99892f, 0.99832f, 0.99744f, 0.99617f, 0.99435f, 0.9918f, 0.98829f, 0.98355f, 0.97725f, 0.96903f, 0.95848f, 0.9452f, 0.92877f, 0.90879f, 0.88493f, 0.85694f, 0.82468f, 0.78814f, 0.74751f, 0.7031f, 0.65542f, 0.60514f, 0.55304f, 0.5f};
+
+using namespace pcl::simulation;
+
+// Finds the maximum level n so a and b are still
+// divisible by 2^n
+int
+max_level (int a, int b)
+{
+ int level = 0;
+ while (true)
+ {
+ if (a%2 || b%2) return level;
+ a = a / 2;
+ b = b / 2;
+ level++;
+ }
+}
+
+// display_tic_toc: a helper function which accepts a set of
+// timestamps and displays the elapsed time between them as
+// a fraction and time used [for profiling]
+void
+display_tic_toc (vector<double> &tic_toc,const string &fun_name)
+{
+ size_t tic_toc_size = tic_toc.size ();
+
+ double percent_tic_toc_last = 0;
+ double dtime = tic_toc[tic_toc_size-1] - tic_toc[0];
+ cout << "fraction_" << fun_name << ",";
+ for (size_t i = 0; i < tic_toc_size; i++)
+ {
+ double percent_tic_toc = (tic_toc[i] - tic_toc[0])/(tic_toc[tic_toc_size-1] - tic_toc[0]);
+ cout << percent_tic_toc - percent_tic_toc_last << ", ";
+ percent_tic_toc_last = percent_tic_toc;
+ }
+ cout << "\ntime_" << fun_name << ",";
+ double time_tic_toc_last = 0;
+ for (size_t i = 0; i < tic_toc_size; i++)
+ {
+ double percent_tic_toc = (tic_toc[i] - tic_toc[0])/(tic_toc[tic_toc_size-1] - tic_toc[0]);
+ cout << percent_tic_toc*dtime - time_tic_toc_last << ", ";
+ time_tic_toc_last = percent_tic_toc*dtime;
+ }
+ cout << "\ntotal_time_" << fun_name << " " << dtime << "\n";
+}
+
+pcl::simulation::RangeLikelihood::RangeLikelihood (int rows, int cols, int row_height, int col_width, Scene::Ptr scene) :
+ scene_(scene), rows_(rows), cols_(cols), row_height_(row_height), col_width_(col_width),
+ depth_buffer_dirty_(true),
+ color_buffer_dirty_(true),
+ score_buffer_dirty_(true),
+ fbo_ (0),
+ depth_render_buffer_ (0),
+ color_render_buffer_ (0),
+ depth_texture_ (0),
+ score_texture_ (0),
+ score_summarized_texture_ (0),
+ sensor_texture_ (0),
+ likelihood_texture_ (0),
+ compute_likelihood_on_cpu_ (false),
+ aggregate_on_cpu_ (false),
+ use_instancing_ (false),
+ use_color_ (true),
+ sum_reduce_ (cols * col_width, rows * row_height, max_level (col_width, row_height))
+{
+ height_ = rows_ * row_height;
+ width_ = cols_ * col_width;
+
+ depth_buffer_ = new float[width_*height_];
+ color_buffer_ = new uint8_t[width_*height_*3];
+
+ // Set Default Camera Intrinstic Parameters. techquad
+ // Correspond closely to those stated here:
+ // http://www.ros.org/wiki/kinect_calibration/technical
+ camera_width_ = 640;
+ camera_height_ = 480;
+ camera_fx_ = 576.09757860f;
+ camera_fy_ = 576.09757860f;
+ camera_cx_ = 321.06398107f;
+ camera_cy_ = 242.97676897f;
+
+ z_near_ = 0.7f;
+ z_far_ = 20.0f;
+
+ which_cost_function_ = 2; // default to commonly used meter based function
+
+ // default lhood parameters - these should always be set by the user
+ // so might want to add to constructor eventually:
+ sigma_ = 0.1;
+ floor_proportion_ = 0.9;
+
+ int height = rows * row_height;
+ int width = cols * col_width;
+
+ // For now we only support a limited size texture
+ assert (height >0 && height <= 8192 && width > 0 && width <= 8192);
+ // throw std::runtime_error "
+
+ // Allocate framebuffer
+ glGenRenderbuffers (1, &depth_render_buffer_);
+ glBindRenderbuffer (GL_RENDERBUFFER, depth_render_buffer_);
+ glRenderbufferStorage (GL_RENDERBUFFER, GL_DEPTH_COMPONENT32, width, height);
+ glBindRenderbuffer (GL_RENDERBUFFER, 0);
+
+ glGenRenderbuffers (1, &color_render_buffer_);
+ glBindRenderbuffer (GL_RENDERBUFFER, color_render_buffer_);
+ glRenderbufferStorage (GL_RENDERBUFFER, GL_RGB8UI, width, height);
+ glBindRenderbuffer (GL_RENDERBUFFER, 0);
+
+ // Setup texture to store depth image
+ glGenTextures (1, &depth_texture_);
+ glBindTexture (GL_TEXTURE_2D, depth_texture_);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
+ glTexImage2D (GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT32, width, height, 0, GL_DEPTH_COMPONENT, GL_FLOAT, NULL);
+ glBindTexture (GL_TEXTURE_2D, 0);
+
+ glGenTextures (1, &color_texture_);
+ glBindTexture (GL_TEXTURE_2D, color_texture_);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
+ glTexImage2D (GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, NULL);
+ glBindTexture (GL_TEXTURE_2D, 0);
+
+ // Setup texture for incoming image
+ glGenTextures (1, &sensor_texture_);
+ glBindTexture (GL_TEXTURE_2D, sensor_texture_);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
+ glTexImage2D (GL_TEXTURE_2D, 0, GL_R32F, col_width, row_height, 0, GL_RED, GL_FLOAT, NULL);
+ glBindTexture (GL_TEXTURE_2D, 0);
+
+ // Texture for to score on each pixel
+ glGenTextures (1, &score_texture_);
+ glBindTexture (GL_TEXTURE_2D, score_texture_);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
+ glTexImage2D (GL_TEXTURE_2D, 0, GL_R32F, width_, height_, 0, GL_RED, GL_FLOAT, NULL);
+ glBindTexture (GL_TEXTURE_2D, 0);
+
+ // Setup texture for likelihood function
+ // size of likelihood texture
+ int likelihood_size = 301;
+ glActiveTexture (GL_TEXTURE2);
+ glGenTextures (1, &likelihood_texture_);
+ glBindTexture (GL_TEXTURE_2D, likelihood_texture_);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_R, GL_CLAMP_TO_EDGE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_R, GL_RED);
+ glTexImage2D (GL_TEXTURE_2D, 0, GL_R32F, likelihood_size, 1, 0, GL_RED, GL_FLOAT, normal_sigma0x5_normal1x0_range0to3_step0x01);
+ glBindTexture (GL_TEXTURE_2D, 0);
+
+ // Setup the framebuffer object for rendering
+ glGenFramebuffers (1, &fbo_);
+ glBindFramebuffer (GL_FRAMEBUFFER, fbo_);
+ glFramebufferTexture2D (GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, depth_texture_, 0);
+ glFramebufferTexture2D (GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, color_texture_, 0);
+ glBindFramebuffer (GL_FRAMEBUFFER, 0);
+
+ if (gllib::getGLError() != GL_NO_ERROR)
+ {
+ std::cerr << "RangeLikelihoodGLSL::RangeLikelihoodGLSL: Failed initializing OpenGL buffers" << std::endl;
+ exit(-1);
+ }
+
+ glGenFramebuffers (1, &score_fbo_);
+ glBindFramebuffer (GL_FRAMEBUFFER, score_fbo_);
+ glFramebufferRenderbuffer (GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, 0);
+ glFramebufferTexture2D (GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, score_texture_, 0);
+ glBindFramebuffer (GL_FRAMEBUFFER, 0);
+
+ // Load shader
+ likelihood_program_ = gllib::Program::Ptr (new gllib::Program ());
+ // TODO: to remove file dependency include the shader source in the binary
+ if (!likelihood_program_->addShaderFile ("compute_score.vert", gllib::VERTEX))
+ {
+ std::cout << "Failed loading vertex shader" << std::endl;
+ exit (-1);
+ }
+
+ if (!likelihood_program_->addShaderFile ("compute_score.frag", gllib::FRAGMENT))
+ {
+ std::cout << "Failed loading fragment shader" << std::endl;
+ exit (-1);
+ }
+
+ likelihood_program_->link ();
+
+ vertices_.push_back (Eigen::Vector3f (-1.0, 1.0, 0.0));
+ vertices_.push_back (Eigen::Vector3f ( 1.0, 1.0, 0.0));
+ vertices_.push_back (Eigen::Vector3f ( 1.0, -1.0, 0.0));
+ vertices_.push_back (Eigen::Vector3f (-1.0, -1.0, 0.0));
+
+ glGenBuffers (1, &quad_vbo_);
+ glBindBuffer (GL_ARRAY_BUFFER, quad_vbo_);
+ glBufferData (GL_ARRAY_BUFFER, sizeof (Eigen::Vector3f) * vertices_.size (), &(vertices_[0]), GL_STATIC_DRAW);
+ glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+ gllib::getGLError ();
+
+ // Go back to the default pipeline
+ glUseProgram (0);
+
+ score_buffer_ = new float[width_*height_];
+
+}
+
+pcl::simulation::RangeLikelihood::~RangeLikelihood ()
+{
+ glDeleteBuffers (1, &quad_vbo_);
+ glDeleteTextures (1, &depth_texture_);
+ glDeleteTextures (1, &color_texture_);
+ glDeleteTextures (1, &score_texture_);
+ glDeleteTextures (1, &score_summarized_texture_);
+ glDeleteTextures (1, &sensor_texture_);
+ glDeleteTextures (1, &likelihood_texture_);
+ glDeleteFramebuffers (1, &fbo_);
+ glDeleteFramebuffers (1, &score_fbo_);
+ glDeleteRenderbuffers (1, &depth_render_buffer_);
+ glDeleteRenderbuffers (1, &color_render_buffer_);
+
+ delete [] depth_buffer_;
+ delete [] color_buffer_;
+ delete [] score_buffer_;
+}
+
+double
+pcl::simulation::RangeLikelihood::sampleNormal (double sigma)
+{
+ typedef boost::normal_distribution<double> Normal;
+ Normal dist (0.0, sigma);
+ boost::variate_generator<boost::minstd_rand&, Normal> norm (generator, dist);
+ return (norm ());
+}
+
+void
+pcl::simulation::RangeLikelihood::setupProjectionMatrix ()
+{
+ glMatrixMode (GL_PROJECTION);
+ glLoadIdentity ();
+
+ // Prepare scaled simulated camera projection matrix
+ float sx = static_cast<float> (camera_width_) / static_cast<float> (col_width_);
+ float sy = static_cast<float> (camera_height_) / static_cast<float> (row_height_);
+ float width = static_cast<float> (col_width_);
+ float height = static_cast<float> (row_height_);
+
+ float fx = camera_fx_/sx;
+ float fy = camera_fy_/sy;
+ float cx = camera_cx_/sx;
+ float cy = camera_cy_/sy;
+ float m[16];
+ float z_nf = (z_near_-z_far_);
+
+ m[0] = 2.0f*fx/width; m[4] = 0; m[ 8] = 1.0f-(2*cx/width); m[12] = 0;
+ m[1] = 0; m[5] = 2.0f*fy/height; m[ 9] = 1.0f-(2*cy/height); m[13] = 0;
+ m[2] = 0; m[6] = 0; m[10] = (z_far_+z_near_)/z_nf; m[14] = 2.0f*z_near_*z_far_/z_nf;
+ m[3] = 0; m[7] = 0; m[11] = -1.0f; m[15] = 0;
+ glMultMatrixf (m);
+}
+
+void
+pcl::simulation::RangeLikelihood::applyCameraTransform (const Eigen::Isometry3d & pose)
+{
+ float T[16];
+ Eigen::Matrix4f m = (pose.matrix ().inverse ()).cast<float> ();
+ T[0] = m(0,0); T[4] = m(0,1); T[8] = m(0,2); T[12] = m(0,3);
+ T[1] = m(1,0); T[5] = m(1,1); T[9] = m(1,2); T[13] = m(1,3);
+ T[2] = m(2,0); T[6] = m(2,1); T[10] = m(2,2); T[14] = m(2,3);
+ T[3] = m(3,0); T[7] = m(3,1); T[11] = m(3,2); T[15] = m(3,3);
+ glMultMatrixf(T);
+}
+
+void
+pcl::simulation::RangeLikelihood::drawParticles (std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses)
+{
+ int n = 0;
+ for (int i=0; i<rows_; ++i)
+ {
+ for (int j=0; j<cols_; ++j)
+ {
+ glMatrixMode (GL_MODELVIEW);
+ glLoadIdentity ();
+
+ glViewport(j*col_width_, i*row_height_, col_width_, row_height_);
+
+ // Go from Z-up, X-forward coordinate frame
+ // to OpenGL Z-out,Y-up [both Right Handed]
+ float T[16];
+ T[0] = 0; T[4] = -1.0; T[8] = 0; T[12] = 0;
+ T[1] = 0; T[5] = 0; T[9] = 1; T[13] = 0;
+ T[2] = -1.0; T[6] = 0; T[10] = 0; T[14] = 0;
+ T[3] = 0; T[7] = 0; T[11] = 0; T[15] = 1;
+ glMultMatrixf (T);
+
+ // Apply camera transformation
+ applyCameraTransform (poses[n++]);
+
+ // Draw the planes in each location:
+ scene_->draw ();
+ }
+ }
+}
+
+
+/////////////////////////////////////////////////////////////////
+// Below are 4 previously used cost functions:
+// 0 original scoring method
+float
+costFunction0 (float ref_val,float depth_val)
+{
+ return (sqr(ref_val - depth_val));
+}
+
+// 1st working cost function:
+// Empirical reverse mapping between depthbuffer and true depth:
+// Version 0: [25 aug 2011]
+// TRUEDEPTH = 1/(1.33 -(DEPTHBUFFER)*1.29)
+//float cost = sqr(ref[col%col_width] - 1/(1.33 -(*depth)*1.29));
+// Version 1: [29 aug 2011] Exact version using correct mappings:
+float
+costFunction1 (float ref_val, float depth_val)
+{
+ float cost = sqr (ref_val - 1/(1.4285f - (depth_val)*1.3788f));
+ //std::cout << " [" << ref_val << "," << 1/(1.4285 -(depth_val)*1.3788) << "] ";
+ if (ref_val < 0)
+ { // all images pixels with no range
+ cost =1;
+ }
+ if (cost > 10)
+ { // required to lessen the effect of modelpixel with no range (ie holes in the model)
+ cost = 10;
+ }
+ //return log (cost);
+ return (cost);
+}
+
+// 1st working likelihood function (by far most commonly used)
+float
+costFunction2 (float ref_val, float depth_val)
+{
+ float min_dist = abs(ref_val - 1/(1.4285f - (depth_val)*1.3788f));
+ int lup = static_cast<int> (ceil (min_dist*100)); // has resolution of 0.01m
+
+ if (lup > 300)
+ { // implicitly this caps the cost if there is a hole in the model
+ lup = 300;
+ }
+
+ double lhood = 1;
+ if (pcl_isnan (depth_val))
+ { // pixels with nan depth - for openNI null points
+ lhood = 1; // log(1) = 0 ---> has no effect
+ }
+ else if(ref_val < 0)
+ { // all RGB pixels with no depth - for freenect null points
+ lhood = 1; // log(1) = 0 ---> has no effect
+ }
+ else
+ {
+ lhood = normal_sigma0x5_normal1x0_range0to3_step0x01[lup];
+ // add a ground floor:
+ // increasing this will mean that the likelihood is less peaked
+ // but you need more particles to do this...
+ // with ~90particles user 0.999, for example in the quad dataset
+ // ratio of uniform to normal
+ double ratio = 0.99;//was always 0.99;
+ double r_min = 0; // metres
+ double r_max = 3; // metres
+ lhood = ratio/(r_max -r_min) + (1-ratio)*lhood ;
+ }
+ return static_cast<float> (log (lhood));
+}
+
+float
+costFunction3 (float ref_val,float depth_val)
+{
+ float log_lhood=0;
+ // log(1) = 0 ---> has no effect
+ if (ref_val < 0)
+ {
+ // all images pixels with no range
+ }
+ else if (ref_val > 7)
+ {
+ // ignore long ranges... for now
+ }
+ else
+ { // working range
+ float min_dist = abs (ref_val - 0.7253f/(1.0360f - (depth_val)));
+
+ int lup = static_cast<int> (ceil (min_dist*100)); // has resulution of 0.01m
+ if (lup > 300)
+ { // implicitly this caps the cost if there is a hole in the model
+ lup = 300;
+ }
+ log_lhood = hard_coded_log_lhood[lup];
+ }
+ return log_lhood;
+}
+
+float
+costFunction4(float ref_val,float depth_val)
+{
+ float disparity_diff = abs( ( -0.7253f/ref_val +1.0360f ) - depth_val );
+
+ int top_lup = static_cast<int> (ceil (disparity_diff*300)); // has resulution of 0.001m
+ if (top_lup > 300)
+ {
+ top_lup =300;
+ }
+ float top = static_cast<float> (top_lookup[top_lup]);// round( abs(x-mu) *1000+1) );
+
+ // bottom:
+ //bottom = bottom_lookup( round(mu*1000+1));
+ int bottom_lup = static_cast<int> (ceil( (depth_val) * 300)); // has resulution of 0.001m
+ if (bottom_lup > 300)
+ {
+ bottom_lup =300;
+ }
+ float bottom = bottom_lookup[bottom_lup];// round( abs(x-mu) *1000+1) );
+
+ float proportion = 0.999f;
+ float lhood = proportion + (1-proportion)*(top/bottom);
+
+ // safety fix thats seems to be required due to opengl ayschronizate
+ // ask hordur about this
+ if (bottom == 0)
+ {
+ lhood = proportion;
+ }
+
+ if (ref_val< 0)
+ { // all images pixels with no range
+ lhood = 1; // log(1) = 0 ---> has no effect
+ }
+ return log(lhood);
+}
+
+// TODO: WHEN WE'RE HAPPY THIS SHOULD BE "THE" LIKELIHOOD FUNCTION
+// add these global variables into the class
+// abd use sigma and floor_proportion directly from class also
+using boost::math::normal; // typedef provides default type is double.
+normal unit_norm_dist(0,1); // (default mean = zero, and standard deviation = unity)
+double costFunction5(double measured_depth, double model_disp, double sigma, double floor_proportion)
+{
+ // NEED TO CONVERT MEASURED TO DISPARITY
+ double measured_disp = (-0.7253/measured_depth + 1.0360 );
+
+ // measured_depth = ref_val [m]
+ // model_disp = depth_val [0-1]
+ // upper and lower bound on depth buffer:
+ double lower_bound =0;
+ double upper_bound =1;
+
+ double gaussian_part = pdf(unit_norm_dist, (measured_disp-model_disp)/sigma)/sigma;
+ double truncation = 1/cdf(unit_norm_dist,(upper_bound-model_disp)/sigma) - cdf(unit_norm_dist, (lower_bound-model_disp)/sigma);
+
+ double trunc_gaussian_part = truncation*gaussian_part;
+
+ double lhood= (floor_proportion/(upper_bound-lower_bound) + (1-floor_proportion)*trunc_gaussian_part);
+
+ if (measured_depth< 0){ // all images pixels with no range
+ lhood = 1; // log(1) = 0 ---> has no effect
+ }
+
+ return log (lhood);
+}
+
+void
+pcl::simulation::RangeLikelihood::computeScores (float* reference,
+ std::vector<float> & scores)
+{
+ const float* depth = getDepthBuffer();
+ // Mapping between disparity and range:
+ // range or depth = 1/disparity
+ //
+ // the freenect produces a disparity <here we call this depth_buffer>
+ // that is mapped between 0->1 to minimize quantization
+ // near_range = n = 0.7m | far_range = f = 20m
+ // disparity can be found as a linear function of the depth_buffer (d = [0,1] )
+ // disparity = 1/n - (f-n)*d / (n*f)
+ // Below We compare range-versus-range using this mapping
+ //
+ // TODO: remove usage of 'depth' and 'depth_buffer_' as variable names as it implies
+ // that that is was held by these variables
+
+ // ref[col%col_width] - z/depth value in metres, 0-> ~20
+ // depth_val - contents of depth buffer [0->1]
+
+ // for row across each image in a row of model images
+ for (int row = 0; row < rows_*row_height_; row++)
+ {
+ float* ref = reference + col_width_*(row % row_height_);
+ // for each column: across each image in a column of model images
+ for (int col = 0; col < cols_*col_width_; col++)
+ {
+ float depth_val = (*depth++); // added jan 2012 - check this is not a breaking fix later mfallon
+ float score = 0;
+ if (which_cost_function_ == 0)
+ {
+ score = costFunction0 (ref[col%col_width_],depth_val);
+ }
+ else if (which_cost_function_ == 1)
+ {
+ score = costFunction1 (ref[col%col_width_],depth_val);
+ }
+ else if (which_cost_function_ == 2)
+ {
+ score = costFunction2 (ref[col%col_width_],depth_val);
+ }
+ else if(which_cost_function_==3)
+ {
+ score = costFunction3 (ref[col%col_width_],depth_val);
+ }
+ else if (which_cost_function_ == 4)
+ {
+ score = costFunction4 (ref[col%col_width_],depth_val);
+ }
+ else if (which_cost_function_ == 5)
+ {
+ //double sigma = 0.025;
+ //double floor_proportion_ = 0.999;
+ score = static_cast<float> (costFunction5 (ref[col%col_width_],depth_val,sigma_,floor_proportion_));
+ }
+ scores[row/row_height_ * cols_ + col/col_width_] += score;
+ //std::cout << "(" << scores[row/row_height_ * cols_ + col/col_width_] <<"," << score << "," << ref[col%col_width_] << "," << depth_val << ") ";
+ score_buffer_[row*width_ + col] = score;
+ }
+ }
+ score_buffer_dirty_ = false;
+}
+
+void
+pcl::simulation::RangeLikelihood::getPointCloud (pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc,
+ bool make_global,
+ const Eigen::Isometry3d & pose)
+{
+ // TODO: check if this works for for rows/cols >1 and for width&height != 640x480
+ // i.e. multiple tiled images
+ pc->width = col_width_;
+ pc->height = row_height_;
+ // Was:
+ //pc->width = camera_width_;
+ //pc->height = camera_height_;
+
+ pc->is_dense = false;
+ pc->points.resize (pc->width*pc->height);
+
+ int points_added = 0;
+
+ float camera_fx_reciprocal_ = 1.0f / camera_fx_;
+ float camera_fy_reciprocal_ = 1.0f / camera_fy_;
+ float zn = z_near_;
+ float zf = z_far_;
+
+ const uint8_t* color_buffer = getColorBuffer();
+
+ // TODO: support decimation
+ // Copied the format of RangeImagePlanar::setDepthImage()
+ // Use this as a template for decimation
+ for (int y = 0; y < row_height_ ; ++y) //camera_height_
+ {
+ for (int x = 0; x < col_width_ ; ++x) // camera_width_
+ {
+ // Find XYZ from normalized 0->1 mapped disparity
+ int idx = points_added; // y*camera_width_ + x;
+ float d = depth_buffer_[y*camera_width_ + x] ;
+ if (d < 1.0) // only add points with depth buffer less than max (20m) range
+ {
+ float z = zf*zn/((zf-zn)*(d - zf/(zf-zn)));
+
+ // TODO: add mode to ignore points with no return i.e. depth_buffer_ ==1
+ // NB: OpenGL uses a Right Hand system with +X right, +Y up, +Z back out of the screen,
+ // The Z-buffer is natively -1 (far) to 1 (near)
+ // But in this class we invert this to be 0 (near, 0.7m) and 1 (far, 20m)
+ // ... so by negating y we get to a right-hand computer vision system
+ // which is also used by PCL and OpenNi
+ pc->points[idx].z = z;
+ pc->points[idx].x = (static_cast<float> (x)-camera_cx_) * z * (-camera_fx_reciprocal_);
+ pc->points[idx].y = (static_cast<float> (y)-camera_cy_) * z * (-camera_fy_reciprocal_);
+
+ int rgb_idx = y*col_width_ + x; //camera_width_
+ pc->points[idx].b = color_buffer[rgb_idx*3+2]; // blue
+ pc->points[idx].g = color_buffer[rgb_idx*3+1]; // green
+ pc->points[idx].r = color_buffer[rgb_idx*3]; // red
+ points_added++;
+ }
+ }
+ }
+ pc->width = 1;
+ pc->height = points_added;
+ pc->points.resize (points_added);
+
+ if (make_global)
+ {
+ // Go from OpenGL to (Z-up, X-forward, Y-left)
+ Eigen::Matrix4f T;
+ T << 0, 0, -1, 0,
+ -1, 0, 0, 0,
+ 0, 1, 0, 0,
+ 0, 0, 0, 1;
+ Eigen::Matrix4f m = pose.matrix ().cast<float> ();
+ m = m * T;
+ pcl::transformPointCloud (*pc, *pc, m);
+ }
+ else
+ {
+ // Go from OpenGL to Camera (Z-forward, X-right, Y-down)
+ Eigen::Matrix4f T;
+ T << 1, 0, 0, 0,
+ 0, -1, 0, 0,
+ 0, 0, -1, 0,
+ 0, 0, 0, 1;
+ pcl::transformPointCloud (*pc, *pc, T);
+
+ // Go from Camera to body (Z-up, X-forward, Y-left)
+ Eigen::Matrix4f cam_to_body;
+ cam_to_body << 0, 0, 1, 0,
+ -1, 0, 0, 0,
+ 0, -1, 0, 0,
+ 0, 0, 0, 1;
+ Eigen::Matrix4f camera = pose.matrix ().cast<float> ();
+ camera = camera * cam_to_body;
+ pc->sensor_origin_ = camera.rightCols (1);
+ Eigen::Quaternion<float> quat (camera.block<3,3> (0,0));
+ pc->sensor_orientation_ = quat;
+ }
+}
+
+void
+pcl::simulation::RangeLikelihood::getRangeImagePlanar(pcl::RangeImagePlanar &rip)
+{
+ rip.setDepthImage (depth_buffer_,
+ camera_width_,camera_height_, camera_fx_,camera_fy_,
+ camera_fx_, camera_fy_);
+}
+
+void
+pcl::simulation::RangeLikelihood::addNoise ()
+{
+ // Other noises:
+ // edge noise: look for edges in the range image and add a few pixels here and there
+ // texture noise: look at the normals and
+
+ // Add Gaussian Noise
+ // TODO: make the variance a parameter
+ // TODO: might want to add a range-based variance
+ float variance = 0.0015f;
+ for (int i = 0; i < camera_width_*camera_height_ ; i++)
+ {
+ if (depth_buffer_[i] < 1)
+ {
+ depth_buffer_[i] = depth_buffer_[i] + variance * static_cast<float> (sampleNormal ());
+ if (depth_buffer_[i] > 1)
+ {
+ depth_buffer_[i] = 1.0;
+ }
+ else if (depth_buffer_[i] < 0)
+ {
+ depth_buffer_[i] = 0.0;
+ }
+ }
+ }
+
+ // Add Kinect/Primesense Quantisation Noise:
+ // TODO: better fit this:
+ // 0.6m = ~600 kinect return
+ // 20m = ~1070 kinect return - not not well calibrated
+ // The fitted model stated here cannot work for long ranges:
+ // http://www.ros.org/wiki/kinect_calibration/technical
+ // TODO: make a parameter
+ float bins = 470;
+ for (int i = 0; i < camera_width_*camera_height_ ; i++)
+ {
+ depth_buffer_[i] = ceil (depth_buffer_[i]*bins)/bins;
+ }
+ cout << "in add noise\n";
+}
+
+void
+RangeLikelihood::computeLikelihoods (float* reference,
+ std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses,
+ std::vector<float> & scores)
+{
+ #if DO_TIMING_PROFILE
+ vector<double> tic_toc;
+ tic_toc.push_back(getTime());
+ #endif
+
+ scores.resize (cols_*rows_);
+ std::fill (scores.begin (), scores.end (), 0);
+
+ // Generate depth image for each particle
+ render (poses);
+
+ #if DO_TIMING_PROFILE
+ tic_toc.push_back (getTime ());
+ #endif
+
+ #if DO_TIMING_PROFILE
+ tic_toc.push_back (getTime ());
+ #endif
+ // The depth image is now in depth_texture_
+
+ // Compute likelihoods
+ if (compute_likelihood_on_cpu_)
+ {
+ computeScores (reference, scores);
+ }
+ else
+ {
+ computeScoresShader (reference);
+
+ // Aggregate results (we do not use GPU to sum cpu scores)
+ if (aggregate_on_cpu_)
+ {
+ const float* score_buffer = getScoreBuffer();
+
+ for (int n = 0, row = 0; row < height_; ++row)
+ {
+ for (int col = 0; col < width_; ++col, ++n)
+ {
+ scores[row/row_height_ * cols_ + col/col_width_] += score_buffer[n];
+ }
+ }
+ }
+ else
+ {
+ int levels = max_level (row_height_, col_width_);
+ int reduced_width = width_ >> levels;
+ int reduced_height = height_ >> levels;
+ int reduced_col_width = col_width_ >> levels;
+ int reduced_row_height = row_height_ >> levels;
+
+ float* score_sum = new float[reduced_width * reduced_height];
+ sum_reduce_.sum (score_texture_, score_sum);
+ for (int n = 0, row = 0; row < reduced_height; ++row)
+ {
+ for (int col = 0; col < reduced_width; ++col, ++n)
+ {
+ scores[row/reduced_row_height * cols_ + col/reduced_col_width] += score_sum[n];
+ }
+ }
+ delete [] score_sum;
+ }
+ }
+
+ #if DO_TIMING_PROFILE
+ tic_toc.push_back (getTime ());
+ display_tic_toc (tic_toc, "range_likelihood");
+ #endif
+}
+
+// Computes the likelihood scores using a shader
+void
+pcl::simulation::RangeLikelihood::computeScoresShader (float* reference)
+{
+ if (gllib::getGLError () != GL_NO_ERROR)
+ {
+ std::cout << "GL error: RangeLikelihood::compute_scores_shader - enter" << std::endl;
+ }
+
+#ifdef SIMULATION_DEBUG
+ std::cout << "DepthSampler location: " << likelihood_program_->getUniformLocation ("DepthSampler") << std::endl;
+ std::cout << "ReferenceSampler location: " << likelihood_program_->getUniformLocation ("ReferenceSampler") << std::endl;
+ std::cout << "CostSampler location: " << likelihood_program_->getUniformLocation ("CostSampler") << std::endl;
+
+ int depth_tex_id;
+ int ref_tex_id;
+ int cost_tex_id;
+
+ glGetUniformiv(likelihood_program_->programId (), likelihood_program_->getUniformLocation ("DepthSampler"), &depth_tex_id);
+ glGetUniformiv(likelihood_program_->programId (), likelihood_program_->getUniformLocation ("ReferenceSampler"), &ref_tex_id);
+ glGetUniformiv(likelihood_program_->programId (), likelihood_program_->getUniformLocation ("CostSampler"), &cost_tex_id);
+ std::cout << "depth id: " << depth_tex_id << " " << GL_TEXTURE0 << std::endl;
+ std::cout << "ref id: " << ref_tex_id << " " << GL_TEXTURE1 << std::endl;
+ std::cout << "cost id: " << cost_tex_id << " " << GL_TEXTURE2 << std::endl;
+#endif
+
+ likelihood_program_->use ();
+ likelihood_program_->setUniform ("DepthSampler", 0);
+ likelihood_program_->setUniform ("ReferenceSampler", 1);
+ likelihood_program_->setUniform ("CostSampler", 2);
+ likelihood_program_->setUniform ("cols", cols_);
+ likelihood_program_->setUniform ("rows", rows_);
+ likelihood_program_->setUniform ("near", z_near_);
+ likelihood_program_->setUniform ("far", z_far_);
+
+ glBindFramebuffer (GL_FRAMEBUFFER, score_fbo_);
+ glDrawBuffer (GL_COLOR_ATTACHMENT0);
+
+ glReadBuffer (GL_NONE);
+ GLboolean enable_depth_test;
+ glGetBooleanv (GL_DEPTH_TEST, &enable_depth_test);
+ glDisable (GL_DEPTH_TEST);
+ glViewport (0, 0, width_, height_);
+
+ // Setup textures
+ glActiveTexture (GL_TEXTURE0);
+ glBindTexture (GL_TEXTURE_2D, depth_texture_);
+
+ glActiveTexture (GL_TEXTURE1);
+ glBindTexture (GL_TEXTURE_2D, sensor_texture_);
+ glTexImage2D (GL_TEXTURE_2D, 0, GL_R32F, col_width_, row_height_, 0, GL_RED, GL_FLOAT, reference);
+
+ glActiveTexture (GL_TEXTURE2);
+ glBindTexture (GL_TEXTURE_2D, likelihood_texture_);
+
+ quad_.render ();
+ glUseProgram (0);
+
+ glBindFramebuffer (GL_FRAMEBUFFER, 0);
+
+ // Unbind all textures that were used
+ glActiveTexture (GL_TEXTURE0);
+ glBindTexture (GL_TEXTURE_2D, 0);
+ glActiveTexture (GL_TEXTURE1);
+ glBindTexture (GL_TEXTURE_2D, 0);
+ glActiveTexture (GL_TEXTURE2);
+ glBindTexture (GL_TEXTURE_2D, 0);
+
+ if (gllib::getGLError () != GL_NO_ERROR)
+ {
+ std::cout << "GL error: RangeLikelihood::compute_scores_shader - exit" << std::endl;
+ }
+
+ if (enable_depth_test == GL_TRUE) glEnable (GL_DEPTH_TEST);
+
+}
+
+void
+RangeLikelihood::render (const std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > & poses)
+{
+ if (gllib::getGLError () != GL_NO_ERROR)
+ {
+ std::cerr << "GL Error: RangeLikelihood::render - enter" << std::endl;
+ }
+
+ GLint old_matrix_mode;
+ GLint old_draw_buffer;
+ GLint old_read_buffer;
+ glGetIntegerv (GL_DRAW_BUFFER, &old_draw_buffer);
+ glGetIntegerv (GL_READ_BUFFER, &old_read_buffer);
+ glGetIntegerv (GL_MATRIX_MODE, &old_matrix_mode);
+
+ glMatrixMode (GL_PROJECTION);
+ glPushMatrix ();
+
+ glMatrixMode (GL_MODELVIEW);
+ glPushMatrix ();
+
+ glBindFramebuffer (GL_FRAMEBUFFER, fbo_);
+
+ if (use_color_)
+ {
+ glDrawBuffer (GL_COLOR_ATTACHMENT0);
+ }
+ else
+ {
+ glDrawBuffer (GL_NONE);
+ }
+ glReadBuffer (GL_NONE);
+
+ GLenum status;
+ status = glCheckFramebufferStatus (GL_FRAMEBUFFER);
+ switch (status)
+ {
+ case GL_FRAMEBUFFER_COMPLETE:
+ {
+ break;
+ }
+ default:
+ {
+ std::cout << "RangeLikelihood::render: Framebuffer failed" << std::endl;
+ exit (-1);
+ }
+ }
+
+ // Render
+ glPushAttrib (GL_ALL_ATTRIB_BITS);
+ glEnable (GL_COLOR_MATERIAL);
+ glClearColor (0.0f, 0.0f, 0.0f, 0.0f);
+ glClearDepth (1.0);
+ glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ // Setup projection matrix
+ setupProjectionMatrix ();
+
+ glEnable (GL_DEPTH_TEST);
+ glDepthMask (GL_TRUE);
+ glCullFace (GL_FRONT);
+ drawParticles (poses);
+
+ glPopAttrib ();
+ glFlush ();
+
+ glBindFramebuffer (GL_FRAMEBUFFER, 0);
+
+ // Restore OpenGL state
+ glReadBuffer (old_read_buffer);
+ glDrawBuffer (old_draw_buffer);
+
+ glMatrixMode (GL_MODELVIEW);
+ glPopMatrix ();
+
+ glMatrixMode (GL_PROJECTION);
+ glPopMatrix ();
+
+ glMatrixMode (old_matrix_mode);
+
+ if (gllib::getGLError () != GL_NO_ERROR)
+ {
+ std::cerr << "GL Error: RangeLikelihood::render - exit" << std::endl;
+ }
+
+ color_buffer_dirty_ = true;
+ depth_buffer_dirty_ = true;
+ score_buffer_dirty_ = true;
+}
+
+const float*
+RangeLikelihood::getDepthBuffer ()
+{
+ if (depth_buffer_dirty_)
+ {
+ // Read depth
+ glBindFramebuffer (GL_FRAMEBUFFER, fbo_);
+ glReadPixels (0, 0, width_, height_, GL_DEPTH_COMPONENT, GL_FLOAT, depth_buffer_);
+ glBindFramebuffer (GL_FRAMEBUFFER, 0);
+
+ if (gllib::getGLError () != GL_NO_ERROR)
+ {
+ std::cerr << "GL Error: RangeLikelihoodGLSL::getDepthBuffer" << std::endl;
+ }
+
+ depth_buffer_dirty_ = false;
+ }
+ return depth_buffer_;
+}
+
+const uint8_t*
+RangeLikelihood::getColorBuffer ()
+{
+ // It's only possible to read the color buffer if it
+ // was rendered in the first place.
+ assert (use_color_);
+
+ if (color_buffer_dirty_)
+ {
+ std::cout << "Read color buffer" << std::endl;
+
+ // Read Color
+ GLint old_read_buffer;
+ glGetIntegerv (GL_READ_BUFFER, &old_read_buffer);
+
+ glBindFramebuffer (GL_FRAMEBUFFER, fbo_);
+ glReadBuffer (GL_COLOR_ATTACHMENT0);
+ glReadPixels (0, 0, width_, height_, GL_RGB, GL_UNSIGNED_BYTE, color_buffer_);
+ glBindFramebuffer (GL_FRAMEBUFFER, 0);
+ glReadBuffer (old_read_buffer);
+
+ if (gllib::getGLError () != GL_NO_ERROR)
+ {
+ std::cerr << "GL Error: RangeLikelihood::getColorBuffer" << std::endl;
+ }
+
+ color_buffer_dirty_ = false;
+ }
+ return color_buffer_;
+}
+
+// The scores are in score_texture_
+const float*
+RangeLikelihood::getScoreBuffer ()
+{
+ if (score_buffer_dirty_ && !compute_likelihood_on_cpu_)
+ {
+ glActiveTexture (GL_TEXTURE0);
+ glBindTexture (GL_TEXTURE_2D, score_texture_);
+ glGetTexImage (GL_TEXTURE_2D, 0, GL_RED, GL_FLOAT, score_buffer_);
+ glBindTexture (GL_TEXTURE_2D, 0);
+ if (gllib::getGLError () != GL_NO_ERROR)
+ {
+ std::cerr << "GL Error: RangeLikelihood::getScoreBuffer" << std::endl;
+ }
+ score_buffer_dirty_ = false;
+ }
+ return score_buffer_;
+}
--- /dev/null
+/*
+ * scene.cpp
+ *
+ * Created on: Aug 16, 2011
+ * Author: Hordur Johannsson
+ */
+
+#include <pcl/simulation/scene.h>
+
+namespace pcl
+{
+
+namespace simulation
+{
+
+void
+Scene::add (Model::Ptr model)
+{
+ models_.push_back(model);
+}
+
+void
+Scene::addCompleteModel (std::vector<Model::Ptr> model)
+{
+ models_.push_back (model[0]);
+}
+
+void
+Scene::draw ()
+{
+ for (std::vector<Model::Ptr>::iterator model = models_.begin (); model != models_.end (); ++model)
+ (*model)->draw ();
+}
+
+void
+Scene::clear ()
+{
+ models_.clear();
+}
+
+} // namespace - simulation
+} // namespace - pcl
--- /dev/null
+uniform sampler2D depth_tex;\r
+uniform sampler2D measurement_tex;\r
+\r
+void main(void)\r
+{ \r
+ float texel = texture2D(depth_tex, gl_TexCoord[0].st).x;\r
+ float texel2 = texture2D(measurement_tex, gl_TexCoord[1].st).x;\r
+ gl_FragColor = gl_Color * texel * texel2;\r
+}\r
--- /dev/null
+void main(void)
+{
+ gl_FrontColor = gl_Color;
+ gl_Position = ftransform();
+ gl_TexCoord[0] = gl_MultiTexCoord0;
+ gl_TexCoord[1] = gl_MultiTexCoord1;
+}
--- /dev/null
+#version 330
+
+uniform sampler2D Texture0;
+
+in vec2 TexCoord0;
+layout(location = 0) out vec4 FragColor;
+
+void main()
+{
+ FragColor = texture2D(Texture0, TexCoord0.st);
+}
--- /dev/null
+#version 330
+
+uniform mat4 MVP;
+
+layout (location = 0) in vec3 Position;
+layout (location = 1) in vec2 TexCoord;
+
+out vec2 TexCoord0;
+
+void main()
+{
+ TexCoord0 = TexCoord;
+ gl_Position = MVP * vec4(Position, 1.0);
+};
--- /dev/null
+#include <pcl/simulation/sum_reduce.h>
+
+using namespace pcl::simulation;
+
+pcl::simulation::SumReduce::SumReduce (int width, int height, int levels) : levels_ (levels),
+ width_ (width),
+ height_ (height)
+{
+ std::cout << "SumReduce: levels: " << levels_ << std::endl;
+
+ // Load shader
+ sum_program_ = gllib::Program::Ptr (new gllib::Program ());
+ // TODO: to remove file dependency include the shader source in the binary
+ if (!sum_program_->addShaderFile ("sum_score.vert", gllib::VERTEX))
+ {
+ std::cout << "Failed loading vertex shader" << std::endl;
+ exit (-1);
+ }
+
+ // TODO: to remove file dependency include the shader source in the binary
+ if (!sum_program_->addShaderFile ("sum_score.frag", gllib::FRAGMENT))
+ {
+ std::cout << "Failed loading fragment shader" << std::endl;
+ exit (-1);
+ }
+
+ sum_program_->link ();
+
+ // Setup the framebuffer object for rendering
+ glGenFramebuffers (1, &fbo_);
+ arrays_ = new GLuint[levels_];
+
+ glGenTextures (levels_, arrays_);
+
+ int level_width = width_;
+ int level_height = height_;
+
+ for (int i = 0; i < levels_; ++i)
+ {
+ level_width = level_width / 2;
+ level_height = level_height / 2;
+
+ glBindTexture (GL_TEXTURE_2D, arrays_[i]);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
+ glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
+ glTexImage2D (GL_TEXTURE_2D, 0, GL_R32F, level_width, level_height, 0, GL_RED, GL_FLOAT, NULL);
+ glBindTexture (GL_TEXTURE_2D, 0);
+ }
+}
+
+pcl::simulation::SumReduce::~SumReduce ()
+{
+ glDeleteTextures (levels_, arrays_);
+ glDeleteFramebuffers (1, &fbo_);
+}
+
+void
+pcl::simulation::SumReduce::sum (GLuint input_array, float* output_array)
+{
+ if (gllib::getGLError () != GL_NO_ERROR)
+ {
+ std::cout << "SumReduce::sum enter" << std::endl;
+ }
+
+ glDisable (GL_DEPTH_TEST);
+
+ glBindFramebuffer (GL_FRAMEBUFFER, fbo_);
+ int width = width_;
+ int height = height_;
+
+ glActiveTexture (GL_TEXTURE0);
+ glBindTexture (GL_TEXTURE_2D, input_array);
+
+ // use program
+ sum_program_->use ();
+ glUniform1i (sum_program_->getUniformLocation ("ArraySampler"), 0);
+
+ if (gllib::getGLError () != GL_NO_ERROR)
+ {
+ std::cout << "SumReduce::sum set sampler" << std::endl;
+ }
+
+ for (int i=0; i < levels_; ++i)
+ {
+ glFramebufferTexture2D (GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, arrays_[i], 0);
+ glDrawBuffer (GL_COLOR_ATTACHMENT0);
+
+ glViewport (0, 0, width/2, height/2);
+
+ float step_x = 1.0f / float (width);
+ float step_y = 1.0f / float (height);
+ sum_program_->setUniform ("step_x", step_x);
+ sum_program_->setUniform ("step_y", step_y);
+ //float step_x = 1.0f / static_cast<float> (width);
+ //float step_y = 1.0f / static_cast<float> (height);
+
+ quad_.render ();
+
+ if (gllib::getGLError () != GL_NO_ERROR)
+ {
+ std::cout << "SumReduce::sum render" << std::endl;
+ }
+
+ width = width / 2;
+ height = height / 2;
+
+ glActiveTexture (GL_TEXTURE0);
+ glBindTexture (GL_TEXTURE_2D, arrays_[i]);
+ }
+
+ glBindFramebuffer (GL_FRAMEBUFFER, 0);
+
+ glActiveTexture (GL_TEXTURE0);
+ glBindTexture (GL_TEXTURE_2D, 0);
+
+ glUseProgram (0);
+
+ // Final results is in arrays_[levels_-1]
+ glActiveTexture (GL_TEXTURE0);
+ glBindTexture (GL_TEXTURE_2D, arrays_[levels_-1]);
+ glGetTexImage (GL_TEXTURE_2D, 0, GL_RED, GL_FLOAT, output_array);
+ glBindTexture (GL_TEXTURE_2D, 0);
+
+ if (gllib::getGLError () != GL_NO_ERROR)
+ {
+ std::cout << "Error: SumReduce exit" << std::endl;
+ }
+}
+
--- /dev/null
+#version 330
+in vec2 TexCoord0;
+
+layout(location = 0) out vec4 FragColor;
+
+uniform float step_x;
+uniform float step_y;
+
+uniform sampler2D ArraySampler;
+
+void main()
+{
+ FragColor = texture2D(ArraySampler, TexCoord0.st) +
+ texture2D(ArraySampler, TexCoord0.st + vec2(step_x, 0.0)) +
+ texture2D(ArraySampler, TexCoord0.st + vec2(0.0, step_y)) +
+ texture2D(ArraySampler, TexCoord0.st + vec2(step_x, step_y));
+}
--- /dev/null
+#version 330
+
+layout (location = 0) in vec3 Position;
+layout (location = 1) in vec2 TexCoord;
+
+out vec2 TexCoord0;
+
+void main()
+{
+ TexCoord0 = TexCoord;
+ gl_Position = vec4(Position, 1.0);
+};
--- /dev/null
+find_package(GLEW)
+if(GLEW_FOUND)
+ find_package(GLUT)
+ if(GLUT_FOUND)
+ include_directories(${GLUT_INCLUDE_DIR} ${GLEW_INCLUDE_DIR})
+ include_directories(${VTK_INCLUDE_DIRS})
+
+ PCL_ADD_EXECUTABLE(pcl_sim_viewer ${SUBSYS_NAME} sim_viewer.cpp)
+ target_link_libraries (pcl_sim_viewer
+ ${VTK_IO_TARGET_LINK_LIBRARIES} pcl_kdtree
+ pcl_simulation pcl_common pcl_io pcl_visualization
+ ${GLEW_LIBRARIES} ${GLUT_LIBRARIES} ${OPENGL_LIBRARIES} ${GLEW_LIBRARIES})
+
+ PCL_ADD_EXECUTABLE(pcl_sim_test_simple ${SUBSYS_NAME} sim_test_simple.cpp)
+ target_link_libraries (pcl_sim_test_simple
+ ${VTK_IO_TARGET_LINK_LIBRARIES}
+ pcl_simulation pcl_common pcl_io pcl_visualization
+ ${GLEW_LIBRARIES} ${GLUT_LIBRARIES} ${OPENGL_LIBRARIES} ${GLEW_LIBRARIES})
+
+ PCL_ADD_EXECUTABLE(pcl_sim_test_performance ${SUBSYS_NAME} sim_test_performance.cpp)
+ target_link_libraries (pcl_sim_test_performance
+ ${VTK_IO_TARGET_LINK_LIBRARIES}
+ pcl_simulation pcl_common pcl_io pcl_visualization
+ ${GLEW_LIBRARIES} ${GLUT_LIBRARIES} ${OPENGL_LIBRARIES} ${GLEW_LIBRARIES})
+
+ set(srcs simulation_io.cpp )
+ set(incs simulation_io.hpp )
+ set(LIB_NAME pcl_simulation_io)
+ PCL_ADD_LIBRARY(${LIB_NAME} ${SUBSYS_NAME}
+ ${srcs} ${incs} ${compression_incs} ${impl_incs}
+ ${VTK_IO_TARGET_LINK_LIBRARIES}
+ ${OPENNI_INCLUDES})
+ target_link_libraries(${LIB_NAME} pcl_simulation pcl_common pcl_io
+ ${VTK_IO_TARGET_LINK_LIBRARIES} ${OPENGL_LIBRARIES} ${GLUT_LIBRARIES})
+
+ PCL_ADD_EXECUTABLE(pcl_sim_terminal_demo ${SUBSYS_NAME} sim_terminal_demo.cpp)
+ target_link_libraries (pcl_sim_terminal_demo
+ ${VTK_IO_TARGET_LINK_LIBRARIES}
+ pcl_simulation pcl_common pcl_io pcl_visualization pcl_simulation_io
+ ${GLEW_LIBRARIES} ${GLUT_LIBRARIES} ${OPENGL_LIBRARIES} ${GLEW_LIBRARIES})
+ endif(GLUT_FOUND)
+endif (GLEW_FOUND)
--- /dev/null
+Test and Example Programs for pcl_simulation
+mfallon and hordurj march 2012
+
+1. sim_viewer.cpp
+purpose: simulate in viewer which is almost the same as pcl_viewer
+status : use the mouse to drive around, and 'v' to capture a cloud. reads vtk and obj.
+ visualizes vtk and makes pcd of obj. conflict between RL and VTK means doesnt visualize/simulate properly
+was : range_pcd_viewer.cpp
+
+2. sim_terminal_demo.cpp
+purpose: simple app to demo speed and api to pcl_simulation
+status : reads obj, make a series of 640x480 simulated point clouds and exits
+depndcy: OpenCV for writing png images
+
+3. sim_test_performance.cpp
+purpose: GLUT/GLEW viewer used by Hordur to test GLSL optimizations. Creates two different viewing planes
+status : reads obj, creates window, use keyboard to drive around environment
+was : range_performance_test.cpp
+
+4. sim_test_simple
+purpose: similar code to #3 but has a 2x2 grid each containing 640x480 windows, but operates as #1. press 'v' to capture a cloud to file (only works properly if 2x2 canged to 1x1)
+status : reads obj, creates window, use keyboard to drive around environment
+was : range_test_v2.cpp
--- /dev/null
+/**
+ * Demo program for simulation library
+ * A virtual camera generates simulated point clouds
+ * No visual output, point clouds saved to file
+ *
+ * three different demo modes:
+ * 0 - static camera, 100 poses
+ * 1 - circular camera flying around the scene, 16 poses
+ * 2 - camera translates between 2 poses using slerp, 20 poses
+ * pcl_sim_terminal_demo 2 ../../../../kmcl/models/table_models/meta_model.ply
+ */
+
+#include <Eigen/Dense>
+#include <cmath>
+#include <iostream>
+#include <boost/shared_ptr.hpp>
+#ifdef _WIN32
+# define WIN32_LEAN_AND_MEAN
+# include <windows.h>
+#endif
+
+#include "simulation_io.hpp"
+
+using namespace Eigen;
+using namespace pcl;
+using namespace pcl::console;
+using namespace pcl::io;
+using namespace pcl::simulation;
+using namespace std;
+
+SimExample::Ptr simexample;
+
+void printHelp (int, char **argv)
+{
+ print_error ("Syntax is: %s <mode 1,2 or 3> <filename>\n", argv[0]);
+ print_info ("acceptable filenames include vtk, obj and ply. ply can support colour\n");
+}
+
+
+// Output the simulated output to file:
+void write_sim_output(string fname_root){
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out (new pcl::PointCloud<pcl::PointXYZRGB>);
+ bool write_cloud=true;
+ bool demo_other_stuff=true;
+
+ if (write_cloud)
+ {
+ // Read Color Buffer from the GPU before creating PointCloud:
+ // By default the buffers are not read back from the GPU
+ simexample->rl_->getColorBuffer ();
+ simexample->rl_->getDepthBuffer ();
+ // Add noise directly to the CPU depth buffer
+ simexample->rl_->addNoise ();
+
+ // Optional argument to save point cloud in global frame:
+ // Save camera relative:
+ //simexample->rl_->getPointCloud(pc_out);
+ // Save in global frame - applying the camera frame:
+ //simexample->rl_->getPointCloud(pc_out,true,simexample->camera_->getPose());
+ // Save in local frame
+ simexample->rl_->getPointCloud (pc_out,false,simexample->camera_->getPose ());
+ // TODO: what to do when there are more than one simulated view?
+
+ if (pc_out->points.size()>0){
+ std::cout << pc_out->points.size() << " points written to file\n";
+
+ pcl::PCDWriter writer;
+ //writer.write ( string (fname_root + ".pcd"), *pc_out, false); /// ASCII
+ writer.writeBinary ( string (fname_root + ".pcd") , *pc_out);
+ //cout << "finished writing file\n";
+ }else{
+ std::cout << pc_out->points.size() << " points in cloud, not written\n";
+ }
+ }
+ if (demo_other_stuff && write_cloud)
+ {
+ //simexample->write_score_image (simexample->rl_->getScoreBuffer (),
+ // string (fname_root + "_score.png") );
+ simexample->write_rgb_image (simexample->rl_->getColorBuffer (),
+ string (fname_root + "_rgb.png") );
+ simexample->write_depth_image (simexample->rl_->getDepthBuffer (),
+ string (fname_root + "_depth.png") );
+ //simexample->write_depth_image_uint (simexample->rl_->getDepthBuffer (),
+ // string (fname_root + "_depth_uint.png") );
+
+ // Demo interacton with RangeImage:
+ pcl::RangeImagePlanar rangeImage;
+ simexample->rl_->getRangeImagePlanar (rangeImage);
+ }
+}
+
+
+// A 'halo' camera - a circular ring of poses all pointing at a center point
+// @param: focus_center: the center points
+// @param: halo_r: radius of the ring
+// @param: halo_dz: elevation of the camera above/below focus_center's z value
+// @param: n_poses: number of generated poses
+void
+generate_halo(
+ std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > &poses,
+ Eigen::Vector3d focus_center,double halo_r,double halo_dz,int n_poses){
+
+ for (double t=0;t < (2*M_PI);t =t + (2*M_PI)/ ((double) n_poses) ){
+ double x = halo_r*cos(t);
+ double y = halo_r*sin(t);
+ double z = halo_dz;
+ double pitch =atan2( halo_dz,halo_r);
+ double yaw = atan2(-y,-x);
+
+ Eigen::Isometry3d pose;
+ pose.setIdentity();
+ Eigen::Matrix3d m;
+ m = AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
+ * AngleAxisd(pitch, Eigen::Vector3d::UnitY())
+ * AngleAxisd(0, Eigen::Vector3d::UnitZ());
+
+ pose *=m;
+ Vector3d v(x,y,z);
+ v += focus_center;
+ pose.translation() = v;
+ poses.push_back(pose);
+ }
+ return ;
+}
+
+int
+main (int argc, char** argv)
+{
+ // 1. Parse arguments:
+ print_info ("Manually generate a simulated RGB-D point cloud using pcl::simulation. For more information, use: %s -h\n", argv[0]);
+ if (argc < 3)
+ {
+ printHelp (argc, argv);
+ return (-1);
+ }
+ int mode=atoi(argv[1]);
+
+ // 2 Construct the simulation method:
+ int width = 640;
+ int height = 480;
+ simexample = SimExample::Ptr (new SimExample (argc, argv, height,width ));
+
+ // 3 Generate a series of simulation poses:
+ // -0 100 fixed poses
+ // -1 a 'halo' camera
+ // -2 slerp between two different poses
+ std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
+ if (mode==0){
+ // Create a pose:
+ Eigen::Isometry3d pose;
+ pose.setIdentity();
+ Matrix3d m;
+ //ypr:
+ m = AngleAxisd(-9.14989, Vector3d::UnitZ()) * AngleAxisd(0.20944, Vector3d::UnitY()) * AngleAxisd(0, Vector3d::UnitX());
+ pose *= m;
+ Vector3d v;
+ v << 1.31762, 0.382931, 1.89533;
+ pose.translation() = v;
+ for (int i=0;i< 100;i++){ // duplicate the pose 100 times
+ poses.push_back(pose);
+ }
+ }else if(mode==1){
+ Eigen::Vector3d focus_center(0,0,1.3);
+ double halo_r = 4;
+ double halo_dz = 2;
+ int n_poses=16;
+ generate_halo(poses,focus_center,halo_r,halo_dz,n_poses);
+ }else if (mode==2){
+ Eigen::Isometry3d pose1;
+ pose1.setIdentity();
+ pose1.translation() << 1,0.75,2;
+ Eigen::Matrix3d rot1;
+ rot1 = AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())
+ * AngleAxisd(M_PI/10, Eigen::Vector3d::UnitY())
+ * AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr
+ pose1.rotate(rot1);
+
+ Eigen::Isometry3d pose2;
+ pose2.setIdentity();
+ pose2.translation() << 1,-1,3;
+ Eigen::Matrix3d rot2;
+ rot2 = AngleAxisd(3*M_PI/4, Eigen::Vector3d::UnitZ())
+ * AngleAxisd(M_PI/4, Eigen::Vector3d::UnitY())
+ * AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr
+ pose2.rotate(rot2);
+
+ int n_poses = 20;
+ for (double i=0; i<=1;i+= 1/((double) n_poses -1) ){
+ Eigen::Quaterniond rot3;
+ Eigen::Quaterniond r1(pose1.rotation());
+ Eigen::Quaterniond r2(pose2.rotation());
+ rot3 = r1.slerp(i,r2);
+ Eigen::Isometry3d pose;
+ pose.setIdentity();
+ Eigen::Vector3d trans3 = (1-i)*pose1.translation() + i*pose2.translation();
+ pose.translation() << trans3[0] ,trans3[1] ,trans3[2];
+ pose.rotate(rot3);
+ poses.push_back(pose);
+ }
+ }
+
+ // 4 Do the simulation and write the output:
+ double tic_main = getTime();
+ for (size_t i=0;i < poses.size();i++){
+ std::stringstream ss;
+ ss.precision(20);
+ ss << "simcloud_" << i;// << ".pcd";
+ double tic = getTime();
+ simexample->doSim(poses[i]);
+
+ write_sim_output(ss.str());
+ cout << (getTime() -tic) << " sec\n";
+ }
+ cout << poses.size() << " poses simulated in " << (getTime() -tic_main) << "seconds\n";
+ cout << (poses.size()/ (getTime() -tic_main) ) << "Hz on average\n";
+
+ return 0;
+}
--- /dev/null
+/**
+ * This program performance tests for the range image likelihood library.
+ *
+ * Esc - quit
+ *
+ */
+
+#include <Eigen/Dense>
+#include <cmath>
+#include <iostream>
+#include <boost/shared_ptr.hpp>
+#ifdef _WIN32
+# define WIN32_LEAN_AND_MEAN
+# include <windows.h>
+#endif
+#include <GL/glew.h>
+
+#include <pcl/pcl_config.h>
+#ifdef OPENGL_IS_A_FRAMEWORK
+# include <OpenGL/gl.h>
+# include <OpenGL/glu.h>
+#else
+# include <GL/gl.h>
+# include <GL/glu.h>
+#endif
+#ifdef GLUT_IS_A_FRAMEWORK
+# include <GLUT/glut.h>
+#else
+# include <GL/glut.h>
+#endif
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+
+#include <pcl/common/common.h>
+#include <pcl/common/transforms.h>
+
+#include <pcl/point_types.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/surface/gp3.h>
+
+// define the following in order to eliminate the deprecated headers warning
+#define VTK_EXCLUDE_STRSTREAM_HEADERS
+#include <pcl/io/vtk_lib_io.h>
+
+#include <pcl/simulation/camera.h>
+#include <pcl/simulation/model.h>
+#include <pcl/simulation/scene.h>
+#include <pcl/simulation/range_likelihood.h>
+
+#include <pcl/console/print.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/time.h>
+
+using namespace Eigen;
+using namespace pcl;
+using namespace pcl::console;
+using namespace pcl::io;
+using namespace pcl::simulation;
+using namespace std;
+
+uint16_t t_gamma[2048];
+
+Scene::Ptr scene_;
+Camera::Ptr camera_;
+RangeLikelihood::Ptr range_likelihood_;
+// This is only used for displaying
+RangeLikelihood::Ptr range_likelihood_visualization_;
+
+int cols_;
+int rows_;
+int col_width_;
+int row_height_;
+int window_width_;
+int window_height_;
+TexturedQuad::Ptr textured_quad_;
+
+void
+printHelp (int, char **argv)
+{
+ print_error ("Syntax is: %s <filename>\n", argv[0]);
+ print_info ("acceptable filenames include vtk, obj and ply. ply can support color\n");
+}
+
+void
+display_score_image (const float* score_buffer)
+{
+ int npixels = range_likelihood_->getWidth () * range_likelihood_->getHeight ();
+ uint8_t* score_img = new uint8_t[npixels * 3];
+
+ float min_score = score_buffer[0];
+ float max_score = score_buffer[0];
+ for (int i=1; i<npixels; i++)
+ {
+ if (score_buffer[i] < min_score) min_score = score_buffer[i];
+ if (score_buffer[i] > max_score) max_score = score_buffer[i];
+ }
+ for (int i=0; i < npixels; i++)
+ {
+ float d = (score_buffer[i]-min_score)/(max_score-min_score);
+ score_img[3*i+0] = 0;
+ score_img[3*i+1] = static_cast<unsigned char> (d*255);
+ score_img[3*i+2] = 0;
+ }
+ textured_quad_->setTexture (score_img);
+ textured_quad_->render ();
+
+ delete [] score_img;
+}
+
+void display_depth_image (const float* depth_buffer, int width, int height)
+{
+ int npixels = width * height;
+ uint8_t* depth_img = new uint8_t[npixels * 3];
+
+ float min_depth = depth_buffer[0];
+ float max_depth = depth_buffer[0];
+ for (int i = 1; i < npixels; ++i)
+ {
+ if (depth_buffer[i] < min_depth) min_depth = depth_buffer[i];
+ if (depth_buffer[i] > max_depth) max_depth = depth_buffer[i];
+ }
+
+ for (int i = 0; i < npixels; ++i)
+ {
+ float zn = 0.7f;
+ float zf = 20.0f;
+ float d = depth_buffer[i];
+ float z = -zf*zn/((zf-zn)*(d - zf/(zf-zn)));
+ float b = 0.075f;
+ float f = 580.0f;
+ int kd = static_cast<int>(1090 - b*f/z*8);
+ if (kd < 0) kd = 0;
+ else if (kd > 2047) kd = 2047;
+
+ int pval = t_gamma[kd];
+ uint8_t lb = static_cast<uint8_t> (pval & 0xff);
+ switch (pval >> 8)
+ {
+ case 0:
+ depth_img[3*i+0] = 255;
+ depth_img[3*i+1] = static_cast<uint8_t> (255-lb);
+ depth_img[3*i+2] = static_cast<uint8_t> (255-lb);
+ break;
+ case 1:
+ depth_img[3*i+0] = 255;
+ depth_img[3*i+1] = lb;
+ depth_img[3*i+2] = 0;
+ break;
+ case 2:
+ depth_img[3*i+0] = static_cast<uint8_t> (255-lb);
+ depth_img[3*i+1] = 255;
+ depth_img[3*i+2] = 0;
+ break;
+ case 3:
+ depth_img[3*i+0] = 0;
+ depth_img[3*i+1] = 255;
+ depth_img[3*i+2] = lb;
+ break;
+ case 4:
+ depth_img[3*i+0] = 0;
+ depth_img[3*i+1] = static_cast<uint8_t> (255-lb);
+ depth_img[3*i+2] = 255;
+ break;
+ case 5:
+ depth_img[3*i+0] = 0;
+ depth_img[3*i+1] = 0;
+ depth_img[3*i+2] = static_cast<uint8_t> (255-lb);
+ break;
+ default:
+ depth_img[3*i+0] = 0;
+ depth_img[3*i+1] = 0;
+ depth_img[3*i+2] = 0;
+ break;
+ }
+ }
+
+ glRasterPos2i (-1,-1);
+ glDrawPixels (width, height,
+ GL_RGB, GL_UNSIGNED_BYTE, depth_img);
+
+ delete [] depth_img;
+}
+
+void
+display ()
+{
+ float* reference = new float[range_likelihood_->getRowHeight () * range_likelihood_->getColWidth ()];
+ const float* depth_buffer = range_likelihood_->getDepthBuffer ();
+ // Copy one image from our last as a reference.
+ for (int i = 0, n = 0; i < range_likelihood_->getRowHeight (); ++i)
+ {
+ for (int j = 0; j < range_likelihood_->getColWidth (); ++j)
+ {
+ reference[n++] = depth_buffer[ (i + range_likelihood_->getRowHeight () * range_likelihood_->getRows () / 2) * range_likelihood_->getWidth ()
+ + j + range_likelihood_->getColWidth () * range_likelihood_->getCols () / 2];
+ }
+ }
+
+ float* reference_vis = new float[range_likelihood_visualization_->getRowHeight () * range_likelihood_visualization_->getColWidth ()];
+ const float* depth_buffer_vis = range_likelihood_visualization_->getDepthBuffer ();
+ // Copy one image from our last as a reference.
+ for (int i = 0, n = 0; i < range_likelihood_visualization_->getRowHeight (); ++i)
+ {
+ for (int j = 0; j < range_likelihood_visualization_->getColWidth (); ++j)
+ {
+ reference_vis[n++] = depth_buffer_vis[i*range_likelihood_visualization_->getWidth () + j];
+ }
+ }
+
+ std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
+ std::vector<float> scores;
+
+ // Render a single pose for visualization
+ poses.clear ();
+ poses.push_back (camera_->getPose ());
+ range_likelihood_visualization_->computeLikelihoods (reference_vis, poses, scores);
+
+ glDrawBuffer (GL_BACK);
+ glReadBuffer (GL_BACK);
+
+ // Draw the resulting images from the range_likelihood
+ glViewport (range_likelihood_visualization_->getWidth (), 0,
+ range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
+ glMatrixMode (GL_PROJECTION);
+ glLoadIdentity ();
+ glMatrixMode (GL_MODELVIEW);
+ glLoadIdentity ();
+
+ // Draw the color image
+ glColorMask (true, true, true, true);
+ glClearColor (0, 0, 0, 0);
+ glClearDepth (1);
+ glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+ glDisable (GL_DEPTH_TEST);
+
+ glRasterPos2i (-1,-1);
+ glDrawPixels (range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight (),
+ GL_RGB, GL_UNSIGNED_BYTE, range_likelihood_visualization_->getColorBuffer ());
+
+ // Draw the depth image
+ glViewport (0, 0, range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
+
+ glMatrixMode (GL_PROJECTION);
+ glLoadIdentity ();
+ glMatrixMode (GL_MODELVIEW);
+ glLoadIdentity ();
+ display_depth_image (range_likelihood_visualization_->getDepthBuffer (),
+ range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
+
+
+ poses.clear ();
+ for (int i = 0; i < range_likelihood_->getRows (); ++i)
+ {
+ for (int j = 0; j < range_likelihood_->getCols (); ++j)
+ {
+ Camera camera (*camera_);
+ camera.move ((j - range_likelihood_->getCols () / 2.0) * 0.1,
+ (i - range_likelihood_->getRows () / 2.0) * 0.1,
+ 0.0);
+ poses.push_back (camera.getPose ());
+ }
+ }
+ std::cout << std::endl;
+
+ TicToc tt;
+ tt.tic();
+ range_likelihood_->computeLikelihoods (reference, poses, scores);
+ tt.toc();
+ tt.toc_print();
+
+ if (gllib::getGLError () != GL_NO_ERROR)
+ {
+ std::cerr << "GL Error: RangeLikelihood::computeLikelihoods: finished" << std::endl;
+ }
+
+#if 0
+ std::cout << "score: ";
+ for (size_t i = 0; i < scores.size (); ++i)
+ {
+ std::cout << " " << scores[i];
+ }
+ std::cout << std::endl;
+#endif
+
+ std::cout << "camera: " << camera_->getX ()
+ << " " << camera_->getY ()
+ << " " << camera_->getZ ()
+ << " " << camera_->getRoll ()
+ << " " << camera_->getPitch ()
+ << " " << camera_->getYaw ()
+ << std::endl;
+
+ delete [] reference_vis;
+ delete [] reference;
+
+ if (gllib::getGLError () != GL_NO_ERROR)
+ {
+ std::cerr << "GL Error: before buffers" << std::endl;
+ }
+
+ glBindFramebuffer (GL_FRAMEBUFFER, 0);
+ glDrawBuffer (GL_BACK);
+ glReadBuffer (GL_BACK);
+
+ if (gllib::getGLError () != GL_NO_ERROR)
+ {
+ std::cerr << "GL Error: after buffers" << std::endl;
+ }
+
+
+ glMatrixMode (GL_PROJECTION);
+ glLoadIdentity ();
+ glMatrixMode (GL_MODELVIEW);
+ glLoadIdentity ();
+
+ if (gllib::getGLError () != GL_NO_ERROR)
+ {
+ std::cerr << "GL Error: before viewport" << std::endl;
+ }
+
+ // Draw the score image for the particles
+ glViewport (0, range_likelihood_visualization_->getHeight (),
+ range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
+
+
+ if (gllib::getGLError () != GL_NO_ERROR)
+ {
+ std::cerr << "GL Error: after viewport" << std::endl;
+ }
+
+ display_score_image (range_likelihood_->getScoreBuffer ());
+
+ // Draw the depth image for the particles
+ glViewport (range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight (),
+ range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
+
+ display_score_image (range_likelihood_->getDepthBuffer ());
+
+ glutSwapBuffers ();
+}
+
+// Handle normal keys
+void
+on_keyboard (unsigned char key, int, int)
+{
+ if (key == 27)
+ exit (0);
+}
+
+// Read in a 3D model
+void
+loadPolygonMeshModel (char* polygon_file)
+{
+ pcl::PolygonMesh mesh;
+ pcl::io::loadPolygonFile (polygon_file, mesh);
+ pcl::PolygonMesh::Ptr cloud (new pcl::PolygonMesh (mesh));
+
+ TriangleMeshModel::Ptr model = TriangleMeshModel::Ptr (new TriangleMeshModel (cloud));
+ scene_->add (model);
+
+ std::cout << "Just read " << polygon_file << std::endl;
+ std::cout << mesh.polygons.size () << " polygons and "
+ << mesh.cloud.data.size () << " triangles\n";
+}
+
+void
+initialize (int argc, char** argv)
+{
+ const GLubyte* version = glGetString (GL_VERSION);
+ print_info ("OpenGL Version: %s\n", version);
+
+ // works well for MIT CSAIL model 2nd floor:
+ camera_->set (27.4503, 37.383, 4.30908, 0.0, 0.0654498, -2.25802);
+
+ if (argc > 1) loadPolygonMeshModel (argv[1]);
+}
+
+int
+main (int argc, char** argv)
+{
+ int width = 640;
+ int height = 480;
+
+ window_width_ = width * 2;
+ window_height_ = height * 2;
+
+ int cols = 30;
+ int rows = 30;
+ int col_width = 20;
+ int row_height = 15;
+
+ print_info ("Range likelihood performance tests using pcl::simulation. For more information, use: %s -h\n", argv[0]);
+
+ if (argc < 2)
+ {
+ printHelp (argc, argv);
+ return (-1);
+ }
+
+ for (int i = 0; i < 2048; ++i)
+ {
+ float v = static_cast<float> (i / 2048.0);
+ v = powf(v, 3)* 6;
+ t_gamma[i] = static_cast<uint16_t> (v*6*256);
+ }
+
+ glutInit (&argc, argv);
+ glutInitDisplayMode (GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);
+ glutInitWindowPosition (10, 10);
+ glutInitWindowSize (window_width_, window_height_);
+ glutCreateWindow ("OpenGL range likelihood");
+
+ GLenum err = glewInit ();
+ if (GLEW_OK != err)
+ {
+ std::cerr << "Error: " << glewGetErrorString (err) << std::endl;
+ exit (-1);
+ }
+
+ std::cout << "Status: Using GLEW " << glewGetString (GLEW_VERSION) << std::endl;
+
+ if (glewIsSupported ("GL_VERSION_2_0"))
+ std::cout << "OpenGL 2.0 supported" << std::endl;
+ else
+ {
+ std::cerr << "Error: OpenGL 2.0 not supported" << std::endl;
+ exit (1);
+ }
+
+ std::cout << "GL_MAX_VIEWPORTS: " << GL_MAX_VIEWPORTS << std::endl;
+
+ camera_ = Camera::Ptr (new Camera ());
+ scene_ = Scene::Ptr (new Scene ());
+
+ range_likelihood_visualization_ = RangeLikelihood::Ptr (new RangeLikelihood (1, 1, height, width, scene_));
+ range_likelihood_ = RangeLikelihood::Ptr (new RangeLikelihood (rows, cols, row_height, col_width, scene_));
+
+ // Actually corresponds to default parameters:
+ range_likelihood_visualization_->setCameraIntrinsicsParameters (
+ 640, 480, 576.09757860f, 576.09757860f, 321.06398107f, 242.97676897f);
+ range_likelihood_visualization_->setComputeOnCPU (false);
+ range_likelihood_visualization_->setSumOnCPU (false);
+ range_likelihood_visualization_->setUseColor (true);
+
+ range_likelihood_->setCameraIntrinsicsParameters (
+ 640, 480, 576.09757860f, 576.09757860f, 321.06398107f, 242.97676897f);
+ range_likelihood_->setComputeOnCPU (false);
+ range_likelihood_->setSumOnCPU (false);
+ range_likelihood_->setUseColor (false);
+
+ textured_quad_ = TexturedQuad::Ptr (new TexturedQuad (range_likelihood_->getWidth (),
+ range_likelihood_->getHeight ()));
+
+ initialize (argc, argv);
+
+ glutDisplayFunc (display);
+ glutIdleFunc (display);
+ glutKeyboardFunc (on_keyboard);
+ glutMainLoop ();
+}
--- /dev/null
+/**
+ * This program tests the range image likelihood library.
+ *
+ * You can move around using
+ * w - forward
+ * a - backward
+ * s - left
+ * d - right
+ * q - up
+ * z - down
+ * f - write point cloud file
+ *
+ * The mouse controls the direction of movement
+ *
+ * Esc - quit
+ *
+ */
+
+#include <Eigen/Dense>
+#include <cmath>
+#include <iostream>
+#include <boost/shared_ptr.hpp>
+#ifdef _WIN32
+# define WIN32_LEAN_AND_MEAN
+# include <windows.h>
+#endif
+#include <GL/glew.h>
+
+#include <pcl/pcl_config.h>
+#ifdef OPENGL_IS_A_FRAMEWORK
+# include <OpenGL/gl.h>
+# include <OpenGL/glu.h>
+#else
+# include <GL/gl.h>
+# include <GL/glu.h>
+#endif
+#ifdef GLUT_IS_A_FRAMEWORK
+# include <GLUT/glut.h>
+#else
+# include <GL/glut.h>
+#endif
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+
+#include "pcl/common/common.h"
+#include "pcl/common/transforms.h"
+
+#include <pcl/point_types.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/surface/gp3.h>
+
+// define the following in order to eliminate the deprecated headers warning
+#define VTK_EXCLUDE_STRSTREAM_HEADERS
+#include <pcl/io/vtk_lib_io.h>
+
+#include "pcl/simulation/camera.h"
+#include "pcl/simulation/model.h"
+#include "pcl/simulation/scene.h"
+#include "pcl/simulation/range_likelihood.h"
+
+#include <pcl/console/print.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/time.h>
+
+// RangeImage:
+#include <pcl/range_image/range_image_planar.h>
+
+// Pop-up viewer
+#include <pcl/visualization/cloud_viewer.h>
+#include <boost/thread/thread.hpp>
+
+using namespace Eigen;
+using namespace pcl;
+using namespace pcl::console;
+using namespace pcl::io;
+using namespace pcl::simulation;
+
+using namespace std;
+
+uint16_t t_gamma[2048];
+
+Scene::Ptr scene_;
+Camera::Ptr camera_;
+RangeLikelihood::Ptr range_likelihood_;
+
+int window_width_;
+int window_height_;
+bool paused_;
+bool write_file_;
+
+void printHelp (int, char **argv)
+{
+ print_error ("Syntax is: %s <filename>\n", argv[0]);
+ print_info ("acceptable filenames include vtk, obj and ply. ply can support colour\n");
+}
+
+void wait ()
+{
+ std::cout << "Press enter to continue";
+ getchar();
+ std::cout << "\n\n";
+}
+
+void display_score_image(const float* score_buffer)
+{
+ int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
+ uint8_t* score_img = new uint8_t[npixels * 3];
+
+ float min_score = score_buffer[0];
+ float max_score = score_buffer[0];
+ for (int i=1; i<npixels; i++)
+ {
+ if (score_buffer[i] < min_score) min_score = score_buffer[i];
+ if (score_buffer[i] > max_score) max_score = score_buffer[i];
+ }
+
+ for (int i=0; i<npixels; i++)
+ {
+ float d = (score_buffer[i]-min_score)/(max_score-min_score);
+ score_img[3*i+0] = 0;
+ score_img[3*i+1] = d*255;
+ score_img[3*i+2] = 0;
+ }
+
+ glRasterPos2i(-1,-1);
+ glDrawPixels(range_likelihood_->getWidth(), range_likelihood_->getHeight(), GL_RGB, GL_UNSIGNED_BYTE, score_img);
+
+ delete [] score_img;
+}
+
+
+
+
+void display_depth_image (const float* depth_buffer, int width, int height)
+{
+ int npixels = width * height;
+ uint8_t* depth_img = new uint8_t[npixels * 3];
+
+ float min_depth = depth_buffer[0];
+ float max_depth = depth_buffer[0];
+ for (int i = 1; i < npixels; ++i)
+ {
+ if (depth_buffer[i] < min_depth) min_depth = depth_buffer[i];
+ if (depth_buffer[i] > max_depth) max_depth = depth_buffer[i];
+ }
+
+ for (int i = 0; i < npixels; ++i)
+ {
+ float zn = 0.7f;
+ float zf = 20.0f;
+ float d = depth_buffer[i];
+ float z = -zf*zn/((zf-zn)*(d - zf/(zf-zn)));
+ float b = 0.075f;
+ float f = 580.0f;
+ uint16_t kd = static_cast<uint16_t>(1090 - b*f/z*8);
+ if (kd > 2047) kd = 2047;
+
+ int pval = t_gamma[kd];
+ int lb = pval & 0xff;
+ switch (pval >> 8)
+ {
+ case 0:
+ depth_img[3*i+0] = 255;
+ depth_img[3*i+1] = 255-lb;
+ depth_img[3*i+2] = 255-lb;
+ break;
+ case 1:
+ depth_img[3*i+0] = 255;
+ depth_img[3*i+1] = lb;
+ depth_img[3*i+2] = 0;
+ break;
+ case 2:
+ depth_img[3*i+0] = 255-lb;
+ depth_img[3*i+1] = 255;
+ depth_img[3*i+2] = 0;
+ break;
+ case 3:
+ depth_img[3*i+0] = 0;
+ depth_img[3*i+1] = 255;
+ depth_img[3*i+2] = lb;
+ break;
+ case 4:
+ depth_img[3*i+0] = 0;
+ depth_img[3*i+1] = 255-lb;
+ depth_img[3*i+2] = 255;
+ break;
+ case 5:
+ depth_img[3*i+0] = 0;
+ depth_img[3*i+1] = 0;
+ depth_img[3*i+2] = 255-lb;
+ break;
+ default:
+ depth_img[3*i+0] = 0;
+ depth_img[3*i+1] = 0;
+ depth_img[3*i+2] = 0;
+ break;
+ }
+ }
+
+ glRasterPos2i (-1,-1);
+ glDrawPixels (width, height,
+ GL_RGB, GL_UNSIGNED_BYTE, depth_img);
+
+ delete [] depth_img;
+}
+
+/*
+void display_depth_image(const float* depth_buffer)
+{
+ int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
+ uint8_t* depth_img = new uint8_t[npixels * 3];
+
+ float min_depth = depth_buffer[0];
+ float max_depth = depth_buffer[0];
+ for (int i=1; i<npixels; i++)
+ {
+ if (depth_buffer[i] < min_depth) min_depth = depth_buffer[i];
+ if (depth_buffer[i] > max_depth) max_depth = depth_buffer[i];
+ }
+
+ for (int i=0; i<npixels; i++)
+ {
+ float zn = 0.7;
+ float zf = 20.0;
+ float d = depth_buffer[i];
+ float z = -zf*zn/((zf-zn)*(d - zf/(zf-zn)));
+ float b = 0.075;
+ float f = 580.0;
+ uint16_t kd = static_cast<uint16_t>(1090 - b*f/z*8);
+ if (kd < 0) kd = 0;
+ else if (kd>2047) kd = 2047;
+
+ int pval = t_gamma[kd];
+ int lb = pval & 0xff;
+ switch (pval>>8) {
+ case 0:
+ depth_img[3*i+0] = 255;
+ depth_img[3*i+1] = 255-lb;
+ depth_img[3*i+2] = 255-lb;
+ break;
+ case 1:
+ depth_img[3*i+0] = 255;
+ depth_img[3*i+1] = lb;
+ depth_img[3*i+2] = 0;
+ break;
+ case 2:
+ depth_img[3*i+0] = 255-lb;
+ depth_img[3*i+1] = 255;
+ depth_img[3*i+2] = 0;
+ break;
+ case 3:
+ depth_img[3*i+0] = 0;
+ depth_img[3*i+1] = 255;
+ depth_img[3*i+2] = lb;
+ break;
+ case 4:
+ depth_img[3*i+0] = 0;
+ depth_img[3*i+1] = 255-lb;
+ depth_img[3*i+2] = 255;
+ break;
+ case 5:
+ depth_img[3*i+0] = 0;
+ depth_img[3*i+1] = 0;
+ depth_img[3*i+2] = 255-lb;
+ break;
+ default:
+ depth_img[3*i+0] = 0;
+ depth_img[3*i+1] = 0;
+ depth_img[3*i+2] = 0;
+ break;
+ }
+ }
+
+ glRasterPos2i(-1,-1);
+ glDrawPixels(range_likelihood_->getWidth(), range_likelihood_->getHeight(), GL_RGB, GL_UNSIGNED_BYTE, depth_img);
+
+ delete [] depth_img;
+}
+*/
+
+boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis (pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
+{
+ // --------------------------------------------
+ // -----Open 3D viewer and add point cloud-----
+ // --------------------------------------------
+ boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
+ viewer->setBackgroundColor (0, 0, 0);
+ pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
+ viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
+ viewer->addCoordinateSystem (1.0, "reference");
+ viewer->initCameraParameters ();
+ return (viewer);
+}
+
+
+void display ()
+{
+ float* reference = new float[range_likelihood_->getRowHeight() * range_likelihood_->getColWidth()];
+ const float* depth_buffer = range_likelihood_->getDepthBuffer();
+ // Copy one image from our last as a reference.
+ for (int i=0, n=0; i<range_likelihood_->getRowHeight(); ++i)
+ {
+ for (int j=0; j<range_likelihood_->getColWidth(); ++j)
+ {
+ reference[n++] = depth_buffer[i*range_likelihood_->getWidth() + j];
+ }
+ }
+
+ std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
+ std::vector<float> scores;
+ int n = range_likelihood_->getRows ()*range_likelihood_->getCols ();
+ for (int i = 0; i < n; ++i)
+ {
+ Camera camera(*camera_);
+ camera.move(0.0,i*0.02,0.0);
+ //camera.move(0.0,i*0.02,0.0);
+ poses.push_back (camera.getPose ());
+ }
+
+ range_likelihood_->computeLikelihoods (reference, poses, scores);
+
+ range_likelihood_->computeLikelihoods (reference, poses, scores);
+ std::cout << "score: ";
+ for (size_t i = 0; i<scores.size (); ++i)
+ {
+ std::cout << " " << scores[i];
+ }
+ std::cout << std::endl;
+
+ std::cout << "camera: " << camera_->getX ()
+ << " " << camera_->getY ()
+ << " " << camera_->getZ ()
+ << " " << camera_->getRoll ()
+ << " " << camera_->getPitch ()
+ << " " << camera_->getYaw ()
+ << std::endl;
+
+ delete [] reference;
+
+ glDrawBuffer (GL_BACK);
+ glReadBuffer (GL_BACK);
+
+ // Draw the resulting images from the range_likelihood
+ glViewport (range_likelihood_->getWidth (), 0, range_likelihood_->getWidth (), range_likelihood_->getHeight ());
+ glMatrixMode (GL_PROJECTION);
+ glLoadIdentity ();
+ glMatrixMode (GL_MODELVIEW);
+ glLoadIdentity ();
+
+ // Draw the color image
+ glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+ glColorMask (true, true, true, true);
+ glDisable (GL_DEPTH_TEST);
+
+ glRasterPos2i (-1,-1);
+ glDrawPixels (range_likelihood_->getWidth (), range_likelihood_->getHeight (),
+ GL_RGB, GL_UNSIGNED_BYTE, range_likelihood_->getColorBuffer ());
+
+ // Draw the depth image
+ glViewport (0, 0, range_likelihood_->getWidth (), range_likelihood_->getHeight ());
+
+ glMatrixMode (GL_PROJECTION);
+ glLoadIdentity ();
+ glMatrixMode (GL_MODELVIEW);
+ glLoadIdentity ();
+// display_depth_image (range_likelihood_->getDepthBuffer ());
+ display_depth_image (range_likelihood_->getDepthBuffer (),
+ range_likelihood_->getWidth (), range_likelihood_->getHeight ());
+
+
+ // Draw the score image
+ glViewport (0, range_likelihood_->getHeight (),
+ range_likelihood_->getWidth (), range_likelihood_->getHeight ());
+ glMatrixMode (GL_PROJECTION);
+ glLoadIdentity ();
+ glMatrixMode (GL_MODELVIEW);
+ glLoadIdentity ();
+ display_score_image (range_likelihood_->getScoreBuffer ());
+
+ glutSwapBuffers ();
+
+ if (write_file_)
+ {
+ range_likelihood_->addNoise ();
+ pcl::RangeImagePlanar rangeImage;
+ range_likelihood_->getRangeImagePlanar (rangeImage);
+
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out (new pcl::PointCloud<pcl::PointXYZRGB>);
+
+ // Optional argument to save point cloud in global frame:
+ // Save camera relative:
+ //range_likelihood_->getPointCloud(pc_out);
+ // Save in global frame - applying the camera frame:
+ //range_likelihood_->getPointCloud(pc_out,true,camera_->pose());
+ // Save in local frame
+ range_likelihood_->getPointCloud (pc_out,false,camera_->getPose ());
+ // TODO: what to do when there are more than one simulated view?
+
+ pcl::PCDWriter writer;
+ writer.write ("simulated_range_image.pcd", *pc_out, false);
+ cout << "finished writing file\n";
+
+// pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
+// viewer.showCloud (pc_out);
+
+
+ boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+ viewer = simpleVis(pc_out);
+ while (!viewer->wasStopped ())
+ {
+ viewer->spinOnce (100);
+ boost::this_thread::sleep (boost::posix_time::microseconds (100000));
+ }
+
+ // doesnt work:
+// viewer->~PCLVisualizer();
+// viewer.reset();
+
+
+ cout << "done\n";
+ // Problem: vtk and opengl dont seem to play very well together
+ // vtk seems to misbehave after a little while and wont keep the window on the screen
+
+ // method1: kill with [x] - but eventually it crashes:
+ //while (!viewer.wasStopped ()){
+ //}
+
+ // method2: eventually starts ignoring cin and pops up on screen and closes almost
+ // immediately
+ // cout << "enter 1 to cont\n";
+ // cin >> pause;
+ // viewer.wasStopped ();
+
+ // method 3: if you interact with the window with keys, the window is not closed properly
+ // TODO: use pcl methods as this time stuff is probably not cross playform
+// struct timespec t;
+// t.tv_sec = 100;
+// //t.tv_nsec = (time_t)(20000000); // short sleep
+// t.tv_nsec = (time_t)(0); // long sleep - normal speed
+// nanosleep (&t, NULL);
+ write_file_ = 0;
+ }
+}
+
+// Handle normal keys
+void
+on_keyboard (unsigned char key, int, int)
+{
+ double speed = 0.1;
+
+ if (key == 27)
+ exit(0);
+ else if (key == 'w' || key == 'W')
+ camera_->move(speed,0,0);
+ else if (key == 's' || key == 'S')
+ camera_->move(-speed,0,0);
+ else if (key == 'a' || key == 'A')
+ camera_->move(0,speed,0);
+ else if (key == 'd' || key == 'D')
+ camera_->move(0,-speed,0);
+ else if (key == 'q' || key == 'Q')
+ camera_->move(0,0,speed);
+ else if (key == 'z' || key == 'Z')
+ camera_->move(0,0,-speed);
+ else if (key == 'p' || key == 'P')
+ paused_ = !paused_;
+ else if (key == 'v' || key == 'V')
+ write_file_ = 1;
+
+ // Use glutGetModifiers for modifiers
+ // GLUT_ACTIVE_SHIFT, GLUT_ACTIVE_CTRL, GLUT_ACTIVE_ALT
+}
+
+// Handle special keys, e.g. F1, F2, ...
+void
+on_special(int key, int, int)
+{
+ switch (key) {
+ case GLUT_KEY_F1:
+ break;
+ case GLUT_KEY_HOME:
+ break;
+ }
+}
+
+void
+on_reshape(int w, int h)
+{
+ // Window size changed
+ window_width_ = w;
+ window_height_ = h;
+}
+
+void
+on_mouse(int, int, int, int)
+{
+ // button:
+ // GLUT_LEFT_BUTTON
+ // GLUT_MIDDLE_BUTTON
+ // GLUT_RIGHT_BUTTON
+ //
+ // state:
+ // GLUT_UP
+ // GLUT_DOWN
+}
+
+void
+on_motion(int, int)
+{
+}
+
+void
+on_passive_motion(int x, int y)
+{
+ if (paused_) return;
+
+ double pitch = -(0.5-(double)y/window_height_)*M_PI * 4; // in window coordinates positive y-axis is down
+ double yaw = (0.5-(double)x/window_width_)*M_PI*2 * 4;
+
+ camera_->setPitch (pitch);
+ camera_->setYaw (yaw);
+}
+
+void on_entry (int)
+{
+ // state:
+ // GLUT_LEFT
+ // GLUT_ENTERED
+}
+
+
+// Read in a 3D model
+void load_PolygonMesh_model (char* polygon_file)
+{
+ pcl::PolygonMesh mesh; // (new pcl::PolygonMesh);
+ //pcl::io::loadPolygonFile("/home/mfallon/data/models/dalet/Darlek_modified_works.obj",mesh);
+ pcl::io::loadPolygonFile (polygon_file, mesh);
+ pcl::PolygonMesh::Ptr cloud (new pcl::PolygonMesh (mesh));
+
+ // Not sure if PolygonMesh assumes triangles if to
+ // TODO: Ask a developer
+ PolygonMeshModel::Ptr model = PolygonMeshModel::Ptr (new PolygonMeshModel (GL_POLYGON, cloud));
+ scene_->add (model);
+
+ std::cout << "Just read " << polygon_file << std::endl;
+ std::cout << mesh.polygons.size () << " polygons and "
+ << mesh.cloud.data.size () << " triangles\n";
+
+}
+
+void
+initialize (int, char** argv)
+{
+ const GLubyte* version = glGetString (GL_VERSION);
+ std::cout << "OpenGL Version: " << version << std::endl;
+
+ // works well for MIT CSAIL model 3rd floor:
+ //camera_->set(4.04454, 44.9377, 1.1, 0.0, 0.0, -2.00352);
+
+ // works well for MIT CSAIL model 2nd floor:
+// camera_->set (27.4503, 37.383, 4.30908, 0.0, 0.0654498, -2.25802);
+
+ // works for small files:
+ //camera_->set(-5.0, 0.0, 1.0, 0.0, 0.0, 0.0);
+ camera_->set( 1.31762, 0.382931, 1.89533, 0, 0.20944, -9.14989);
+ camera_->setPitch(0.20944); // not sure why this is here:
+ //camera_->set(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+
+ cout << "About to read: " << argv[1] << endl;
+ load_PolygonMesh_model (argv[1]);
+
+ paused_ = false;
+}
+
+int
+main (int argc, char** argv)
+{
+ int width = 640;
+ int height = 480;
+
+ window_width_ = width * 2;
+ window_height_ = height * 2;
+
+ print_info ("Manually generate a simulated RGB-D point cloud using pcl::simulation. For more information, use: %s -h\n", argv[0]);
+
+ if (argc < 2)
+ {
+ printHelp (argc, argv);
+ return (-1);
+ }
+
+ int i;
+ for (i=0; i<2048; i++)
+ {
+ float v = i/2048.0;
+ v = powf(v, 3)* 6;
+ t_gamma[i] = v*6*256;
+ }
+
+ glutInit (&argc, argv);
+ glutInitDisplayMode (GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);// was GLUT_RGBA
+ glutInitWindowPosition (10, 10);
+ glutInitWindowSize (window_width_, window_height_);
+ glutCreateWindow ("OpenGL range likelihood");
+
+ GLenum err = glewInit ();
+ if (GLEW_OK != err)
+ {
+ std::cerr << "Error: " << glewGetErrorString (err) << std::endl;
+ exit (-1);
+ }
+
+ std::cout << "Status: Using GLEW " << glewGetString (GLEW_VERSION) << std::endl;
+
+ if (glewIsSupported ("GL_VERSION_2_0"))
+ std::cout << "OpenGL 2.0 supported" << std::endl;
+ else
+ {
+ std::cerr << "Error: OpenGL 2.0 not supported" << std::endl;
+ exit(1);
+ }
+ std::cout << "GL_MAX_VIEWPORTS: " << GL_MAX_VIEWPORTS << std::endl;
+
+ camera_ = Camera::Ptr (new Camera ());
+ scene_ = Scene::Ptr (new Scene ());
+
+ //range_likelihood_ = RangeLikelihoodGLSL::Ptr(new RangeLikelihoodGLSL(1, 1, height, width, scene_, 0));
+
+ range_likelihood_ = RangeLikelihood::Ptr (new RangeLikelihood (2, 2, height/2, width/2, scene_));
+ // range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96, 96, scene_));
+ // range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, 480, 640, scene_));
+
+ // Actually corresponds to default parameters:
+ range_likelihood_->setCameraIntrinsicsParameters (640,480, 576.09757860,
+ 576.09757860, 321.06398107, 242.97676897);
+ range_likelihood_->setComputeOnCPU (false);
+ range_likelihood_->setSumOnCPU (true);
+ range_likelihood_->setUseColor (true);
+
+ initialize (argc, argv);
+
+ glutReshapeFunc (on_reshape);
+ glutDisplayFunc (display);
+ glutIdleFunc (display);
+ glutKeyboardFunc (on_keyboard);
+ glutMouseFunc (on_mouse);
+ glutMotionFunc (on_motion);
+ glutPassiveMotionFunc (on_passive_motion);
+ glutEntryFunc (on_entry);
+ glutMainLoop ();
+
+ return 0;
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2009-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: pcd_viewer.cpp 5094 2012-03-15 01:03:51Z rusu $
+ *
+ */
+#include <Eigen/Geometry>
+#include <Eigen/Dense>
+#include <cmath>
+#include <iostream>
+#include <boost/shared_ptr.hpp>
+#ifdef _WIN32
+# define WIN32_LEAN_AND_MEAN
+# include <windows.h>
+#endif
+
+#include <GL/glew.h>
+
+#include <pcl/pcl_config.h>
+#ifdef OPENGL_IS_A_FRAMEWORK
+# include <OpenGL/gl.h>
+#else
+# include <GL/gl.h>
+#endif
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+
+#include "pcl/common/common.h"
+#include "pcl/common/transforms.h"
+
+#include <pcl/point_types.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/surface/gp3.h>
+
+// define the following in order to eliminate the deprecated headers warning
+#define VTK_EXCLUDE_STRSTREAM_HEADERS
+#include <pcl/io/vtk_lib_io.h>
+
+#include "pcl/simulation/camera.h"
+#include "pcl/simulation/model.h"
+#include "pcl/simulation/scene.h"
+#include "pcl/simulation/range_likelihood.h"
+
+#include <pcl/console/print.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/time.h>
+
+// RangeImage:
+#include <pcl/range_image/range_image_planar.h>
+
+// Pop-up viewer
+#include <pcl/visualization/cloud_viewer.h>
+#include <boost/thread/thread.hpp>
+
+
+#include <pcl/common/common.h>
+#include <pcl/io/pcd_io.h>
+#include <cfloat>
+#include <pcl/visualization/point_cloud_handlers.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/visualization/histogram_visualizer.h>
+#include <pcl/visualization/point_picking_event.h>
+#include <pcl/console/print.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/time.h>
+#include <vtkPolyDataReader.h>
+#include <pcl/visualization/keyboard_event.h>
+
+using namespace Eigen;
+using namespace pcl;
+using namespace pcl::console;
+using namespace pcl::io;
+using namespace pcl::simulation;
+
+using namespace std;
+
+typedef pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2> ColorHandler;
+typedef ColorHandler::Ptr ColorHandlerPtr;
+typedef ColorHandler::ConstPtr ColorHandlerConstPtr;
+
+typedef pcl::visualization::PointCloudGeometryHandler<pcl::PCLPointCloud2> GeometryHandler;
+typedef GeometryHandler::Ptr GeometryHandlerPtr;
+typedef GeometryHandler::ConstPtr GeometryHandlerConstPtr;
+
+#define NORMALS_SCALE 0.01
+#define PC_SCALE 0.001
+
+uint16_t t_gamma[2048];
+Scene::Ptr scene_;
+Camera::Ptr camera_;
+RangeLikelihood::Ptr range_likelihood_;
+int window_width_;
+int window_height_;
+bool paused_;
+bool write_file_;
+
+bool
+isValidFieldName (const std::string &field)
+{
+ if (field == "_")
+ return (false);
+
+ if ((field == "vp_x") || (field == "vx") || (field == "vp_y") || (field == "vy") || (field == "vp_z") || (field == "vz"))
+ return (false);
+ return (true);
+}
+
+bool
+isMultiDimensionalFeatureField (const pcl::PCLPointField &field)
+{
+ if (field.count > 1)
+ return (true);
+ return (false);
+}
+
+void
+printHelp (int, char **argv)
+{
+ print_error ("Syntax is: %s <file_name 1..N>.<pcd or vtk> <options>\n", argv[0]);
+ print_info ("pcl::simulation viewer\n");
+ print_info (" where options are:\n");
+ print_info (" -bc r,g,b = background color\n");
+ print_info (" -fc r,g,b = foreground color\n");
+ print_info (" -ps X = point size ("); print_value ("1..64"); print_info (") \n");
+ print_info (" -opaque X = rendered point cloud opacity ("); print_value ("0..1"); print_info (")\n");
+
+ print_info (" -ax "); print_value ("n"); print_info (" = enable on-screen display of ");
+ print_color (stdout, TT_BRIGHT, TT_RED, "X"); print_color (stdout, TT_BRIGHT, TT_GREEN, "Y"); print_color (stdout, TT_BRIGHT, TT_BLUE, "Z");
+ print_info (" axes and scale them to "); print_value ("n\n");
+ print_info (" -ax_pos X,Y,Z = if axes are enabled, set their X,Y,Z position in space (default "); print_value ("0,0,0"); print_info (")\n");
+
+ print_info ("\n");
+ print_info (" -cam (*) = use given camera settings as initial view\n");
+ print_info (stderr, " (*) [Clipping Range / Focal Point / Position / ViewUp / Distance / Field of View Y / Window Size / Window Pos] or use a <filename.cam> that contains the same information.\n");
+
+ print_info ("\n");
+ print_info (" -multiview 0/1 = enable/disable auto-multi viewport rendering (default "); print_value ("disabled"); print_info (")\n");
+ print_info ("\n");
+
+ print_info ("\n");
+ print_info (" -normals 0/X = disable/enable the display of every Xth point's surface normal as lines (default "); print_value ("disabled"); print_info (")\n");
+ print_info (" -normals_scale X = resize the normal unit vector size to X (default "); print_value ("0.02"); print_info (")\n");
+ print_info ("\n");
+ print_info (" -pc 0/X = disable/enable the display of every Xth point's principal curvatures as lines (default "); print_value ("disabled"); print_info (")\n");
+ print_info (" -pc_scale X = resize the principal curvatures vectors size to X (default "); print_value ("0.02"); print_info (")\n");
+ print_info ("\n");
+
+ print_info ("\n(Note: for multiple .pcd files, provide multiple -{fc,ps,opaque} parameters; they will be automatically assigned to the right file)\n");
+}
+
+// Global visualizer object
+pcl::visualization::PCLHistogramVisualizer ph_global;
+boost::shared_ptr<pcl::visualization::PCLVisualizer> p;
+
+void
+pp_callback (const pcl::visualization::PointPickingEvent& event, void* cookie)
+{
+ if (event.getPointIndex () == -1)
+ return;
+ pcl::PCLPointCloud2::Ptr cloud = *static_cast<pcl::PCLPointCloud2::Ptr*>(cookie);
+ if (!cloud)
+ return;
+
+ // If two points were selected, draw an arrow between them
+ pcl::PointXYZ p1, p2;
+ if (event.getPoints (p1.x, p1.y, p1.z, p2.x, p2.y, p2.z) && p)
+ {
+ std::stringstream ss;
+ ss << p1 << p2;
+ p->addArrow<pcl::PointXYZ, pcl::PointXYZ> (p1, p2, 1.0, 1.0, 1.0, ss.str ());
+ return;
+ }
+
+ // Else, if a single point has been selected
+ std::stringstream ss;
+ ss << event.getPointIndex ();
+ // Get the cloud's fields
+ for (size_t i = 0; i < cloud->fields.size (); ++i)
+ {
+ if (!isMultiDimensionalFeatureField (cloud->fields[i]))
+ continue;
+ ph_global.addFeatureHistogram (*cloud, cloud->fields[i].name, event.getPointIndex (), ss.str ());
+ }
+ if (p)
+ {
+ pcl::PointXYZ pos;
+ event.getPoint (pos.x, pos.y, pos.z);
+ p->addText3D<pcl::PointXYZ> (ss.str (), pos, 0.0005, 1.0, 1.0, 1.0, ss.str ());
+ }
+ ph_global.spinOnce ();
+}
+
+
+/*
+void write_score_image(const float* score_buffer)
+{
+ int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
+ uint8_t* score_img = new uint8_t[npixels * 3];
+
+ float min_score = score_buffer[0];
+ float max_score = score_buffer[0];
+ for (int i=1; i<npixels; i++)
+ {
+ if (score_buffer[i] < min_score) min_score = score_buffer[i];
+ if (score_buffer[i] > max_score) max_score = score_buffer[i];
+ }
+
+ for (int y = 0; y < 480; ++y)
+ {
+ for (int x = 0; x < 640; ++x)
+ {
+ int i = y*640 + x ;
+ int i_in= (480-1 -y) *640 + x ; // flip up
+
+ float d = (score_buffer[i_in]-min_score)/(max_score-min_score);
+ score_img[3*i+0] = 0;
+ score_img[3*i+1] = d*255;
+ score_img[3*i+2] = 0;
+ }
+ }
+
+ // Write to file:
+ IplImage *cv_ipl = cvCreateImage( cvSize(640 ,480), 8, 3);
+ cv::Mat cv_mat(cv_ipl);
+ cv_mat.data = score_img;
+ cv::imwrite("score_image.png", cv_mat);
+
+ delete [] score_img;
+}
+
+void write_depth_image(const float* depth_buffer)
+{
+ int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
+ uint8_t* depth_img = new uint8_t[npixels * 3];
+
+ float min_depth = depth_buffer[0];
+ float max_depth = depth_buffer[0];
+ for (int i=1; i<npixels; i++)
+ {
+ if (depth_buffer[i] < min_depth) min_depth = depth_buffer[i];
+ if (depth_buffer[i] > max_depth) max_depth = depth_buffer[i];
+ }
+
+ for (int y = 0; y < 480; ++y)
+ {
+ for (int x = 0; x < 640; ++x)
+ {
+ int i= y*640 + x ;
+ int i_in= (480-1 -y) *640 + x ; // flip up down
+
+
+ float zn = 0.7;
+ float zf = 20.0;
+ float d = depth_buffer[i_in];
+ float z = -zf*zn/((zf-zn)*(d - zf/(zf-zn)));
+ float b = 0.075;
+ float f = 580.0;
+ uint16_t kd = static_cast<uint16_t>(1090 - b*f/z*8);
+ if (kd < 0) kd = 0;
+ else if (kd>2047) kd = 2047;
+
+ int pval = t_gamma[kd];
+ int lb = pval & 0xff;
+ switch (pval>>8) {
+ case 0:
+ depth_img[3*i+2] = 255;
+ depth_img[3*i+1] = 255-lb;
+ depth_img[3*i+0] = 255-lb;
+ break;
+ case 1:
+ depth_img[3*i+2] = 255;
+ depth_img[3*i+1] = lb;
+ depth_img[3*i+0] = 0;
+ break;
+ case 2:
+ depth_img[3*i+2] = 255-lb;
+ depth_img[3*i+1] = 255;
+ depth_img[3*i+0] = 0;
+ break;
+ case 3:
+ depth_img[3*i+2] = 0;
+ depth_img[3*i+1] = 255;
+ depth_img[3*i+0] = lb;
+ break;
+ case 4:
+ depth_img[3*i+2] = 0;
+ depth_img[3*i+1] = 255-lb;
+ depth_img[3*i+0] = 255;
+ break;
+ case 5:
+ depth_img[3*i+2] = 0;
+ depth_img[3*i+1] = 0;
+ depth_img[3*i+0] = 255-lb;
+ break;
+ default:
+ depth_img[3*i+2] = 0;
+ depth_img[3*i+1] = 0;
+ depth_img[3*i+0] = 0;
+ break;
+ }
+ }
+ }
+
+ // Write to file:
+ IplImage *cv_ipl = cvCreateImage( cvSize(640 ,480), 8, 3);
+ cv::Mat cv_mat(cv_ipl);
+ cv_mat.data = depth_img;
+ cv::imwrite("depth_image.png", cv_mat);
+
+ delete [] depth_img;
+}
+
+
+void write_rgb_image(const uint8_t* rgb_buffer)
+{
+ int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
+ uint8_t* rgb_img = new uint8_t[npixels * 3];
+
+ for (int y = 0; y < 480; ++y)
+ {
+ for (int x = 0; x < 640; ++x)
+ {
+ int px = y*640 + x ;
+ rgb_img [3* (px) +0] = rgb_buffer[3*px+0];
+ rgb_img [3* (px) +1] = rgb_buffer[3*px+1];
+ rgb_img [3* (px) +2] = rgb_buffer[3*px+2];
+ }
+ }
+
+ // Write to file:
+ IplImage *cv_ipl = cvCreateImage( cvSize(640 ,480), 8, 3);
+ cv::Mat cv_mat(cv_ipl);
+ cv_mat.data = rgb_img ;
+ cv::imwrite("rgb_image.png", cv_mat);
+
+ delete [] rgb_img;
+}
+
+
+boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis (pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
+{
+ // Snippet taken from PCLVisualizer tutorial:
+ boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
+ viewer->setBackgroundColor (0, 0, 0);
+ pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
+ viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
+ viewer->addCoordinateSystem (1.0);
+ viewer->initCameraParameters ();
+
+ //viewer->addModelFromPLYFile("/home/mfallon/projects/kmcl/kmcl/models/table_scene/meta_model.ply");
+
+
+ return (viewer);
+}
+*/
+
+void capture (Eigen::Isometry3d pose_in, string point_cloud_fname)
+{
+ // No reference image - but this is kept for compatability with range_test_v2:
+ float* reference = new float[range_likelihood_->getRowHeight() * range_likelihood_->getColWidth()];
+ const float* depth_buffer = range_likelihood_->getDepthBuffer();
+ // Copy one image from our last as a reference.
+ for (int i=0, n=0; i<range_likelihood_->getRowHeight(); ++i)
+ {
+ for (int j=0; j<range_likelihood_->getColWidth(); ++j)
+ {
+ reference[n++] = depth_buffer[i*range_likelihood_->getWidth() + j];
+ }
+ }
+
+ std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
+ std::vector<float> scores;
+ poses.push_back (pose_in);
+
+ range_likelihood_->computeLikelihoods (reference, poses, scores);
+ std::cout << "score: ";
+ for (size_t i = 0; i<scores.size (); ++i)
+ {
+ std::cout << " " << scores[i];
+ }
+ std::cout << std::endl;
+
+ std::cout << "camera: " << camera_->getX ()
+ << " " << camera_->getY ()
+ << " " << camera_->getZ ()
+ << " " << camera_->getRoll ()
+ << " " << camera_->getPitch ()
+ << " " << camera_->getYaw ()
+ << std::endl;
+
+ delete [] reference;
+
+
+ // Benchmark Values for
+ // 27840 triangle faces
+ // 13670 vertices
+
+ // 45.00Hz: simuation only
+ // 1.28Hz: simuation, addNoise? , getPointCloud, writeASCII
+ // 33.33Hz: simuation, getPointCloud
+ // 23.81Hz: simuation, getPointCloud, writeBinary
+ // 14.28Hz: simuation, addNoise, getPointCloud, writeBinary
+ // MODULE TIME FRACTION
+ // simuation 0.02222 31%
+ // addNoise 0.03 41%
+ // getPointCloud 0.008 11%
+ // writeBinary 0.012 16%
+ // total 0.07222
+
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out (new pcl::PointCloud<pcl::PointXYZRGB>);
+ bool write_cloud = true;
+
+ if (write_cloud)
+ {
+ // Read Color Buffer from the GPU before creating PointCloud:
+ // By default the buffers are not read back from the GPU
+ range_likelihood_->getColorBuffer ();
+ range_likelihood_->getDepthBuffer ();
+
+ // Add noise directly to the CPU depth buffer
+ range_likelihood_->addNoise ();
+
+ // Optional argument to save point cloud in global frame:
+ // Save camera relative:
+ //range_likelihood_->getPointCloud(pc_out);
+ // Save in global frame - applying the camera frame:
+ //range_likelihood_->getPointCloud(pc_out,true,camera_->pose());
+ // Save in local frame
+ range_likelihood_->getPointCloud (pc_out,false,camera_->getPose ());
+ // TODO: what to do when there are more than one simulated view?
+ std::cout << pc_out->points.size() << " points written to file\n";
+
+ pcl::PCDWriter writer;
+ //writer.write (point_cloud_fname, *pc_out, false); /// ASCII
+ writer.writeBinary (point_cloud_fname, *pc_out);
+ //cout << "finished writing file\n";
+ }
+ // Disabled all OpenCV stuff for now: dont want the dependency
+ /*
+ bool demo_other_stuff = false;
+ if (demo_other_stuff && write_cloud)
+ {
+ write_score_image (range_likelihood_->getScoreBuffer ());
+ write_rgb_image (range_likelihood_->getColorBuffer ());
+ write_depth_image (range_likelihood_->getDepthBuffer ());
+
+ // Demo interacton with RangeImage:
+ pcl::RangeImagePlanar rangeImage;
+ range_likelihood_->getRangeImagePlanar (rangeImage);
+
+ // display viewer: (currently seqfaults on exit of viewer)
+ if (1==0){
+ boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+ viewer = simpleVis(pc_out);
+
+ while (!viewer->wasStopped ()){
+ viewer->spinOnce (100);
+ boost::this_thread::sleep (boost::posix_time::microseconds (100000));
+ }
+ }
+ }
+ */
+}
+
+
+
+
+
+
+void print_Quaterniond(Eigen::Quaterniond r, std::stringstream &ss){
+ ss <<r.w()<<", "<<r.x()<<", "<<r.y()<<", "<<r.z() ;
+ // std::cout << r.str() << "q\n";
+}
+
+
+// Normalize angle to be within the interval [-pi,pi].
+double standardRad(double t) {
+ if (t >= 0.) {
+ t = fmod(t+M_PI, 2*M_PI) - M_PI;
+ } else {
+ t = fmod(t-M_PI, -2*M_PI) + M_PI;
+ }
+ return t;
+}
+
+void wRo_to_euler(const Eigen::Matrix3f& wRo, double& yaw, double& pitch, double& roll) {
+ yaw = standardRad(atan2(wRo(1,0), wRo(0,0)));
+ double c = cos(yaw);
+ double s = sin(yaw);
+ pitch = standardRad(atan2(static_cast<double> (-wRo(2,0)), wRo(0,0)*c + wRo(1,0)*s));
+ roll = standardRad(atan2(wRo(0,2)*s - wRo(1,2)*c, -wRo(0,1)*s + wRo(1,1)*c));
+}
+
+void print_Isometry3d(Eigen::Isometry3d pose, std::stringstream &ss){
+ Eigen::Vector3d t(pose.translation());
+ Eigen::Quaterniond r(pose.rotation());
+ ss <<t[0]<<", "<<t[1]<<", "<<t[2]<<" | "
+ <<r.w()<<", "<<r.x()<<", "<<r.y()<<", "<<r.z() ;
+ // std::cout << ss.str() << "q\n";
+}
+
+
+
+void simulate_callback (const pcl::visualization::KeyboardEvent &event,
+ void* viewer_void)
+{
+ boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = *static_cast<boost::shared_ptr<pcl::visualization::PCLVisualizer> *> (viewer_void);
+ // I choose v for virtual as s for simulate is takwen
+ if (event.getKeySym () == "v" && event.keyDown ())
+ {
+ std::cout << "v was pressed => simulate cloud" << std::endl;
+
+ std::vector<pcl::visualization::Camera> cams;
+ viewer->getCameras(cams);
+
+ if (cams.size() !=1){
+ std::cout << "n cams not 1 exiting\n"; // for now in case ...
+ return;
+ }
+ // cout << "n cams: " << cams.size() << "\n";
+ pcl::visualization::Camera cam = cams[0];
+
+
+
+
+ Eigen::Affine3f pose;
+ pose = viewer->getViewerPose() ;
+
+
+ std::cout << cam.pos[0] << " "
+ << cam.pos[1] << " "
+ << cam.pos[2] << " p\n";
+
+ Eigen::Matrix3f m;
+ m =pose.rotation();
+ //All axies use right hand rule. x=red axis, y=green axis, z=blue axis z direction is point into the screen. z \ \ \ -----------> x | | | | | | y
+
+ cout << pose(0,0) << " " << pose(0,1) << " " << pose(0,2) << " " << pose(0,3) << " x0\n";
+ cout << pose(1,0) << " " << pose(1,1) << " " << pose(1,2) << " " << pose(1,3) << " x1\n";
+ cout << pose(2,0) << " " << pose(2,1) << " " << pose(2,2) << " " << pose(2,3)<< "x2\n";
+
+ double yaw,pitch, roll;
+ wRo_to_euler(m,yaw,pitch,roll);
+
+ printf("RPY: %f %f %f\n", roll*180/M_PI,pitch*180/M_PI,yaw*180/M_PI);
+
+// matrix->GetElement(1,0);
+
+ cout << m(0,0) << " " << m(0,1) << " " << m(0,2) << " " << " x0\n";
+ cout << m(1,0) << " " << m(1,1) << " " << m(1,2) << " " << " x1\n";
+ cout << m(2,0) << " " << m(2,1) << " " << m(2,2) << " "<< "x2\n\n";
+
+ Eigen::Quaternionf rf;
+ rf = Eigen::Quaternionf(m);
+
+
+ Eigen::Quaterniond r(rf.w(),rf.x(),rf.y(),rf.z());
+
+ Eigen::Isometry3d init_pose;
+ init_pose.setIdentity();
+ init_pose.translation() << cam.pos[0], cam.pos[1], cam.pos[2];
+ //Eigen::Quaterniond m = euler_to_quat(-1.54, 0, 0);
+ init_pose.rotate(r);
+//
+
+ std::stringstream ss;
+ print_Isometry3d(init_pose,ss);
+ std::cout << "init_pose: " << ss.str() << "\n";
+
+
+
+
+ viewer->addCoordinateSystem (1.0,pose,"reference");
+
+
+
+ double tic = getTime();
+ std::stringstream ss2;
+ ss2.precision(20);
+ ss2 << "simulated_pcl_" << tic << ".pcd";
+ capture(init_pose,ss2.str());
+ cout << (getTime() -tic) << " sec\n";
+
+
+
+ // these three variables determine the position and orientation of
+ // the camera.
+
+
+// double lookat[3]; - focal location
+// double eye[3]; - my location
+// double up[3]; - updirection
+
+
+
+// std::cout << view[0] << "," << view[1] << "," << view[2]
+
+// cameras.back ().view[2] = renderer->GetActiveCamera ()->GetOrientationWXYZ()[2];
+
+//ViewTransform->GetOrientationWXYZ();
+
+ // Superclass::OnKeyUp ();
+
+// vtkSmartPointer<vtkCamera> cam = event.Interactor->GetRenderWindow ()->GetRenderers ()->GetFirstRenderer ()->GetActiveCamera ();
+// double clip[2], focal[3], pos[3], view[3];
+// cam->GetClippingRange (clip);
+/* cam->GetFocalPoint (focal);
+ cam->GetPosition (pos);
+ cam->GetViewUp (view);
+ int *win_pos = Interactor->GetRenderWindow ()->GetPosition ();
+ int *win_size = Interactor->GetRenderWindow ()->GetSize ();
+ std::cerr << clip[0] << "," << clip[1] << "/" << focal[0] << "," << focal[1] << "," << focal[2] << "/" <<
+ pos[0] << "," << pos[1] << "," << pos[2] << "/" << view[0] << "," << view[1] << "," << view[2] << "/" <<
+ cam->GetViewAngle () / 180.0 * M_PI << "/" << win_size[0] << "," << win_size[1] << "/" << win_pos[0] << "," << win_pos[1]
+ << endl;
+ */
+ }
+}
+
+/* ---[ */
+
+// Read in a 3D model
+void
+loadPolygonMeshModel (char* polygon_file)
+{
+ pcl::PolygonMesh mesh; // (new pcl::PolygonMesh);
+ //pcl::io::loadPolygonFile("/home/mfallon/data/models/dalet/Darlek_modified_works.obj",mesh);
+ pcl::io::loadPolygonFile (polygon_file, mesh);
+ pcl::PolygonMesh::Ptr cloud (new pcl::PolygonMesh (mesh));
+
+ // Not sure if PolygonMesh assumes triangles if to
+ // TODO: Ask a developer
+ //PolygonMeshModel::Ptr model = PolygonMeshModel::Ptr (new PolygonMeshModel (GL_POLYGON, cloud));
+ TriangleMeshModel::Ptr model = TriangleMeshModel::Ptr (new TriangleMeshModel (cloud));
+ scene_->add (model);
+
+ std::cout << "Just read " << polygon_file << std::endl;
+ std::cout << mesh.polygons.size () << " polygons and "
+ << mesh.cloud.data.size () << " triangles\n";
+}
+
+void
+initialize (int, char** argv)
+{
+ const GLubyte* version = glGetString (GL_VERSION);
+ std::cout << "OpenGL Version: " << version << std::endl;
+
+ // works for small files:
+ camera_->set(-5.0, 0.0, 1.0, 0.0, 0.0, 0.0);
+ pcl::console::print_info("About to read: %s", argv[2]);
+ loadPolygonMeshModel (argv[2]);
+}
+
+int
+main (int argc, char** argv)
+{
+ srand (time (0));
+
+ print_info ("The viewer window provides interactive commands; for help, press 'h' or 'H' from within the window.\n");
+
+ if (argc < 2)
+ {
+ printHelp (argc, argv);
+ return (-1);
+ }
+
+ // Command line parsing
+ double bcolor[3] = {0, 0, 0};
+ pcl::console::parse_3x_arguments (argc, argv, "-bc", bcolor[0], bcolor[1], bcolor[2]);
+
+ std::vector<double> fcolor_r, fcolor_b, fcolor_g;
+ bool fcolorparam = pcl::console::parse_multiple_3x_arguments (argc, argv, "-fc", fcolor_r, fcolor_g, fcolor_b);
+
+ std::vector<int> psize;
+ pcl::console::parse_multiple_arguments (argc, argv, "-ps", psize);
+
+ std::vector<double> opaque;
+ pcl::console::parse_multiple_arguments (argc, argv, "-opaque", opaque);
+
+ int mview = 0;
+ pcl::console::parse_argument (argc, argv, "-multiview", mview);
+
+ int normals = 0;
+ pcl::console::parse_argument (argc, argv, "-normals", normals);
+ double normals_scale = NORMALS_SCALE;
+ pcl::console::parse_argument (argc, argv, "-normals_scale", normals_scale);
+
+ int pc = 0;
+ pcl::console::parse_argument (argc, argv, "-pc", pc);
+ double pc_scale = PC_SCALE;
+ pcl::console::parse_argument (argc, argv, "-pc_scale", pc_scale);
+
+ // Parse the command line arguments for .pcd files
+ std::vector<int> p_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
+ std::vector<int> vtk_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".vtk");
+
+ if (p_file_indices.size () == 0 && vtk_file_indices.size () == 0)
+ {
+ print_error ("No .PCD or .VTK file given. Nothing to visualize.\n");
+ return (-1);
+ }
+
+ // Multiview enabled?
+ int y_s = 0, x_s = 0;
+ double x_step = 0, y_step = 0;
+ if (mview)
+ {
+ print_highlight ("Multi-viewport rendering enabled.\n");
+
+ if (p_file_indices.size () != 0)
+ {
+ y_s = static_cast<int>(floor (sqrt (static_cast<float>(p_file_indices.size ()))));
+ x_s = y_s + static_cast<int>(ceil ((p_file_indices.size () / static_cast<double>(y_s)) - y_s));
+ print_highlight ("Preparing to load "); print_value ("%d", p_file_indices.size ());
+ }
+ else if (vtk_file_indices.size () != 0)
+ {
+ y_s = static_cast<int>(floor (sqrt (static_cast<float>(vtk_file_indices.size ()))));
+ x_s = y_s + static_cast<int>(ceil ((vtk_file_indices.size () / static_cast<double>(y_s)) - y_s));
+ print_highlight ("Preparing to load "); print_value ("%d", vtk_file_indices.size ());
+ }
+
+ x_step = static_cast<double>(1.0 / static_cast<double>(x_s));
+ y_step = static_cast<double>(1.0 / static_cast<double>(y_s));
+ print_info (" files ("); print_value ("%d", x_s); print_info ("x"); print_value ("%d", y_s);
+ print_info (" / "); print_value ("%f", x_step); print_info ("x"); print_value ("%f", y_step);
+ print_info (")\n");
+ }
+
+ // Fix invalid multiple arguments
+ if (psize.size () != p_file_indices.size () && psize.size () > 0)
+ for (size_t i = psize.size (); i < p_file_indices.size (); ++i)
+ psize.push_back (1);
+ if (opaque.size () != p_file_indices.size () && opaque.size () > 0)
+ for (size_t i = opaque.size (); i < p_file_indices.size (); ++i)
+ opaque.push_back (1.0);
+
+ // Create the PCLVisualizer object
+ boost::shared_ptr<pcl::visualization::PCLHistogramVisualizer> ph;
+
+ // Using min_p, max_p to set the global Y min/max range for the histogram
+ float min_p = FLT_MAX; float max_p = -FLT_MAX;
+
+ int k = 0, l = 0, viewport = 0;
+ // Load the data files
+ pcl::PCDReader pcd;
+ pcl::console::TicToc tt;
+ ColorHandlerPtr color_handler;
+ GeometryHandlerPtr geometry_handler;
+
+ // Go through VTK files
+ for (size_t i = 0; i < vtk_file_indices.size (); ++i)
+ {
+ // Load file
+ tt.tic ();
+ print_highlight (stderr, "Loading "); print_value (stderr, "%s ", argv[vtk_file_indices.at (i)]);
+ vtkPolyDataReader* reader = vtkPolyDataReader::New ();
+ reader->SetFileName (argv[vtk_file_indices.at (i)]);
+ reader->Update ();
+ vtkSmartPointer<vtkPolyData> polydata = reader->GetOutput ();
+ if (!polydata)
+ return (-1);
+ print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", polydata->GetNumberOfPoints ()); print_info (" points]\n");
+
+ // Create the PCLVisualizer object here on the first encountered XYZ file
+ if (!p)
+ p.reset (new pcl::visualization::PCLVisualizer (argc, argv, "PCD viewer"));
+
+ // Multiview enabled?
+ if (mview)
+ {
+ p->createViewPort (k * x_step, l * y_step, (k + 1) * x_step, (l + 1) * y_step, viewport);
+ k++;
+ if (k >= x_s)
+ {
+ k = 0;
+ l++;
+ }
+ }
+
+ // Add as actor
+ std::stringstream cloud_name ("vtk-");
+ cloud_name << argv[vtk_file_indices.at (i)] << "-" << i;
+ p->addModelFromPolyData (polydata, cloud_name.str (), viewport);
+
+ // Change the shape rendered color
+ if (fcolorparam && fcolor_r.size () > i && fcolor_g.size () > i && fcolor_b.size () > i)
+ p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, fcolor_r[i], fcolor_g[i], fcolor_b[i], cloud_name.str ());
+
+ // Change the shape rendered point size
+ if (psize.size () > 0)
+ p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at (i), cloud_name.str ());
+
+ // Change the shape rendered opacity
+ if (opaque.size () > 0)
+ p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at (i), cloud_name.str ());
+ }
+
+ pcl::PCLPointCloud2::Ptr cloud;
+ // Go through PCD files
+ for (size_t i = 0; i < p_file_indices.size (); ++i)
+ {
+ cloud.reset (new pcl::PCLPointCloud2);
+ Eigen::Vector4f origin;
+ Eigen::Quaternionf orientation;
+ int version;
+
+ print_highlight (stderr, "Loading "); print_value (stderr, "%s ", argv[p_file_indices.at (i)]);
+
+ tt.tic ();
+ if (pcd.read (argv[p_file_indices.at (i)], *cloud, origin, orientation, version) < 0)
+ return (-1);
+
+ std::stringstream cloud_name;
+
+ // ---[ Special check for 1-point multi-dimension histograms
+ if (cloud->fields.size () == 1 && isMultiDimensionalFeatureField (cloud->fields[0]))
+ {
+ cloud_name << argv[p_file_indices.at (i)];
+
+ if (!ph)
+ ph.reset (new pcl::visualization::PCLHistogramVisualizer);
+ print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud->fields[0].count); print_info (" points]\n");
+
+ pcl::getMinMax (*cloud, 0, cloud->fields[0].name, min_p, max_p);
+ ph->addFeatureHistogram (*cloud, cloud->fields[0].name, cloud_name.str ());
+ continue;
+ }
+
+ cloud_name << argv[p_file_indices.at (i)] << "-" << i;
+
+ // Create the PCLVisualizer object here on the first encountered XYZ file
+ if (!p)
+ {
+ p.reset (new pcl::visualization::PCLVisualizer (argc, argv, "PCD viewer"));
+ p->registerPointPickingCallback (&pp_callback, (void*)&cloud);
+ Eigen::Matrix3f rotation;
+ rotation = orientation;
+ p->setCameraPosition (origin [0] , origin [1] , origin [2],
+ origin [0] + rotation (0, 2), origin [1] + rotation (1, 2), origin [2] + rotation (2, 2),
+ rotation (0, 1), rotation (1, 1), rotation (2, 1));
+ }
+
+ // Multiview enabled?
+ if (mview)
+ {
+ p->createViewPort (k * x_step, l * y_step, (k + 1) * x_step, (l + 1) * y_step, viewport);
+ k++;
+ if (k >= x_s)
+ {
+ k = 0;
+ l++;
+ }
+ }
+
+ if (cloud->width * cloud->height == 0)
+ {
+ print_error ("[error: no points found!]\n");
+ return (-1);
+ }
+ print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", (int)cloud->width * cloud->height); print_info (" points]\n");
+ print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (*cloud).c_str ());
+
+ // If no color was given, get random colors
+ if (fcolorparam)
+ {
+ if (fcolor_r.size () > i && fcolor_g.size () > i && fcolor_b.size () > i)
+ color_handler.reset (new pcl::visualization::PointCloudColorHandlerCustom<pcl::PCLPointCloud2> (cloud, fcolor_r[i], fcolor_g[i], fcolor_b[i]));
+ else
+ color_handler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (cloud));
+ }
+ else
+ color_handler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (cloud));
+
+ // Add the dataset with a XYZ and a random handler
+ geometry_handler.reset (new pcl::visualization::PointCloudGeometryHandlerXYZ<pcl::PCLPointCloud2> (cloud));
+ // Add the cloud to the renderer
+ //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, color_handler, cloud_name.str (), viewport);
+ p->addPointCloud (cloud, geometry_handler, color_handler, origin, orientation, cloud_name.str (), viewport);
+
+ // If normal lines are enabled
+ if (normals != 0)
+ {
+ int normal_idx = pcl::getFieldIndex (*cloud, "normal_x");
+ if (normal_idx == -1)
+ {
+ print_error ("Normal information requested but not available.\n");
+ continue;
+ //return (-1);
+ }
+ //
+ // Convert from blob to pcl::PointCloud
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::fromPCLPointCloud2 (*cloud, *cloud_xyz);
+ cloud_xyz->sensor_origin_ = origin;
+ cloud_xyz->sensor_orientation_ = orientation;
+
+ pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
+ pcl::fromPCLPointCloud2 (*cloud, *cloud_normals);
+ std::stringstream cloud_name_normals;
+ cloud_name_normals << argv[p_file_indices.at (i)] << "-" << i << "-normals";
+ p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, normals, normals_scale, cloud_name_normals.str (), viewport);
+ }
+
+ // If principal curvature lines are enabled
+ if (pc != 0)
+ {
+ if (normals == 0)
+ normals = pc;
+
+ int normal_idx = pcl::getFieldIndex (*cloud, "normal_x");
+ if (normal_idx == -1)
+ {
+ print_error ("Normal information requested but not available.\n");
+ continue;
+ //return (-1);
+ }
+ int pc_idx = pcl::getFieldIndex (*cloud, "principal_curvature_x");
+ if (pc_idx == -1)
+ {
+ print_error ("Principal Curvature information requested but not available.\n");
+ continue;
+ //return (-1);
+ }
+ //
+ // Convert from blob to pcl::PointCloud
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::fromPCLPointCloud2 (*cloud, *cloud_xyz);
+ cloud_xyz->sensor_origin_ = origin;
+ cloud_xyz->sensor_orientation_ = orientation;
+ pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
+ pcl::fromPCLPointCloud2 (*cloud, *cloud_normals);
+ pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr cloud_pc (new pcl::PointCloud<pcl::PrincipalCurvatures>);
+ pcl::fromPCLPointCloud2 (*cloud, *cloud_pc);
+ std::stringstream cloud_name_normals_pc;
+ cloud_name_normals_pc << argv[p_file_indices.at (i)] << "-" << i << "-normals";
+ int factor = (std::min)(normals, pc);
+ p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, factor, normals_scale, cloud_name_normals_pc.str (), viewport);
+ p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, cloud_name_normals_pc.str ());
+ p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc.str ());
+ cloud_name_normals_pc << "-pc";
+ p->addPointCloudPrincipalCurvatures<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, cloud_pc, factor, pc_scale, cloud_name_normals_pc.str (), viewport);
+ p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc.str ());
+ }
+
+ // Add every dimension as a possible color
+ if (!fcolorparam)
+ {
+ for (size_t f = 0; f < cloud->fields.size (); ++f)
+ {
+ if (cloud->fields[f].name == "rgb" || cloud->fields[f].name == "rgba")
+ color_handler.reset (new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PCLPointCloud2> (cloud));
+ else
+ {
+ if (!isValidFieldName (cloud->fields[f].name))
+ continue;
+ color_handler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (cloud, cloud->fields[f].name));
+ }
+ // Add the cloud to the renderer
+ //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, color_handler, cloud_name.str (), viewport);
+ p->addPointCloud (cloud, color_handler, origin, orientation, cloud_name.str (), viewport);
+ }
+ }
+ // Additionally, add normals as a handler
+ geometry_handler.reset (new pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<pcl::PCLPointCloud2> (cloud));
+ if (geometry_handler->isCapable ())
+ //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, cloud_name.str (), viewport);
+ p->addPointCloud (cloud, geometry_handler, origin, orientation, cloud_name.str (), viewport);
+
+ // Set immediate mode rendering ON
+ p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_IMMEDIATE_RENDERING, 1.0, cloud_name.str ());
+
+ // Change the cloud rendered point size
+ if (psize.size () > 0)
+ p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at (i), cloud_name.str ());
+
+ // Change the cloud rendered opacity
+ if (opaque.size () > 0)
+ p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at (i), cloud_name.str ());
+
+ // Reset camera viewpoint to center of cloud if camera parameters were not passed manually and this is the first loaded cloud
+ //if (i == 0 && !p->cameraParamsSet ())
+ // p->resetCameraViewpoint (cloud_name.str ());
+ }
+
+
+ ////////////////////////////////////////////////////////////////
+ // Key binding for saving simulated point cloud:
+ if (p)
+ p->registerKeyboardCallback(simulate_callback, (void*)&p);
+
+
+
+
+ int width = 640;
+ int height = 480;
+ window_width_ = width * 2;
+ window_height_ = height * 2;
+
+ print_info ("Manually generate a simulated RGB-D point cloud using pcl::simulation. For more information, use: %s -h\n", argv[0]);
+
+ for (int i=0; i<2048; i++)
+ {
+ float v = i/2048.0;
+ v = powf(v, 3)* 6;
+ t_gamma[i] = v*6*256;
+ }
+
+ GLenum err = glewInit ();
+ if (GLEW_OK != err)
+ {
+ std::cerr << "Error: " << glewGetErrorString (err) << std::endl;
+ exit (-1);
+ }
+
+ std::cout << "Status: Using GLEW " << glewGetString (GLEW_VERSION) << std::endl;
+
+ if (glewIsSupported ("GL_VERSION_2_0"))
+ std::cout << "OpenGL 2.0 supported" << std::endl;
+ else
+ {
+ std::cerr << "Error: OpenGL 2.0 not supported" << std::endl;
+ exit(1);
+ }
+
+
+ camera_ = Camera::Ptr (new Camera ());
+ scene_ = Scene::Ptr (new Scene ());
+
+ range_likelihood_ = RangeLikelihood::Ptr (new RangeLikelihood(1, 1, height, width, scene_));
+
+ // range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96, 96, scene_));
+ // range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, 480, 640, scene_));
+
+ // Actually corresponds to default parameters:
+ range_likelihood_->setCameraIntrinsicsParameters (640,480, 576.09757860,
+ 576.09757860, 321.06398107, 242.97676897);
+ range_likelihood_->setComputeOnCPU (false);
+ range_likelihood_->setSumOnCPU (true);
+ range_likelihood_->setUseColor (true);
+ initialize (argc, argv);
+
+ if (p)
+ p->setBackgroundColor (bcolor[0], bcolor[1], bcolor[2]);
+ // Read axes settings
+ double axes = 0.0;
+ pcl::console::parse_argument (argc, argv, "-ax", axes);
+ if (axes != 0.0 && p)
+ {
+ double ax_x = 0.0, ax_y = 0.0, ax_z = 0.0;
+ pcl::console::parse_3x_arguments (argc, argv, "-ax_pos", ax_x, ax_y, ax_z, false);
+ // Draw XYZ axes if command-line enabled
+ p->addCoordinateSystem (axes, ax_x, ax_y, ax_z, "reference");
+ }
+
+ // Clean up the memory used by the binary blob
+ // Note: avoid resetting the cloud, otherwise the PointPicking callback will fail
+ //cloud.reset ();
+
+ if (ph)
+ {
+ print_highlight ("Setting the global Y range for all histograms to: "); print_value ("%f -> %f\n", min_p, max_p);
+ ph->setGlobalYRange (min_p, max_p);
+ ph->updateWindowPositions ();
+ if (p)
+ p->spin ();
+ else
+ ph->spin ();
+ }
+ else if (p)
+ p->spin ();
+}
+/* ]--- */
--- /dev/null
+#include "simulation_io.hpp"
+#include <pcl/io/png_io.h>
+
+
+
+pcl::simulation::SimExample::SimExample(int argc, char** argv,
+ int height,int width):
+ height_(height), width_(width){
+
+ initializeGL (argc, argv);
+
+ // 1. construct member elements:
+ camera_ = Camera::Ptr (new Camera ());
+ scene_ = Scene::Ptr (new Scene ());
+
+ //rl_ = RangeLikelihoodGLSL::Ptr(new RangeLikelihoodGLSL(1, 1, height, width, scene_, 0));
+ rl_ = RangeLikelihood::Ptr (new RangeLikelihood (1, 1, height, width, scene_));
+ // rl_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96, 96, scene_));
+ // rl_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, height_, width_, scene_));
+
+ // Actually corresponds to default parameters:
+ rl_->setCameraIntrinsicsParameters (width_,height_, 576.09757860,
+ 576.09757860, 321.06398107, 242.97676897);
+ rl_->setComputeOnCPU (false);
+ rl_->setSumOnCPU (true);
+ rl_->setUseColor (true);
+
+ // 2. read mesh and setup model:
+ std::cout << "About to read: " << argv[2] << std::endl;
+ pcl::PolygonMesh mesh; // (new pcl::PolygonMesh);
+ pcl::io::loadPolygonFile (argv[2], mesh);
+ pcl::PolygonMesh::Ptr cloud (new pcl::PolygonMesh (mesh));
+
+ // Not sure if PolygonMesh assumes triangles if to, TODO: Ask a developer
+ PolygonMeshModel::Ptr model = PolygonMeshModel::Ptr (new PolygonMeshModel (GL_POLYGON, cloud));
+ scene_->add (model);
+
+ std::cout << "Just read " << argv[2] << std::endl;
+ std::cout << mesh.polygons.size () << " polygons and "
+ << mesh.cloud.data.size () << " triangles\n";
+
+ // works well for MIT CSAIL model 3rd floor:
+ //camera_->set(4.04454, 44.9377, 1.1, 0.0, 0.0, -2.00352);
+
+ // works well for MIT CSAIL model 2nd floor:
+// camera_->set (27.4503, 37.383, 4.30908, 0.0, 0.0654498, -2.25802);
+
+ // works for small files:
+ //camera_->set(-5.0, 0.0, 1.0, 0.0, 0.0, 0.0);
+ camera_->set(0.471703, 1.59862, 3.10937, 0, 0.418879, -12.2129);
+ camera_->setPitch(0.418879); // not sure why this is here:
+
+ for (int i=0; i<2048; i++)
+ {
+ float v = i/2048.0;
+ v = powf(v, 3)* 6;
+ t_gamma[i] = v*6*256;
+ }
+}
+
+
+
+
+void
+pcl::simulation::SimExample::initializeGL (int argc, char** argv)
+{
+ glutInit (&argc, argv);
+ glutInitDisplayMode (GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);// was GLUT_RGBA
+ glutInitWindowPosition (10, 10);
+ glutInitWindowSize (10, 10);
+ //glutInitWindowSize (window_width_, window_height_);
+ glutCreateWindow ("OpenGL range likelihood");
+
+ GLenum err = glewInit ();
+ if (GLEW_OK != err)
+ {
+ std::cerr << "Error: " << glewGetErrorString (err) << std::endl;
+ exit (-1);
+ }
+
+ std::cout << "Status: Using GLEW " << glewGetString (GLEW_VERSION) << std::endl;
+ if (glewIsSupported ("GL_VERSION_2_0"))
+ std::cout << "OpenGL 2.0 supported" << std::endl;
+ else
+ {
+ std::cerr << "Error: OpenGL 2.0 not supported" << std::endl;
+ exit(1);
+ }
+
+ std::cout << "GL_MAX_VIEWPORTS: " << GL_MAX_VIEWPORTS << std::endl;
+ const GLubyte* version = glGetString (GL_VERSION);
+ std::cout << "OpenGL Version: " << version << std::endl;
+}
+
+
+
+void
+pcl::simulation::SimExample::doSim (Eigen::Isometry3d pose_in)
+{
+ // No reference image - but this is kept for compatability with range_test_v2:
+ float* reference = new float[rl_->getRowHeight() * rl_->getColWidth()];
+ const float* depth_buffer = rl_->getDepthBuffer();
+ // Copy one image from our last as a reference.
+ for (int i=0, n=0; i<rl_->getRowHeight(); ++i)
+ {
+ for (int j=0; j<rl_->getColWidth(); ++j)
+ {
+ reference[n++] = depth_buffer[i*rl_->getWidth() + j];
+ }
+ }
+
+ std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
+ std::vector<float> scores;
+ poses.push_back (pose_in);
+ rl_->computeLikelihoods (reference, poses, scores);
+ std::cout << "camera: " << camera_->getX ()
+ << " " << camera_->getY ()
+ << " " << camera_->getZ ()
+ << " " << camera_->getRoll ()
+ << " " << camera_->getPitch ()
+ << " " << camera_->getYaw ()
+ << std::endl;
+
+ delete [] reference;
+}
+
+
+
+void
+pcl::simulation::SimExample::write_score_image(const float* score_buffer, std::string fname)
+{
+ int npixels = rl_->getWidth() * rl_->getHeight();
+ uint8_t* score_img = new uint8_t[npixels * 3];
+
+ float min_score = score_buffer[0];
+ float max_score = score_buffer[0];
+ for (int i=1; i<npixels; i++)
+ {
+ if (score_buffer[i] < min_score) min_score = score_buffer[i];
+ if (score_buffer[i] > max_score) max_score = score_buffer[i];
+ }
+
+ for (int y = 0; y < height_; ++y)
+ {
+ for (int x = 0; x < width_; ++x)
+ {
+ int i = y*width_ + x ;
+ int i_in= (height_-1 -y) *width_ + x ; // flip up
+
+ float d = (score_buffer[i_in]-min_score)/(max_score-min_score);
+ score_img[3*i+0] = 0;
+ score_img[3*i+1] = d*255;
+ score_img[3*i+2] = 0;
+ }
+ }
+
+ // Write to file:
+ pcl::io::saveRgbPNGFile (fname, score_img, width_, height_);
+
+ delete [] score_img;
+}
+
+void
+pcl::simulation::SimExample::write_depth_image(const float* depth_buffer, std::string fname)
+{
+ int npixels = rl_->getWidth() * rl_->getHeight();
+ uint8_t* depth_img = new uint8_t[npixels * 3];
+
+ float min_depth = depth_buffer[0];
+ float max_depth = depth_buffer[0];
+ for (int i=1; i<npixels; i++)
+ {
+ if (depth_buffer[i] < min_depth) min_depth = depth_buffer[i];
+ if (depth_buffer[i] > max_depth) max_depth = depth_buffer[i];
+ }
+
+ for (int y = 0; y < height_; ++y)
+ {
+ for (int x = 0; x < width_; ++x)
+ {
+ int i= y*width_ + x ;
+ int i_in= (height_-1 -y) *width_ + x ; // flip up down
+
+ float zn = 0.7;
+ float zf = 20.0;
+ float d = depth_buffer[i_in];
+ float z = -zf*zn/((zf-zn)*(d - zf/(zf-zn)));
+ float b = 0.075;
+ float f = 580.0;
+ uint16_t kd = static_cast<uint16_t>(1090 - b*f/z*8);
+ if (kd>2047) kd = 2047;
+
+ int pval = t_gamma[kd];
+ int lb = pval & 0xff;
+ switch (pval>>8) {
+ case 0:
+ depth_img[3*i+0] = 255;
+ depth_img[3*i+1] = 255-lb;
+ depth_img[3*i+2] = 255-lb;
+ break;
+ case 1:
+ depth_img[3*i+0] = 255;
+ depth_img[3*i+1] = lb;
+ depth_img[3*i+2] = 0;
+ break;
+ case 2:
+ depth_img[3*i+0] = 255-lb;
+ depth_img[3*i+1] = 255;
+ depth_img[3*i+2] = 0;
+ break;
+ case 3:
+ depth_img[3*i+0] = 0;
+ depth_img[3*i+1] = 255;
+ depth_img[3*i+2] = lb;
+ break;
+ case 4:
+ depth_img[3*i+0] = 0;
+ depth_img[3*i+1] = 255-lb;
+ depth_img[3*i+2] = 255;
+ break;
+ case 5:
+ depth_img[3*i+0] = 0;
+ depth_img[3*i+1] = 0;
+ depth_img[3*i+2] = 255-lb;
+ break;
+ default:
+ depth_img[3*i+0] = 0;
+ depth_img[3*i+1] = 0;
+ depth_img[3*i+2] = 0;
+ break;
+ }
+ }
+ }
+
+ // Write to file:
+ pcl::io::saveRgbPNGFile (fname, depth_img, width_, height_);
+
+ delete [] depth_img;
+}
+
+
+void
+pcl::simulation::SimExample::write_depth_image_uint(const float* depth_buffer, std::string fname)
+{
+ int npixels = rl_->getWidth() * rl_->getHeight();
+ unsigned short * depth_img = new unsigned short[npixels ];
+
+ float min_depth = depth_buffer[0];
+ float max_depth = depth_buffer[0];
+ for (int i=1; i<npixels; i++)
+ {
+ if (depth_buffer[i] < min_depth) min_depth = depth_buffer[i];
+ if (depth_buffer[i] > max_depth) max_depth = depth_buffer[i];
+ }
+
+ for (int y = 0; y < height_; ++y)
+ {
+ for (int x = 0; x < width_; ++x)
+ {
+ int i= y*width_ + x ;
+ int i_in= (height_-1 -y) *width_ + x ; // flip up down
+
+ float zn = 0.7;
+ float zf = 20.0;
+ float d = depth_buffer[i_in];
+
+ unsigned short z_new = (unsigned short) floor( 1000*( -zf*zn/((zf-zn)*(d - zf/(zf-zn)))));
+ if (z_new>65535) z_new = 65535;
+
+ if ( z_new < 18000){
+ cout << z_new << " " << d << " " << x << "\n";
+ }
+
+ float z = 1000*( -zf*zn/((zf-zn)*(d - zf/(zf-zn))));
+ float b = 0.075;
+ float f = 580.0;
+ uint16_t kd = static_cast<uint16_t>(1090 - b*f/z*8);
+ if (kd>2047) kd = 2047;
+
+ depth_img[i] = z_new;
+ }
+ }
+
+ // Write to file:
+ pcl::io::saveShortPNGFile (fname, depth_img, width_, height_, 1);
+
+ delete [] depth_img;
+}
+
+
+void
+pcl::simulation::SimExample::write_rgb_image(const uint8_t* rgb_buffer, std::string fname)
+{
+ int npixels = rl_->getWidth() * rl_->getHeight();
+ uint8_t* rgb_img = new uint8_t[npixels * 3];
+
+ for (int y = 0; y < height_; ++y)
+ {
+ for (int x = 0; x < width_; ++x)
+ {
+ int px= y*width_ + x ;
+ int px_in= (height_-1 -y) *width_ + x ; // flip up down
+ rgb_img [3* (px) +0] = rgb_buffer[3*px_in+0];
+ rgb_img [3* (px) +1] = rgb_buffer[3*px_in+1];
+ rgb_img [3* (px) +2] = rgb_buffer[3*px_in+2];
+ }
+ }
+
+ // Write to file:
+ pcl::io::saveRgbPNGFile (fname, rgb_img, width_, height_);
+
+ delete [] rgb_img;
+}
+
+
--- /dev/null
+#ifndef PCL_SIMULATION_IO_
+#define PCL_SIMULATION_IO_
+
+#include <boost/shared_ptr.hpp>
+
+#include <GL/glew.h>
+
+#include <pcl/pcl_config.h>
+#ifdef OPENGL_IS_A_FRAMEWORK
+# include <OpenGL/gl.h>
+# include <OpenGL/glu.h>
+#else
+# include <GL/gl.h>
+# include <GL/glu.h>
+#endif
+#ifdef GLUT_IS_A_FRAMEWORK
+# include <GLUT/glut.h>
+#else
+# include <GL/glut.h>
+#endif
+
+// define the following in order to eliminate the deprecated headers warning
+#define VTK_EXCLUDE_STRSTREAM_HEADERS
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/io/vtk_lib_io.h>
+
+
+#include <pcl/simulation/camera.h>
+#include <pcl/simulation/scene.h>
+#include <pcl/simulation/range_likelihood.h>
+
+namespace pcl
+{
+ namespace simulation
+ {
+ class PCL_EXPORTS SimExample
+ {
+ public:
+ typedef boost::shared_ptr<SimExample> Ptr;
+ typedef boost::shared_ptr<const SimExample> ConstPtr;
+
+ SimExample (int argc, char** argv,
+ int height,int width);
+ void initializeGL (int argc, char** argv);
+
+ Scene::Ptr scene_;
+ Camera::Ptr camera_;
+ RangeLikelihood::Ptr rl_;
+
+ void doSim (Eigen::Isometry3d pose_in);
+
+ void write_score_image(const float* score_buffer,std::string fname);
+ void write_depth_image(const float* depth_buffer,std::string fname);
+ void write_depth_image_uint(const float* depth_buffer,std::string fname);
+ void write_rgb_image(const uint8_t* rgb_buffer,std::string fname);
+
+ private:
+ uint16_t t_gamma[2048];
+
+ // of platter, usually 640x480
+ int height_;
+ int width_;
+ };
+ }
+}
+
+
+
+
+#endif
--- /dev/null
+set(SUBSYS_NAME stereo)
+set(SUBSYS_DESC "Point cloud stereo library")
+set(SUBSYS_DEPS common io)
+
+set(build TRUE)
+PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS})
+
+PCL_ADD_DOC("${SUBSYS_NAME}")
+
+if(build)
+ set(incs
+ include/pcl/${SUBSYS_NAME}/stereo_grabber.h
+ include/pcl/${SUBSYS_NAME}/stereo_matching.h
+ include/pcl/${SUBSYS_NAME}/disparity_map_converter.h
+ include/pcl/${SUBSYS_NAME}/digital_elevation_map.h
+ )
+
+ set(impl_incs
+ include/pcl/${SUBSYS_NAME}/impl/disparity_map_converter.hpp
+ )
+
+ set(srcs
+ src/stereo_grabber.cpp
+ src/stereo_matching.cpp
+ src/stereo_block_based.cpp
+ src/stereo_adaptive_cost_so.cpp
+ src/disparity_map_converter.cpp
+ src/digital_elevation_map.cpp
+ )
+
+ set(LIB_NAME "pcl_${SUBSYS_NAME}")
+ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
+ PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs})
+ target_link_libraries("${LIB_NAME}" pcl_common)
+ PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "")
+ # Install include files
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs})
+ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs})
+
+endif(build)
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef PCL_DIGITAL_ELEVATION_MAP_H_
+#define PCL_DIGITAL_ELEVATION_MAP_H_
+
+#include <pcl/point_types.h>
+#include <pcl/stereo/disparity_map_converter.h>
+
+namespace pcl
+{
+ /** \brief Build a Digital Elevation Map in the column-disparity space from a disparity map and a color image of the scene.
+ *
+ * Example of usage:
+ *
+ * \code
+ * pcl::PointCloud<pcl::PointDEM>::Ptr cloud (new
+ * pcl::PointCloud<pcl::PointDEM>);
+ * pcl::PointCloud<pcl::RGB>::Ptr left_image (new
+ * pcl::PointCloud<pcl::RGB>);
+ * // Fill left image cloud.
+ *
+ * pcl::DigitalElevationMapBuilder demb;
+ * demb.setBaseline (0.8387445f);
+ * demb.setFocalLength (368.534700f);
+ * demb.setImageCenterX (318.112200f);
+ * demb.setImageCenterY (224.334900f);
+ * demb.setDisparityThresholdMin (15.0f);
+ * demb.setDisparityThresholdMax (80.0f);
+ * demb.setResolution (64, 32);
+ *
+ * // Left view of the scene.
+ * demb.loadImage (left_image);
+ * // Disparity map of the scene.
+ * demb.loadDisparityMap ("disparity_map.txt", 640, 480);
+ *
+ * demb.compute(*cloud);
+ * \endcode
+ *
+ * \author Timur Ibadov (ibadov.timur@gmail.com)
+ * \ingroup stereo
+ */
+ class PCL_EXPORTS DigitalElevationMapBuilder : public DisparityMapConverter <PointDEM>
+ {
+ public:
+ using DisparityMapConverter<PointDEM>::baseline_;
+ using DisparityMapConverter<PointDEM>::translateCoordinates;
+ using DisparityMapConverter<PointDEM>::image_;
+ using DisparityMapConverter<PointDEM>::disparity_map_;
+ using DisparityMapConverter<PointDEM>::disparity_map_width_;
+ using DisparityMapConverter<PointDEM>::disparity_map_height_;
+ using DisparityMapConverter<PointDEM>::disparity_threshold_min_;
+ using DisparityMapConverter<PointDEM>::disparity_threshold_max_;
+
+ /** \brief DigitalElevationMapBuilder constructor. */
+ DigitalElevationMapBuilder ();
+ /** \brief Empty destructor. */
+ virtual ~DigitalElevationMapBuilder ();
+
+ /** \brief Set resolution of the DEM.
+ * \param[in] resolution_column the column resolution.
+ * \param[in] resolution_disparity the disparity resolution.
+ */
+ void
+ setResolution (size_t resolution_column, size_t resolution_disparity);
+
+ /** \brief Get column resolution of the DEM.
+ * \return column resolution of the DEM.
+ */
+ size_t
+ getColumnResolution () const;
+
+ /** \brief Get disparity resolution of the DEM.
+ * \return disparity resolution of the DEM.
+ */
+ size_t
+ getDisparityResolution () const;
+
+ /** \brief Set minimum amount of points in a DEM's cell.
+ * \param[in] min_points_in_cell minimum amount of points in a DEM's cell.
+ */
+ void
+ setMinPointsInCell (size_t min_points_in_cell);
+
+ /** \brief Get minimum amount of points in a DEM's cell.
+ * \return minimum amount of points in a DEM's cell.
+ */
+ size_t
+ getMinPointsInCell () const;
+
+ /** \brief Compute the Digital Elevation Map.
+ * \param[out] out_cloud the variable to return the resulting cloud.
+ */
+ void
+ compute (pcl::PointCloud<PointDEM> &out_cloud);
+
+ protected:
+ /** \brief Column resolution of the DEM. */
+ size_t resolution_column_;
+ /** \brief disparity resolution of the DEM. */
+ size_t resolution_disparity_;
+
+ /** \brief Minimum amount of points in a DEM's cell. */
+ size_t min_points_in_cell_;
+ };
+}
+
+#endif // PCL_DIGITAL_ELEVATION_MAP_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef PCL_DISPARITY_MAP_CONVERTER_H_
+#define PCL_DISPARITY_MAP_CONVERTER_H_
+
+#include <cstring>
+#include <vector>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+namespace pcl
+{
+ /** \brief Compute point cloud from the disparity map.
+ *
+ * Example of usage:
+ *
+ * \code
+ * pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new
+ * pcl::PointCloud<pcl::PointXYZI>);
+ * pcl::PointCloud<pcl::RGB>::Ptr left_image (new
+ * pcl::PointCloud<pcl::RGB>);
+ * // Fill left image cloud.
+ *
+ * pcl::DisparityMapConverter<pcl::PointXYZI> dmc;
+ * dmc.setBaseline (0.8387445f);
+ * dmc.setFocalLength (368.534700f);
+ * dmc.setImageCenterX (318.112200f);
+ * dmc.setImageCenterY (224.334900f);
+ * dmc.setDisparityThresholdMin(15.0f);
+ *
+ * // Left view of the scene.
+ * dmc.setImage (left_image);
+ * // Disparity map of the scene.
+ * dmc.loadDisparityMap ("disparity_map.txt", 640, 480);
+ *
+ * dmc.compute(*cloud);
+ * \endcode
+ *
+ * \author Timur Ibadov (ibadov.timur@gmail.com)
+ * \ingroup stereo
+ */
+ template <typename PointT>
+ class DisparityMapConverter
+ {
+ protected:
+ typedef pcl::PointCloud<PointT> PointCloud;
+
+ public:
+ /** \brief DisparityMapConverter constructor. */
+ DisparityMapConverter ();
+ /** \brief Empty destructor. */
+ virtual ~DisparityMapConverter ();
+
+ /** \brief Set x-coordinate of the image center.
+ * \param[in] center_x x-coordinate of the image center.
+ */
+ inline void
+ setImageCenterX (const float center_x);
+
+ /** \brief Get x-coordinate of the image center.
+ * \return x-coordinate of the image center.
+ */
+ inline float
+ getImageCenterX () const;
+
+ /** \brief Set y-coordinate of the image center.
+ * \param[in] center_y y-coordinate of the image center.
+ */
+ inline void
+ setImageCenterY (const float center_y);
+
+ /** \brief Get y-coordinate of the image center.
+ * \return y-coordinate of the image center.
+ */
+ inline float
+ getImageCenterY () const;
+
+ /** \brief Set focal length.
+ * \param[in] focal_length the focal length.
+ */
+ inline void
+ setFocalLength (const float focal_length);
+
+ /** \brief Get focal length.
+ * \return the focal length.
+ */
+ inline float
+ getFocalLength () const;
+
+ /** \brief Set baseline.
+ * \param[in] baseline baseline.
+ */
+ inline void
+ setBaseline (const float baseline);
+
+ /** \brief Get baseline.
+ * \return the baseline.
+ */
+ inline float
+ getBaseline () const;
+
+ /** \brief Set min disparity threshold.
+ * \param[in] disparity_threshold_min min disparity threshold.
+ */
+ inline void
+ setDisparityThresholdMin (const float disparity_threshold_min);
+
+ /** \brief Get min disparity threshold.
+ * \return min disparity threshold.
+ */
+ inline float
+ getDisparityThresholdMin () const;
+
+ /** \brief Set max disparity threshold.
+ * \param[in] disparity_threshold_max max disparity threshold.
+ */
+ inline void
+ setDisparityThresholdMax (const float disparity_threshold_max);
+
+ /** \brief Get max disparity threshold.
+ * \return max disparity threshold.
+ */
+ inline float
+ getDisparityThresholdMax () const;
+
+ /** \brief Set an image, that will be used for coloring of the output cloud.
+ * \param[in] image the image.
+ */
+ void
+ setImage (const pcl::PointCloud<pcl::RGB>::ConstPtr &image);
+
+ /** \brief Get the image, that is used for coloring of the output cloud.
+ * \return the image.
+ */
+ pcl::PointCloud<RGB>::Ptr
+ getImage ();
+
+ /** \brief Load the disparity map.
+ * \param[in] file_name the name of the disparity map file.
+ * \return "true" if the disparity map was successfully loaded; "false" otherwise
+ */
+ bool
+ loadDisparityMap (const std::string &file_name);
+
+ /** \brief Load the disparity map and initialize it's size.
+ * \param[in] file_name the name of the disparity map file.
+ * \param[in] width width of the disparity map.
+ * \param[in] height height of the disparity map.
+ * \return "true" if the disparity map was successfully loaded; "false" otherwise
+ */
+ bool
+ loadDisparityMap (const std::string &file_name, const size_t width, const size_t height);
+
+ /** \brief Set the disparity map.
+ * \param[in] disparity_map the disparity map.
+ */
+ void
+ setDisparityMap (const std::vector<float> &disparity_map);
+
+ /** \brief Set the disparity map and initialize it's size.
+ * \param[in] disparity_map the disparity map.
+ * \param[in] width width of the disparity map.
+ * \param[in] height height of the disparity map.
+ * \return "true" if the disparity map was successfully loaded; "false" otherwise
+ */
+ void
+ setDisparityMap (const std::vector<float> &disparity_map,
+ const size_t width, const size_t height);
+
+ /** \brief Get the disparity map.
+ * \return the disparity map.
+ */
+ std::vector<float>
+ getDisparityMap ();
+
+ /** \brief Compute the output cloud.
+ * \param[out] out_cloud the variable to return the resulting cloud.
+ */
+ virtual void
+ compute (PointCloud &out_cloud);
+
+ protected:
+ /** \brief Translate point from image coordinates and disparity to 3D-coordinates
+ * \param[in] row
+ * \param[in] column
+ * \param[in] disparity
+ * \return the 3D point, that corresponds to the input parametres and the camera calibration.
+ */
+ PointXYZ
+ translateCoordinates (size_t row, size_t column, float disparity) const;
+
+ /** \brief X-coordinate of the image center. */
+ float center_x_;
+ /** \brief Y-coordinate of the image center. */
+ float center_y_;
+ /** \brief Focal length. */
+ float focal_length_;
+ /** \brief Baseline. */
+ float baseline_;
+
+ /** \brief Is color image is set. */
+ bool is_color_;
+ /** \brief Color image of the scene. */
+ pcl::PointCloud<pcl::RGB>::ConstPtr image_;
+
+ /** \brief Vector for the disparity map. */
+ std::vector<float> disparity_map_;
+ /** \brief X-size of the disparity map. */
+ size_t disparity_map_width_;
+ /** \brief Y-size of the disparity map. */
+ size_t disparity_map_height_;
+
+ /** \brief Thresholds of the disparity. */
+ float disparity_threshold_min_;
+ float disparity_threshold_max_;
+ };
+
+}
+
+#include <pcl/stereo/impl/disparity_map_converter.hpp>
+
+#endif // PCL_DISPARITY_MAP_CONVERTER_H_
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
+#define PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
+
+#include <pcl/stereo/disparity_map_converter.h>
+
+#include <fstream>
+#include <limits>
+
+#include <pcl/point_types.h>
+#include <pcl/console/print.h>
+#include <pcl/common/intensity.h>
+
+template <typename PointT>
+pcl::DisparityMapConverter<PointT>::DisparityMapConverter () :
+ center_x_ (0.0f),
+ center_y_ (0.0f),
+ focal_length_ (0.0f),
+ baseline_ (0.0f),
+ is_color_ (false),
+ disparity_map_width_ (640),
+ disparity_map_height_ (480),
+ disparity_threshold_min_ (0.0f),
+ disparity_threshold_max_ (std::numeric_limits<float>::max ())
+{
+}
+
+template <typename PointT>
+pcl::DisparityMapConverter<PointT>::~DisparityMapConverter ()
+{
+}
+
+template <typename PointT> inline void
+pcl::DisparityMapConverter<PointT>::setImageCenterX (const float center_x)
+{
+ center_x_ = center_x;
+}
+
+template <typename PointT> inline float
+pcl::DisparityMapConverter<PointT>::getImageCenterX () const
+{
+ return center_x_;
+}
+
+template <typename PointT> inline void
+pcl::DisparityMapConverter<PointT>::setImageCenterY (const float center_y)
+{
+ center_y_ = center_y;
+}
+
+template <typename PointT> inline float
+pcl::DisparityMapConverter<PointT>::getImageCenterY () const
+{
+ return center_y_;
+}
+
+template <typename PointT> inline void
+pcl::DisparityMapConverter<PointT>::setFocalLength (const float focal_length)
+{
+ focal_length_ = focal_length;
+}
+
+template <typename PointT> inline float
+pcl::DisparityMapConverter<PointT>::getFocalLength () const
+{
+ return focal_length_;
+}
+
+template <typename PointT> inline void
+pcl::DisparityMapConverter<PointT>::setBaseline (const float baseline)
+{
+ baseline_ = baseline;
+}
+
+template <typename PointT> inline float
+pcl::DisparityMapConverter<PointT>::getBaseline () const
+{
+ return baseline_;
+}
+
+template <typename PointT> inline void
+pcl::DisparityMapConverter<PointT>::setDisparityThresholdMin (
+ const float disparity_threshold_min)
+{
+ disparity_threshold_min_ = disparity_threshold_min;
+}
+
+template <typename PointT> inline float
+pcl::DisparityMapConverter<PointT>::getDisparityThresholdMin () const
+{
+ return disparity_threshold_min_;
+}
+
+template <typename PointT> inline void
+pcl::DisparityMapConverter<PointT>::setDisparityThresholdMax (
+ const float disparity_threshold_max)
+{
+ disparity_threshold_max_ = disparity_threshold_max;
+}
+
+template <typename PointT> inline float
+pcl::DisparityMapConverter<PointT>::getDisparityThresholdMax () const
+{
+ return disparity_threshold_max_;
+}
+
+template <typename PointT> void
+pcl::DisparityMapConverter<PointT>::setImage (
+ const pcl::PointCloud<pcl::RGB>::ConstPtr &image)
+{
+ image_ = image;
+
+ // Set disparity map's size same with the image size.
+ disparity_map_width_ = image_->width;
+ disparity_map_height_ = image_->height;
+
+ is_color_ = true;
+}
+
+template <typename PointT> pcl::PointCloud<pcl::RGB>::Ptr
+pcl::DisparityMapConverter<PointT>::getImage ()
+{
+ pcl::PointCloud<pcl::RGB>::Ptr image_pointer (new pcl::PointCloud<pcl::RGB>);
+ *image_pointer = *image_;
+ return image_pointer;
+}
+
+template <typename PointT> bool
+pcl::DisparityMapConverter<PointT>::loadDisparityMap (
+ const std::string &file_name)
+{
+ std::fstream disparity_file;
+
+ // Open the disparity file
+ disparity_file.open (file_name.c_str (), std::fstream::in);
+ if (!disparity_file.is_open ())
+ {
+ PCL_ERROR ("[pcl::DisparityMapConverter::loadDisparityMap] Can't load the file.\n");
+ return false;
+ }
+
+ // Allocate memory for the disparity map.
+ disparity_map_.resize (disparity_map_width_ * disparity_map_height_);
+
+ // Reading the disparity map.
+ for (size_t row = 0; row < disparity_map_height_; ++row)
+ {
+ for (size_t column = 0; column < disparity_map_width_; ++column)
+ {
+ float disparity;
+ disparity_file >> disparity;
+
+ disparity_map_[column + row * disparity_map_width_] = disparity;
+ } // column
+ } // row
+
+ return true;
+}
+
+template <typename PointT> bool
+pcl::DisparityMapConverter<PointT>::loadDisparityMap (
+ const std::string &file_name, const size_t width, const size_t height)
+{
+ // Initialize disparity map's size.
+ disparity_map_width_ = width;
+ disparity_map_height_ = height;
+
+ // Load the disparity map.
+ return loadDisparityMap (file_name);
+}
+
+template <typename PointT> void
+pcl::DisparityMapConverter<PointT>::setDisparityMap (
+ const std::vector<float> &disparity_map)
+{
+ disparity_map_ = disparity_map;
+}
+
+template <typename PointT> void
+pcl::DisparityMapConverter<PointT>::setDisparityMap (
+ const std::vector<float> &disparity_map,
+ const size_t width, const size_t height)
+{
+ disparity_map_width_ = width;
+ disparity_map_height_ = height;
+
+ disparity_map_ = disparity_map;
+}
+
+template <typename PointT> std::vector<float>
+pcl::DisparityMapConverter<PointT>::getDisparityMap ()
+{
+ return disparity_map_;
+}
+
+template <typename PointT> void
+pcl::DisparityMapConverter<PointT>::compute (PointCloud &out_cloud)
+{
+ // Initialize the output cloud.
+ out_cloud.clear ();
+ out_cloud.width = disparity_map_width_;
+ out_cloud.height = disparity_map_height_;
+ out_cloud.resize (out_cloud.width * out_cloud.height);
+
+ if (is_color_ && !image_)
+ {
+ PCL_ERROR ("[pcl::DisparityMapConverter::compute] Memory for the image was not allocated.\n");
+ return;
+ }
+
+ for (size_t row = 0; row < disparity_map_height_; ++row)
+ {
+ for (size_t column = 0; column < disparity_map_width_; ++column)
+ {
+ // ID of current disparity point.
+ size_t disparity_point = column + row * disparity_map_width_;
+
+ // Disparity value.
+ float disparity = disparity_map_[disparity_point];
+
+ // New point for the output cloud.
+ PointT new_point;
+
+ // Init color
+ if (is_color_)
+ {
+ pcl::common::IntensityFieldAccessor<PointT> intensity_accessor;
+ intensity_accessor.set (new_point, static_cast<float> (
+ image_->points[disparity_point].r +
+ image_->points[disparity_point].g +
+ image_->points[disparity_point].b) / 3.0f);
+ }
+
+ // Init coordinates.
+ if (disparity_threshold_min_ < disparity && disparity < disparity_threshold_max_)
+ {
+ // Compute new coordinates.
+ PointXYZ point_3D (translateCoordinates (row, column, disparity));
+ new_point.x = point_3D.x;
+ new_point.y = point_3D.y;
+ new_point.z = point_3D.z;
+ }
+ else
+ {
+ new_point.x = std::numeric_limits<float>::quiet_NaN ();
+ new_point.y = std::numeric_limits<float>::quiet_NaN ();
+ new_point.z = std::numeric_limits<float>::quiet_NaN ();
+ }
+ // Put the point to the output cloud.
+ out_cloud[disparity_point] = new_point;
+ } // column
+ } // row
+}
+
+template <typename PointT> pcl::PointXYZ
+pcl::DisparityMapConverter<PointT>::translateCoordinates (
+ size_t row, size_t column, float disparity) const
+{
+ // Returning point.
+ PointXYZ point_3D;
+
+ if (disparity != 0.0f)
+ {
+ // Compute 3D-coordinates based on the image coordinates, the disparity and the camera parameters.
+ float z_value = focal_length_ * baseline_ / disparity;
+ point_3D.z = z_value;
+ point_3D.x = (static_cast<float> (column) - center_x_) * (z_value / focal_length_);
+ point_3D.y = (static_cast<float> (row) - center_y_) * (z_value / focal_length_);
+ }
+
+ return point_3D;
+}
+
+#define PCL_INSTANTIATE_DisparityMapConverter(T) template class PCL_EXPORTS pcl::DisparityMapConverter<T>;
+
+#endif // PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_STEREO_STEREO_GRABER_H_
+#define PCL_STEREO_STEREO_GRABER_H_
+
+#include <pcl/io/grabber.h>
+#include <pcl/common/time_trigger.h>
+#include <pcl/conversions.h>
+#include <pcl/stereo/stereo_matching.h>
+
+namespace pcl
+{
+ /** \brief Base class for Stereo file grabber.
+ * \ingroup io
+ */
+ class PCL_EXPORTS StereoGrabberBase : public Grabber
+ {
+ public:
+ /** \brief Constructor taking just one Stereo pair.
+ * \param[in] pair_files the name of the the stereo (left + right) images.
+ * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next pair in the list.
+ * \param[in] repeat whether to play files in an endless loop or not.
+ */
+ StereoGrabberBase (const std::pair<std::string, std::string>& pair_files, float frames_per_second, bool repeat);
+
+ /** \brief Constructor taking a list of paths to Stereo pair files, that are played in the order they appear in the list.
+ * \param[in] files vector of paths to stereo (left+right) images.
+ * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next pair in the list.
+ * \param[in] repeat whether to play files in an endless loop or not.
+ */
+ StereoGrabberBase (const std::vector<std::pair<std::string, std::string> >& files, float frames_per_second, bool repeat);
+
+ /** \brief Copy constructor.
+ * \param[in] src the Stereo Grabber base object to copy into this
+ */
+ StereoGrabberBase (const StereoGrabberBase &src) : Grabber (), impl_ ()
+ {
+ *this = src;
+ }
+
+ /** \brief Copy operator.
+ * \param[in] src the Stereo Grabber base object to copy into this
+ */
+ StereoGrabberBase&
+ operator = (const StereoGrabberBase &src)
+ {
+ impl_ = src.impl_;
+ return (*this);
+ }
+
+ /** \brief Virtual destructor. */
+ virtual ~StereoGrabberBase () throw ();
+
+ /** \brief Starts playing the list of Stereo images if frames_per_second is > 0. Otherwise it works as a trigger: publishes only the next pair in the list. */
+ virtual void
+ start ();
+
+ /** \brief Stops playing the list of Stereo images if frames_per_second is > 0. Otherwise the method has no effect. */
+ virtual void
+ stop ();
+
+ /** \brief Triggers a callback with new data */
+ virtual void
+ trigger ();
+
+ /** \brief whether the grabber is started (publishing) or not.
+ * \return true only if publishing.
+ */
+ virtual bool
+ isRunning () const;
+
+ /** \return The name of the grabber */
+ virtual std::string
+ getName () const;
+
+ /** \brief Rewinds to the first pair of files in the list.*/
+ virtual void
+ rewind ();
+
+ /** \brief Returns the frames_per_second. 0 if grabber is trigger-based */
+ virtual float
+ getFramesPerSecond () const;
+
+ /** \brief Returns whether the repeat flag is on */
+ bool
+ isRepeatOn () const;
+
+ private:
+ virtual void
+ publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const = 0;
+
+ // to separate and hide the implementation from interface: PIMPL
+ struct StereoGrabberImpl;
+ StereoGrabberImpl* impl_;
+ };
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ template <typename T> class PointCloud;
+ template <typename PointT> class StereoGrabber : public StereoGrabberBase
+ {
+ public:
+ StereoGrabber (const std::pair<std::string, std::string> & pair_files, float frames_per_second = 0, bool repeat = false);
+ StereoGrabber (const std::vector<std::pair<std::string, std::string> >& files, float frames_per_second = 0, bool repeat = false);
+ protected:
+ virtual void
+ publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const;
+
+ boost::signals2::signal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>* signal_;
+ };
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ template<typename PointT>
+ StereoGrabber<PointT>::StereoGrabber (const std::pair<std::string, std::string>& pair_files, float frames_per_second, bool repeat)
+ : StereoGrabberBase (pair_files, frames_per_second, repeat)
+ {
+ signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ template<typename PointT>
+ StereoGrabber<PointT>::StereoGrabber (const std::vector<std::pair<std::string, std::string> >& files, float frames_per_second, bool repeat)
+ : StereoGrabberBase (files, frames_per_second, repeat), signal_ ()
+ {
+ signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ template<typename PointT> void
+ StereoGrabber<PointT>::publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const
+ {
+ typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ());
+ pcl::fromPCLPointCloud2 (blob, *cloud);
+ cloud->sensor_origin_ = origin;
+ cloud->sensor_orientation_ = orientation;
+
+ signal_->operator () (cloud);
+ }
+}
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+#ifndef PCL_STEREO_H_
+#define PCL_STEREO_H_
+
+#include <pcl/conversions.h>
+#include <pcl/point_types.h>
+
+namespace pcl
+{
+ /** \brief Stereo Matching abstract class
+ *
+ * The class performs stereo matching on a rectified stereo pair
+ * Includes the following functionalities:
+ * * preprocessing of the image pair, to improve robustness against photometric distortions
+ * (wrt. to a spatially constant additive photometric factor)
+ * * postprocessing: filtering of wrong disparities via Peak Filter (eliminating ambiguities due to low-textured regions)
+ * and Ratio Filter (eliminating generic matching ambiguities, similar to that present in OpenCV Block Matching Stereo)
+ * * postprocessing: Left-Right consistency check (eliminates wrong disparities at the cost of twice the stereo matching
+ * computation)
+ * * postprocessing: subpixel refinement of computed disparities, to reduce the depth quantization effect
+ * * postprocessing: smoothing of the disparity map via median filter
+ * * after stereo matching a PCL point cloud can be computed, given the stereo intrinsic (focal, principal point
+ * coordinates) and extrinsic (baseline) calibration parameters
+ *
+ * \author Federico Tombari (federico.tombari@unibo.it)
+ * \ingroup stereo
+ */
+ class PCL_EXPORTS StereoMatching
+ {
+ public:
+ StereoMatching(void);
+
+ virtual ~StereoMatching(void);
+
+ /** \brief setter for number of disparity candidates (disparity range)
+ *
+ * \param[in] max_disp number of disparity candidates (disparity range); has to be > 0
+ */
+ void
+ setMaxDisparity (int max_disp)
+ {
+ max_disp_ = max_disp;
+ };
+
+ /** \brief setter for horizontal offset, i.e. number of pixels to shift the disparity range over the target image
+ *
+ * \param[in] x_off horizontal offset value; has to be >= 0
+ */
+ void
+ setXOffset (int x_off)
+ {
+ x_off_ = x_off;
+ };
+
+ /** \brief setter for the value of the ratio filter
+ *
+ * \param[in] ratio_filter value of the ratio filter; it is a number in the range [0, 100]
+ * (0: no filtering action; 100: all disparities are filtered)
+ */
+ void
+ setRatioFilter (int ratio_filter)
+ {
+ ratio_filter_ = ratio_filter;
+ };
+
+ /** \brief setter for the value of the peak filter
+ *
+ * \param[in] peak_filter value of the peak filter; it is a number in the range [0, inf]
+ * (0: no filtering action)
+ */
+ void
+ setPeakFilter (int peak_filter)
+ {
+ peak_filter_ = peak_filter;
+ };
+
+ /** \brief setter for the pre processing step
+ *
+ * \param[in] is_pre_proc setting the boolean to true activates the pre-processing step for both stereo images
+ */
+ void
+ setPreProcessing (bool is_pre_proc)
+ {
+ is_pre_proc_ = is_pre_proc;
+ };
+
+ /** \brief setter for the left-right consistency check stage, that eliminates inconsistent/wrong disparity
+ * values from the disparity map at approx. twice the processing cost of the selected stereo algorithm
+ *
+ * \param[in] is_lr_check setting the boolean to true activates the left-right consistency check
+ */
+ void
+ setLeftRightCheck (bool is_lr_check)
+ {
+ is_lr_check_ = is_lr_check;
+ };
+
+ /** \brief setter for the left-right consistency check threshold
+ *
+ * \param[in] lr_check_th sets the value of the left-right consistency check threshold
+ * only has some influence if the left-right check is active
+ * typically has either the value 0 ("strong" consistency check, more points being filtered) or 1 ("weak"
+ * consistency check, less points being filtered)
+ */
+ void
+ setLeftRightCheckThreshold (int lr_check_th)
+ {
+ lr_check_th_ = lr_check_th;
+ };
+
+ /** \brief stereo processing, it computes a disparity map stored internally by the class
+ *
+ * \param[in] ref_img reference array of image pixels (left image)
+ * \param[in] trg_img target array of image pixels (right image)
+ * \param[in] width number of elements per row for both input arrays
+ * \param[in] height number of elements per column for both input arrays
+ */
+ virtual void
+ compute (unsigned char* ref_img, unsigned char* trg_img, int width, int height) = 0;
+
+ /** \brief stereo processing, it computes a disparity map stored internally by the class
+ *
+ * \param[in] ref point cloud of pcl::RGB type containing the pixels of the reference image (left image)
+ * \param[in] trg point cloud of pcl::RGB type containing the pixels of the target image (right image)
+ */
+ virtual void
+ compute (pcl::PointCloud<pcl::RGB> &ref, pcl::PointCloud<pcl::RGB> &trg) = 0;
+
+ /** \brief median filter applied on the previously computed disparity map
+ * Note: the "compute" method must have been previously called at least once in order for this function
+ * to have any effect
+ * \param[in] radius radius of the squared window used to compute the median filter; the window side is
+ * equal to 2*radius + 1
+ */
+ void
+ medianFilter (int radius);
+
+ /** \brief computation of the 3D point cloud from the previously computed disparity map without color information
+ * Note: the "compute" method must have been previously called at least once in order for this function
+ * to have any effect
+ * \param[in] u_c horizontal coordinate of the principal point (calibration parameter)
+ * \param[in] v_c vertical coordinate of the principal point (calibration parameter)
+ * \param[in] focal focal length in pixels (calibration parameter)
+ * \param[in] baseline distance between the two cameras (calibration parameter); the measure unit used to
+ * specify this parameter will be the same as the 3D points in the output point cloud
+ * \param[out] cloud output 3D point cloud; it is organized and non-dense, with NaNs where 3D points are invalid
+ */
+ virtual bool
+ getPointCloud (float u_c, float v_c, float focal, float baseline, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
+
+ /** \brief computation of the 3D point cloud from the previously computed disparity map including color information
+ * Note: the "compute" method must have been previously called at least once in order for this function
+ * to have any effect
+ * \param[in] u_c horizontal coordinate of the principal point (calibration parameter)
+ * \param[in] v_c vertical coordinate of the principal point (calibration parameter)
+ * \param[in] focal focal length in pixels (calibration parameter)
+ * \param[in] baseline distance between the two cameras (calibration parameter); the measure unit used to
+ * specify this parameter will be the same as the 3D points in the output point cloud
+ * \param[out] cloud output 3D point cloud; it is organized and non-dense, with NaNs where 3D points are invalid
+ * \param[in] texture 3D cloud (same size of the output cloud) used to associate to each 3D point of the
+ * output cloud a color triplet
+ */
+ virtual bool
+ getPointCloud (float u_c, float v_c, float focal, float baseline, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, pcl::PointCloud<pcl::RGB>::Ptr texture);
+
+ /** \brief computation of a pcl::RGB cloud with scaled disparity values
+ * it can be used to display a rescaled version of the disparity map by means of the pcl::ImageViewer
+ * invalid disparity values are shown in green
+ * Note: the "compute" method must have been previously called at least once in order for this function
+ * to have any effect
+ * \param[out] vMap output cloud
+ */
+ void
+ getVisualMap (pcl::PointCloud<pcl::RGB>::Ptr vMap);
+
+ protected:
+ /** \brief The internal disparity map. */
+ short int *disp_map_;
+
+ /** \brief Local aligned copies of the cloud data. */
+ unsigned char* ref_img_;
+ unsigned char* trg_img_;
+
+ /** \brief Disparity map used for left-right check. */
+ short int *disp_map_trg_;
+
+ /** \brief Local aligned copies used for pre processing. */
+ unsigned char* pp_ref_img_;
+ unsigned char* pp_trg_img_;
+
+ /** \brief number of pixels per column of the input stereo pair . */
+ int width_;
+
+ /** \brief number of pixels per row of the input stereo pair . */
+ int height_;
+
+ /** \brief Disparity range used for stereo processing. */
+ int max_disp_;
+
+ /** \brief Horizontal displacemente (x offset) used for stereo processing */
+ int x_off_;
+
+ /** \brief Threshold for the ratio filter, \f$\in [0 100]\f$ */
+ int ratio_filter_;
+
+ /** \brief Threshold for the peak filter, \f$\in [0 \infty]\f$ */
+ int peak_filter_;
+
+ /** \brief toggle for the activation of the pre-processing stage */
+ bool is_pre_proc_;
+
+ /** \brief toggle for the activation of the left-right consistency check stage */
+ bool is_lr_check_;
+
+ /** \brief Threshold for the left-right consistency check, typically either 0 or 1 */
+ int lr_check_th_;
+
+ virtual void
+ preProcessing (unsigned char *img, unsigned char *pp_img) = 0;
+
+ virtual void
+ imgFlip (unsigned char * & img) = 0;
+
+ virtual void
+ compute_impl (unsigned char* ref_img, unsigned char* trg_img) = 0;
+
+ void
+ leftRightCheck ();
+
+ inline short int
+ computeStereoSubpixel (int dbest, int s1, int s2, int s3)
+ {
+ int den = (s1+s3-2*s2);
+ if (den != 0)
+ return (static_cast<short int> (16*dbest + (((s1 - s3)*8) / den)));
+ else
+ return (static_cast<short int> (dbest*16));
+ }
+
+ inline short int
+ computeStereoSubpixel (int dbest, float s1, float s2, float s3)
+ {
+ float den = (s1+s3-2*s2);
+ if (den != 0)
+ return (static_cast<short int> (16*dbest + floor(.5 + (((s1 - s3)*8) / den))));
+ else
+ return (static_cast<short int> (dbest*16));
+ }
+
+ inline short int
+ doStereoRatioFilter (int *acc, short int dbest, int sad_min, int ratio_filter, int maxdisp, int precision = 100)
+ {
+ int sad_second_min = std::numeric_limits<int>::max ();
+
+ for (int d = 0; d < dbest - 1; d++)
+ if (acc[d] < sad_second_min)
+ sad_second_min = acc[d];
+
+ for (int d = dbest + 2; d < maxdisp; d++)
+ if (acc[d] < sad_second_min)
+ sad_second_min = acc[d];
+
+ if (sad_min * precision > (precision - ratio_filter) * sad_second_min)
+ return (-2);
+ else
+ return (dbest);
+ }
+
+ inline short int
+ doStereoRatioFilter (float *acc, short int dbest, float sad_min, int ratio_filter, int maxdisp, int precision = 100)
+ {
+ float sad_second_min = std::numeric_limits<float>::max ();
+
+ for (int d = 0; d < dbest - 1; d++)
+ if (acc[d] < sad_second_min)
+ sad_second_min = acc[d];
+
+ for (int d = dbest + 2; d < maxdisp; d++)
+ if (acc[d] < sad_second_min)
+ sad_second_min = acc[d];
+
+ if (sad_min * static_cast<float> (precision) > static_cast<float> (precision - ratio_filter) * sad_second_min)
+ return (-2);
+ else
+ return (dbest);
+ }
+
+ inline short int
+ doStereoPeakFilter (int *acc, short int dbest, int peak_filter, int maxdisp)
+ {
+ int da = (dbest>1) ? ( acc[dbest-2] - acc[dbest] ) : (acc[dbest+2] - acc[dbest]);
+ int db = (dbest<maxdisp-2) ? (acc[dbest+2] - acc[dbest]) : (acc[dbest-2] - acc[dbest]);
+
+ if (da + db < peak_filter)
+ return (-4);
+ else
+ return (dbest);
+ }
+
+ inline short int
+ doStereoPeakFilter (float *acc, short int dbest, int peak_filter, int maxdisp)
+ {
+ float da = (dbest>1) ? ( acc[dbest-2] - acc[dbest] ) : (acc[dbest+2] - acc[dbest]);
+ float db = (dbest<maxdisp-2) ? (acc[dbest+2] - acc[dbest]) : (acc[dbest-2] - acc[dbest]);
+
+ if (da + db < peak_filter)
+ return (-4);
+ else
+ return (dbest);
+ }
+
+ };
+
+ /** \brief Stereo Matching abstract class for Grayscale images
+ *
+ * The class implements some functionalities of pcl::StereoMatching specific for grayscale stereo processing,
+ * such as image pre-processing and left
+ *
+ * \author Federico Tombari (federico.tombari@unibo.it)
+ * \ingroup stereo
+ */
+ class PCL_EXPORTS GrayStereoMatching : public StereoMatching
+ {
+ public:
+ GrayStereoMatching (void);
+ virtual ~GrayStereoMatching (void);
+
+ /** \brief stereo processing, it computes a disparity map stored internally by the class
+ *
+ * \param[in] ref_img reference array of image pixels (left image), has to be grayscale single channel
+ * \param[in] trg_img target array of image pixels (right image), has to be grayscale single channel
+ * \param[in] width number of elements per row for both input arrays
+ * \param[in] height number of elements per column for both input arrays
+ */
+ virtual void
+ compute (unsigned char* ref_img, unsigned char* trg_img, int width, int height);
+
+ /** \brief stereo processing, it computes a disparity map stored internally by the class
+ *
+ * \param[in] ref point cloud of pcl::RGB type containing the pixels of the reference image (left image)
+ * the pcl::RGB triplets are automatically converted to grayscale upon call of the method
+ * \param[in] trg point cloud of pcl::RGB type containing the pixels of the target image (right image)
+ * the pcl::RGB triplets are automatically converted to grayscale upon call of the method
+ */
+ virtual void
+ compute (pcl::PointCloud<pcl::RGB> &ref, pcl::PointCloud<pcl::RGB> &trg);
+ protected:
+ virtual void
+ compute_impl (unsigned char* ref_img, unsigned char* trg_img) = 0;
+
+ virtual void
+ preProcessing (unsigned char *img, unsigned char *pp_img);
+
+ virtual void
+ imgFlip (unsigned char * & img);
+ };
+
+ /** \brief Block based (or fixed window) Stereo Matching class
+ *
+ * This class implements the baseline Block-based - aka Fixed Window - stereo matching algorithm.
+ * The algorithm includes a running box filter so that the computational complexity is independent of
+ * the size of the window ( O(1) wrt. to the size of window)
+ * The algorithm is based on the Sum of Absolute Differences (SAD) matching function
+ * Only works with grayscale (single channel) rectified images
+ *
+ * \author Federico Tombari (federico.tombari@unibo.it)
+ * \ingroup stereo
+ */
+
+ class PCL_EXPORTS BlockBasedStereoMatching : public GrayStereoMatching
+ {
+ public:
+ BlockBasedStereoMatching (void);
+ virtual ~BlockBasedStereoMatching (void)
+ {
+ };
+
+ /** \brief setter for the radius of the squared window
+ * \param[in] radius radius of the squared window used to compute the block-based stereo algorithm
+ * the window side is equal to 2*radius + 1
+ */
+ void
+ setRadius (int radius)
+ {
+ radius_ = radius;
+ };
+ private:
+ virtual void
+ compute_impl (unsigned char* ref_img, unsigned char* trg_img);
+
+ int radius_;
+ };
+
+ /** \brief Adaptive Cost 2-pass Scanline Optimization Stereo Matching class
+ *
+ * This class implements an adaptive-cost stereo matching algorithm based on 2-pass Scanline Optimization.
+ * The algorithm is inspired by the paper:
+ * [1] L. Wang et al., "High Quality Real-time Stereo using Adaptive Cost Aggregation and Dynamic Programming", 3DPVT 2006
+ * Cost aggregation is performed using adaptive weigths computed on a single column as proposed in [1].
+ * Instead of using Dynamic Programming as in [1], the optimization is performed via 2-pass Scanline Optimization.
+ * The algorithm is based on the Sum of Absolute Differences (SAD) matching function
+ * Only works with grayscale (single channel) rectified images
+ *
+ * \author Federico Tombari (federico.tombari@unibo.it)
+ * \ingroup stereo
+ */
+ class PCL_EXPORTS AdaptiveCostSOStereoMatching : public GrayStereoMatching
+ {
+ public:
+ AdaptiveCostSOStereoMatching (void);
+
+ virtual ~AdaptiveCostSOStereoMatching (void)
+ {
+ };
+
+ /** \brief setter for the radius (half length) of the column used for cost aggregation
+ * \param[in] radius radius (half length) of the column used for cost aggregation; the total column length
+ * is equal to 2*radius + 1
+ */
+ void
+ setRadius (int radius)
+ {
+ radius_ = radius;
+ };
+
+ /** \brief setter for the spatial bandwith used for cost aggregation based on adaptive weights
+ * \param[in] gamma_s spatial bandwith used for cost aggregation based on adaptive weights
+ */
+ void
+ setGammaS (int gamma_s)
+ {
+ gamma_s_ = gamma_s;
+ };
+
+ /** \brief setter for the color bandwith used for cost aggregation based on adaptive weights
+ * \param[in] gamma_c color bandwith used for cost aggregation based on adaptive weights
+ */
+ void
+ setGammaC (int gamma_c)
+ {
+ gamma_c_ = gamma_c;
+ };
+
+ /** \brief "weak" smoothness penalty used within 2-pass Scanline Optimization
+ * \param[in] smoothness_weak "weak" smoothness penalty cost
+ */
+ void
+ setSmoothWeak (int smoothness_weak)
+ {
+ smoothness_weak_ = smoothness_weak;
+ };
+
+ /** \brief "strong" smoothness penalty used within 2-pass Scanline Optimization
+ * \param[in] smoothness_strong "strong" smoothness penalty cost
+ */
+ void
+ setSmoothStrong (int smoothness_strong)
+ {
+ smoothness_strong_ = smoothness_strong;
+ };
+
+ private:
+ virtual void
+ compute_impl (unsigned char* ref_img, unsigned char* trg_img);
+
+ int radius_;
+
+ //parameters for adaptive weight cost aggregation
+ double gamma_c_;
+ double gamma_s_;
+
+ //Parameters for 2-pass SO optimization
+ int smoothness_strong_;
+ int smoothness_weak_;
+ };
+}
+
+#endif
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <pcl/stereo/digital_elevation_map.h>
+
+#include <pcl/console/print.h>
+#include <pcl/common/feature_histogram.h>
+
+pcl::DigitalElevationMapBuilder::DigitalElevationMapBuilder () :
+ resolution_column_ (64),
+ resolution_disparity_ (32),
+ min_points_in_cell_ (1)
+{
+}
+
+pcl::DigitalElevationMapBuilder::~DigitalElevationMapBuilder ()
+{
+
+}
+
+void
+pcl::DigitalElevationMapBuilder::setResolution (size_t resolution_column,
+ size_t resolution_disparity)
+{
+ resolution_column_ = resolution_column;
+ resolution_disparity_ = resolution_disparity;
+}
+
+size_t
+pcl::DigitalElevationMapBuilder::getColumnResolution () const
+{
+ return resolution_column_;
+}
+
+size_t
+pcl::DigitalElevationMapBuilder::getDisparityResolution () const
+{
+ return resolution_disparity_;
+}
+
+void
+pcl::DigitalElevationMapBuilder::setMinPointsInCell (size_t min_points_in_cell)
+{
+ min_points_in_cell_ = min_points_in_cell;
+}
+
+size_t
+pcl::DigitalElevationMapBuilder::getMinPointsInCell () const
+{
+ return min_points_in_cell_;
+}
+
+// Build DEM.
+void
+pcl::DigitalElevationMapBuilder::compute (pcl::PointCloud<PointDEM> &out_cloud)
+{
+ // Initialize.
+ // Initialize the output cloud.
+ out_cloud.clear ();
+ out_cloud.width = resolution_column_;
+ out_cloud.height = resolution_disparity_;
+ out_cloud.resize (out_cloud.width * out_cloud.height);
+
+ // Initialize steps.
+ const size_t kColumnStep = (disparity_map_width_ - 1) / resolution_column_ + 1;
+ const float kDisparityStep = (disparity_threshold_max_ -
+ disparity_threshold_min_) / resolution_disparity_;
+
+ // Initialize histograms.
+ const size_t kNumberOfHistograms = resolution_column_ * resolution_disparity_;
+
+ const float kHeightMin = -0.5f;
+ const float kHeightMax = 1.5f;
+ const float kHeightResolution = 0.01f;
+ const size_t kHeightBins = static_cast<size_t> ((kHeightMax - kHeightMin) / kHeightResolution);
+ // Histogram for initializing other height histograms.
+ FeatureHistogram height_histogram_example (kHeightBins, kHeightMin, kHeightMax);
+
+ const float kIntensityMin = 0.0f;
+ const float kIntensityMax = 255.0f;
+ const size_t kIntensityBins = 256;
+ // Histogram for initializing other intensity histograms.
+ FeatureHistogram intensity_histogram_example (kIntensityBins, kIntensityMin, kIntensityMax);
+
+ std::vector <FeatureHistogram> height_histograms
+ (kNumberOfHistograms, height_histogram_example);
+ std::vector <FeatureHistogram> intensity_histograms
+ (kNumberOfHistograms, intensity_histogram_example);
+
+ // Check, if an image was loaded.
+ if (!image_)
+ {
+ PCL_ERROR ("[pcl::DisparityMapConverter::compute] Memory for the image was not allocated.\n");
+ return;
+ }
+
+ for (size_t column = 0; column < disparity_map_width_; ++column)
+ {
+ for (size_t row = 0; row < disparity_map_height_; ++row)
+ {
+ float disparity = disparity_map_[column + row * disparity_map_width_];
+ if (disparity_threshold_min_ < disparity && disparity < disparity_threshold_max_)
+ {
+ // Find a height and an intensity of the point of interest.
+ PointXYZ point_3D = translateCoordinates (row, column, disparity);
+ float height = point_3D.y;
+
+ RGB point_RGB = image_->points[column + row * disparity_map_width_];
+ float intensity = static_cast<float> ((point_RGB.r + point_RGB.g + point_RGB.b) / 3);
+
+ // Calculate index of histograms.
+ size_t index_column = column / kColumnStep;
+ size_t index_disparity = static_cast<size_t> (
+ (disparity - disparity_threshold_min_) / kDisparityStep);
+
+ size_t index = index_column + index_disparity * resolution_column_;
+
+ // Increase the histograms.
+ height_histograms[index].addValue (height);
+ intensity_histograms[index].addValue (intensity);
+
+ } // if
+ } // row
+ } // column
+
+ // For all histograms.
+ for (size_t index_column = 0; index_column < resolution_column_; ++index_column)
+ {
+ for (size_t index_disparity = 0; index_disparity < resolution_disparity_; ++index_disparity)
+ {
+ size_t index = index_column + index_disparity * resolution_column_;
+ // Compute the corresponding DEM cell.
+ size_t column = index_column * kColumnStep;
+ float disparity = disparity_threshold_min_ +
+ static_cast<float> (index_disparity) * kDisparityStep;
+
+ PointXYZ point_3D = translateCoordinates (0, column, disparity);
+ PointDEM point_DEM;
+ point_DEM.x = point_3D.x;
+ point_DEM.z = point_3D.z;
+
+ if (height_histograms[index].getNumberOfElements () >= min_points_in_cell_)
+ {
+ point_DEM.y = height_histograms[index].getMeanValue ();
+ point_DEM.height_variance = 0.5f * baseline_ / disparity;
+
+ point_DEM.intensity = intensity_histograms[index].getMeanValue ();
+ point_DEM.intensity_variance = intensity_histograms[index].getVariance (point_DEM.intensity);
+ }
+ else // height_histograms[index].getNumberOfElements () < min_points_in_cell_
+ {
+ point_DEM.y = std::numeric_limits<float>::quiet_NaN ();
+ point_DEM.intensity = std::numeric_limits<float>::quiet_NaN ();
+ point_DEM.height_variance = std::numeric_limits<float>::quiet_NaN ();
+ point_DEM.intensity_variance = std::numeric_limits<float>::quiet_NaN ();
+ }
+
+ out_cloud.at (index_column, index_disparity) = point_DEM;
+
+ } // index_disparity
+ } // index_column
+}
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <pcl/stereo/disparity_map_converter.h>
+
+#ifndef PCL_NO_PRECOMPILE
+#include <pcl/impl/instantiate.hpp>
+#include <pcl/point_types.h>
+PCL_INSTANTIATE(DisparityMapConverter, PCL_XYZ_POINT_TYPES);
+#endif // PCL_NO_PRECOMPILE
\ No newline at end of file
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ */
+
+/** \brief Adaptive Cost 2-pass Scanline Optimization Stereo Matching algorithm implementation
+ * please see related documentation on stereo/stereo_matching.h
+ *
+ * \author Federico Tombari (federico.tombari@unibo.it)
+ * \ingroup stereo
+ */
+
+#include "pcl/stereo/stereo_matching.h"
+
+//////////////////////////////////////////////////////////////////////////////
+pcl::AdaptiveCostSOStereoMatching::AdaptiveCostSOStereoMatching ()
+{
+ radius_ = 5;
+
+ gamma_s_ = 15;
+ gamma_c_ = 25;
+
+ smoothness_strong_ = 100;
+ smoothness_weak_ = 20;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+void
+pcl::AdaptiveCostSOStereoMatching::compute_impl (unsigned char* ref_img, unsigned char* trg_img)
+{
+ //int n = radius_ * 2 + 1;
+ //int sad_max = std::numeric_limits<int>::max ();
+
+ float **acc = new float *[width_];
+ for (int d = 0; d < width_; d++)
+ {
+ acc[d] = new float[max_disp_];
+ memset (acc[d], 0, sizeof (float) * max_disp_);
+ }
+
+ //data structures for Scanline Optimization
+ float **fwd = new float *[width_];
+ float **bck = new float *[width_];
+ for(int d=0; d<width_; d++)
+ {
+ fwd[d] = new float[max_disp_];
+ memset ( fwd[d], 0, sizeof(float)*max_disp_);
+ bck[d] = new float[max_disp_];
+ memset ( bck[d], 0, sizeof(float)*max_disp_);
+ }
+
+ //spatial distance init
+ float *ds = new float[ 2*radius_+1 ];
+ for (int j = -radius_; j <= radius_; j++)
+ ds[j+radius_] = static_cast<float> (exp (- abs (j) / gamma_s_));
+
+ //LUT for color distance weight computation
+ float lut[256];
+ for (int j = 0; j < 256; j++)
+ lut[j] = float (exp (-j / gamma_c_));
+
+ //left weight array alloc
+ float *wl = new float [ 2*radius_+1 ];
+
+ for (int y = radius_ + 1; y < height_ - radius_; y++)
+ {
+ for (int x = x_off_ + max_disp_ + 1; x < width_; x++)
+ {
+ for (int j = -radius_; j <= radius_; j++)
+ wl[j+radius_] = lut[ abs(ref_img[(y+j)*width_+x] - ref_img[y*width_+x]) ] * ds[j+radius_];
+
+ for (int d = 0; d < max_disp_; d++)
+ {
+ float sumw = 0.0;
+ float num = 0.0;
+
+ for (int j = -radius_; j <= radius_; j++)
+ {
+ float weight_r = lut[ abs(trg_img[(y+j)*width_+x-d-x_off_] - trg_img[y*width_+x-d-x_off_]) ] * ds[j+radius_];
+ int sad = abs (ref_img[(y+j)*width_+x] - trg_img[(y+j)*width_+x-d-x_off_]);
+ num += wl[j+radius_] * weight_r * static_cast<float> (sad);
+ sumw += wl[j+radius_] * weight_r;
+ }
+
+ acc[x][d] = num / sumw;
+
+ }//d
+ }//x
+
+ //Forward
+ for (int d = 0; d < max_disp_; d++)
+ fwd[max_disp_+1][d] = acc[max_disp_+1][d];
+
+ for (int x = x_off_ + max_disp_+2; x<width_; x++)
+ {
+ float c_min = fwd[x-1][0];
+ for (int d = 1; d < max_disp_; d++)
+ if (fwd[x-1][d] < c_min)
+ c_min = fwd[x-1][d];
+
+ fwd[x][0] = acc[x][0] - c_min + std::min (fwd[x-1][0], std::min (fwd[x-1][1] + static_cast<float> (smoothness_weak_), c_min + static_cast<float> (smoothness_strong_)));
+ for (int d = 1; d < max_disp_ - 1; d++)
+ {
+ fwd[x][d] = acc[x][d] - c_min + std::min (std::min (fwd[x-1][d], fwd[x-1][d-1] + static_cast<float> (smoothness_weak_)), std::min (fwd[x-1][d+1] + static_cast<float> (smoothness_weak_), c_min + static_cast<float> (smoothness_strong_)));
+ }
+ fwd[x][max_disp_-1] = acc[x][max_disp_-1] - c_min + std::min (fwd[x-1][max_disp_-1], std::min(fwd[x-1][max_disp_-2] + static_cast<float> (smoothness_weak_), c_min + static_cast<float> (smoothness_strong_)));
+ }//x
+
+ //Backward
+ for (int d = 0; d < max_disp_; d++)
+ bck[width_-1][d] = acc[width_-1][d];
+
+ for (int x = width_-2; x > max_disp_ + x_off_; x--)
+ {
+
+ float c_min = bck[x+1][0];
+ for (int d = 1; d < max_disp_; d++)
+ if (bck[x+1][d] < c_min)
+ c_min = bck[x+1][d];
+
+ bck[x][0] = acc[x][0] - c_min + std::min (bck[x+1][0], std::min (bck[x+1][1] + static_cast<float> (smoothness_weak_), c_min + static_cast<float> (smoothness_strong_)));
+ for (int d = 1; d < max_disp_ - 1; d++)
+ bck[x][d] = acc[x][d] - c_min + std::min (std::min(bck[x+1][d], bck[x+1][d-1] + static_cast<float> (smoothness_weak_)), std::min (bck[x+1][d+1] + static_cast<float> (smoothness_weak_), c_min + static_cast<float> (smoothness_strong_)));
+ bck[x][max_disp_-1] = acc[x][max_disp_-1] - c_min + std::min (bck[x+1][max_disp_-1], std::min (bck[x+1][max_disp_-2] + static_cast<float> (smoothness_weak_), c_min + static_cast<float> (smoothness_strong_)));
+ }//x
+
+ //last scan
+ for (int x = x_off_ + max_disp_ + 1; x < width_; x++)
+ {
+ float c_min = std::numeric_limits<float>::max ();
+ short int dbest = 0;
+
+ for (int d = 0; d < max_disp_; d++)
+ {
+ acc[x][d] = fwd[x][d] + bck[x][d];
+ if (acc[x][d] < c_min)
+ {
+ c_min = acc[x][d];
+ dbest = static_cast<short int> (d);
+ }
+ }
+
+ if (ratio_filter_ > 0)
+ dbest = doStereoRatioFilter (acc[x], dbest, c_min, ratio_filter_, max_disp_);
+ if (peak_filter_ > 0)
+ dbest = doStereoPeakFilter (acc[x], dbest, peak_filter_, max_disp_);
+
+ disp_map_[y*width_+x] = static_cast<short int> (dbest * 16);
+
+ //subpixel refinement
+ if (dbest > 0 && dbest < max_disp_ - 1)
+ disp_map_[y*width_+x] = computeStereoSubpixel (dbest, acc[x][dbest-1], acc[x][dbest], acc[x][dbest+1]);
+ } //x
+ }//y
+
+ for (int x = 0; x < width_; x++)
+ {
+ delete [] fwd[x];
+ delete [] bck[x];
+ delete [] acc[x];
+ }
+ delete [] fwd;
+ delete [] bck;
+ delete [] acc;
+
+ delete [] wl;
+ delete [] ds;
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ */
+
+/** \brief Block based (or fixed window) Stereo Matching algorithm implementation
+ * please see related documentation on stereo/stereo_matching.h
+ *
+ * \author Federico Tombari (federico.tombari@unibo.it)
+ * \ingroup stereo
+ */
+
+
+#include "pcl/stereo/stereo_matching.h"
+
+//////////////////////////////////////////////////////////////////////////////
+pcl::BlockBasedStereoMatching::BlockBasedStereoMatching ()
+{
+ radius_ = 5; //default value
+}
+
+//////////////////////////////////////////////////////////////////////////////
+void
+pcl::BlockBasedStereoMatching::compute_impl (unsigned char* ref_img, unsigned char* trg_img)
+{
+ int n = radius_ * 2 + 1;
+ int sad_min;
+ short int dbest = 0;
+
+ int sad_max = std::numeric_limits<int>::max ();
+
+ int *acc = new int[max_disp_];
+ memset (acc, 0, sizeof (int) * max_disp_);
+
+ int **v = new int *[width_];
+ for (int d = 0; d < width_; d++)
+ {
+ v[d] = new int[max_disp_];
+ memset (v[d], 0, sizeof (int) * max_disp_);
+ }
+
+ //First row
+ for (int x = max_disp_ + x_off_; x < max_disp_ + x_off_ + n; x++)
+ {
+ for (int d = 0; d < max_disp_; d++)
+ {
+ for (int y = 0; y < n; y++)
+ v[x][d] += abs (ref_img[y*width_+x] - trg_img[y*width_+x -d-x_off_]);
+ //acc[d] += V[x][d];
+ }
+ }
+
+ //STAGE 2: other positions
+ for (int x = max_disp_ + x_off_ + radius_ + 1; x < width_ - radius_; x++)
+ {
+ for (int d = 0; d < max_disp_; d++)
+ {
+ for (int y = 0; y < n; y++)
+ {
+ v[x+radius_][d] += abs (ref_img[y*width_ + x+radius_] - trg_img[ y*width_ + x+radius_ -d-x_off_]);
+ }
+ acc[d] = acc[d] + v[x+radius_][d] - v[x-radius_-1][d];
+ }//d
+ }//x
+
+ //2
+ unsigned char *lp, *rp, *lpp, *rpp;
+ int ind1 = radius_ + radius_ + max_disp_ + x_off_;
+
+ for (int y = radius_ + 1; y < height_ - radius_; y++)
+ {
+ //first position
+ for (int d = 0; d < max_disp_; d++)
+ {
+ acc[d] = 0;
+ for (int x = max_disp_ + x_off_; x < max_disp_ + x_off_ + n; x++)
+ {
+ v[x][d] = v[x][d] + abs( ref_img[ (y+radius_)*width_+x] - trg_img[ (y+radius_)*width_+x -d-x_off_] ) - abs( ref_img[ (y-radius_-1)*width_+x] - trg_img[ (y-radius_-1)*width_ +x -d-x_off_] );
+
+ acc[d] += v[x][d];
+ }
+ }
+
+ //all other positions
+ lp = static_cast<unsigned char*> (ref_img) + (y + radius_) * width_ + ind1;
+ rp = static_cast<unsigned char*> (trg_img) + (y + radius_) * width_ + ind1 - x_off_;
+ lpp = static_cast<unsigned char*> (ref_img) + (y - radius_ -1) * width_ + ind1;
+ rpp = static_cast<unsigned char*> (trg_img) + (y - radius_ -1) * width_ + ind1 - x_off_;
+
+ for (int x = max_disp_ + x_off_ + radius_ + 1; x < width_ - radius_; x++)
+ {
+ sad_min = sad_max;
+
+ lp++;
+ rp++;
+ lpp++;
+ rpp++;
+
+ for (int d = 0; d < max_disp_; d++)
+ {
+ v[x+radius_][d] = v[x+radius_][d] + abs( *lp - *rp ) - abs( *lpp - *rpp );
+ rp--;
+ rpp--;
+
+ acc[d] = acc[d] + v[x+radius_][d] - v[x-radius_-1][d];
+
+ if (acc[d] < sad_min)
+ {
+ sad_min = acc[d];
+ dbest = static_cast<short int> (d);
+ }
+ }
+
+ rp += max_disp_;
+ rpp += max_disp_;
+
+ if (ratio_filter_ > 0)
+ dbest = doStereoRatioFilter (acc, dbest, sad_min, ratio_filter_, max_disp_);
+ if (peak_filter_ > 0)
+ dbest = doStereoPeakFilter (acc, dbest, peak_filter_, max_disp_);
+
+ disp_map_[y * width_ + x] = static_cast<short int> (dbest * 16);
+
+ //subpixel refinement
+ if (dbest > 0 && dbest < max_disp_ - 1)
+ disp_map_[y*width_+x] = computeStereoSubpixel (dbest, acc[dbest-1], acc[dbest], acc[dbest+1]);
+
+ }//x
+ }//y
+
+ for (int d = 0; d < width_; d++)
+ delete [] v[d];
+ delete [] v;
+ delete [] acc;
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <pcl/stereo/stereo_grabber.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+///////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////// GrabberImplementation //////////////////////
+struct pcl::StereoGrabberBase::StereoGrabberImpl
+{
+ StereoGrabberImpl (pcl::StereoGrabberBase& grabber,
+ const std::pair<std::string, std::string>& pair_files,
+ float frames_per_second,
+ bool repeat);
+ StereoGrabberImpl (pcl::StereoGrabberBase& grabber,
+ const std::vector<std::pair<std::string, std::string> >& files,
+ float frames_per_second,
+ bool repeat);
+ void trigger ();
+ void readAhead ();
+
+ pcl::StereoGrabberBase& grabber_;
+ float frames_per_second_;
+ bool repeat_;
+ bool running_;
+ std::vector<std::pair<std::string, std::string> > pair_files_;
+ std::vector<std::pair<std::string, std::string> >::iterator pair_iterator_;
+ TimeTrigger time_trigger_;
+
+ pcl::PCLPointCloud2 next_cloud_;
+ Eigen::Vector4f origin_;
+ Eigen::Quaternionf orientation_;
+ bool valid_;
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////
+pcl::StereoGrabberBase::StereoGrabberImpl::StereoGrabberImpl (pcl::StereoGrabberBase& grabber,
+ const std::pair<std::string, std::string>& pair_files,
+ float frames_per_second,
+ bool repeat)
+ : grabber_ (grabber)
+ , frames_per_second_ (frames_per_second)
+ , repeat_ (repeat)
+ , running_ (false)
+ , pair_files_ ()
+ , pair_iterator_ ()
+ , time_trigger_ (1.0 / static_cast<double> (std::max (frames_per_second, 0.001f)), boost::bind (&StereoGrabberImpl::trigger, this))
+ , next_cloud_ ()
+ , origin_ ()
+ , orientation_ ()
+ , valid_ (false)
+{
+ pair_files_.push_back (pair_files);
+ pair_iterator_ = pair_files_.begin ();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+pcl::StereoGrabberBase::StereoGrabberImpl::StereoGrabberImpl (pcl::StereoGrabberBase& grabber,
+ const std::vector<std::pair<std::string, std::string> >& files,
+ float frames_per_second,
+ bool repeat)
+ : grabber_ (grabber)
+ , frames_per_second_ (frames_per_second)
+ , repeat_ (repeat)
+ , running_ (false)
+ , pair_files_ ()
+ , pair_iterator_ ()
+ , time_trigger_ (1.0 / static_cast<double> (std::max (frames_per_second, 0.001f)), boost::bind (&StereoGrabberImpl::trigger, this))
+ , next_cloud_ ()
+ , origin_ ()
+ , orientation_ ()
+ , valid_ (false)
+{
+ pair_files_ = files;
+ pair_iterator_ = pair_files_.begin ();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::StereoGrabberBase::StereoGrabberImpl::readAhead ()
+{
+ if (pair_iterator_ != pair_files_.end ())
+ {
+ // read next image pair and produce a cloud
+ //valid_ = //(reader.read (*pair_iterator_, next_cloud_, origin_, orientation_, pcd_version) == 0);
+
+ if (++pair_iterator_ == pair_files_.end () && repeat_)
+ pair_iterator_ = pair_files_.begin ();
+ }
+ else
+ valid_ = false;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::StereoGrabberBase::StereoGrabberImpl::trigger ()
+{
+ // If the stereo image pair was successfully read and a cloud was produced, simply publish it
+ if (valid_)
+ grabber_.publish (next_cloud_, origin_, orientation_);
+
+ // use remaining time, if there is time left!
+ readAhead ();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////// GrabberBase //////////////////////
+pcl::StereoGrabberBase::StereoGrabberBase (const std::pair<std::string, std::string>& pair_files,
+ float frames_per_second,
+ bool repeat)
+ : impl_ (new StereoGrabberImpl (*this, pair_files, frames_per_second, repeat))
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+pcl::StereoGrabberBase::StereoGrabberBase (const std::vector<std::pair<std::string, std::string> >& files,
+ float frames_per_second,
+ bool repeat)
+ : impl_ (new StereoGrabberImpl (*this, files, frames_per_second, repeat))
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+pcl::StereoGrabberBase::~StereoGrabberBase () throw ()
+{
+ stop ();
+ delete impl_;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::StereoGrabberBase::start ()
+{
+ if (impl_->frames_per_second_ > 0)
+ {
+ impl_->running_ = true;
+ impl_->time_trigger_.start ();
+ }
+ else // manual trigger
+ impl_->trigger ();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::StereoGrabberBase::stop ()
+{
+ if (impl_->frames_per_second_ > 0)
+ {
+ impl_->time_trigger_.stop ();
+ impl_->running_ = false;
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::StereoGrabberBase::trigger ()
+{
+ if (impl_->frames_per_second_ > 0)
+ return;
+ impl_->trigger ();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::StereoGrabberBase::isRunning () const
+{
+ return (impl_->running_);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+std::string
+pcl::StereoGrabberBase::getName () const
+{
+ return ("StereoGrabber");
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::StereoGrabberBase::rewind ()
+{
+ impl_->pair_iterator_ = impl_->pair_files_.begin ();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+float
+pcl::StereoGrabberBase::getFramesPerSecond () const
+{
+ return (impl_->frames_per_second_);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::StereoGrabberBase::isRepeatOn () const
+{
+ return (impl_->repeat_);
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ */
+
+
+/** \brief please see related documentation on stereo/stereo_matching.h
+ *
+ * \author Federico Tombari (federico.tombari@unibo.it)
+ * \ingroup stereo
+ */
+
+#include "pcl/stereo/stereo_matching.h"
+
+//////////////////////////////////////////////////////////////////////////////
+pcl::StereoMatching::StereoMatching (void)
+{
+ disp_map_ = NULL;
+ disp_map_trg_ = NULL;
+
+ ref_img_ = NULL;
+ trg_img_ = NULL;
+
+ pp_ref_img_ = NULL;
+ pp_trg_img_ = NULL;
+
+ width_ = -1;
+ height_ = -1;
+
+ max_disp_ = -1;
+ x_off_ = 0;
+
+ ratio_filter_ = 0;
+ peak_filter_ = 0;
+
+ is_pre_proc_ = false;
+ is_lr_check_ = false;
+ lr_check_th_ = 1;
+
+}
+
+//////////////////////////////////////////////////////////////////////////////
+pcl::StereoMatching::~StereoMatching (void)
+{
+ if ( disp_map_ != NULL)
+ {
+ delete [] disp_map_;
+ //disp_map_ = NULL;
+ }
+
+ if ( disp_map_trg_ != NULL)
+ {
+ delete [] disp_map_trg_;
+ //disp_map_trg_ = NULL;
+ }
+
+ if ( ref_img_ != NULL)
+ {
+ delete [] ref_img_;
+ delete [] trg_img_;
+ }
+
+ if ( pp_ref_img_ != NULL)
+ {
+ delete [] pp_ref_img_;
+ delete [] pp_trg_img_;
+ }
+
+}
+
+//////////////////////////////////////////////////////////////////////////////
+void
+pcl::StereoMatching::medianFilter (int radius)
+{
+
+ //TODO: do median filter
+ int side = radius*2+1;
+
+ short int *out = new short int [width_*height_];
+ memset(out, 0, width_*height_*sizeof(short int));
+
+ short int *v = new short int [side*side];
+
+ for(int y=radius; y<height_-radius; y++)
+ {
+ for(int x=radius; x<width_-radius; x++){
+
+ if(disp_map_[y*width_+x] <= 0)
+ out[y*width_+x] = disp_map_[y*width_+x];
+ else
+ {
+
+ int n=0;
+ for(int j=-radius; j<=radius; j++)
+ {
+ for(int i=-radius; i<=radius; i++)
+ {
+ if(disp_map_[(y+j)*width_ + x+i] > 0)
+ {
+ v[n] = disp_map_[(y+j)*width_ + x+i];
+ n++;
+ }
+ }
+ }
+
+ std::sort(v, v+n);
+ out[y*width_+x] = v[n/2];
+ }
+ }
+ }
+
+ short int* temp_ptr = out;
+ out = disp_map_;
+ disp_map_ = temp_ptr;
+
+ delete [] out;
+ delete [] v;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+void
+pcl::StereoMatching::getVisualMap (pcl::PointCloud<pcl::RGB>::Ptr vMap)
+{
+ if ( static_cast<int> (vMap->width) != width_ || static_cast<int> (vMap->height) != height_)
+ {
+ vMap->resize(width_*height_);
+ vMap->width = width_;
+ vMap->height = height_;
+ }
+
+ if ( vMap->is_dense)
+ vMap->is_dense = false;
+
+ pcl::RGB invalid_val;
+ invalid_val.r = 0;
+ invalid_val.g = 255;
+ invalid_val.b = 0;
+
+ float scale = 255.0f / (16.0f * static_cast<float> (max_disp_));
+
+ for (int y = 0; y<height_; y++)
+ {
+ for (int x = 0; x<width_; x++)
+ {
+ if (disp_map_[y * width_+ x] <= 0)
+ {
+ vMap->at (x,y) = invalid_val;
+ }
+ else
+ {
+ unsigned char val = static_cast<unsigned char> (floor (scale*disp_map_[y * width_+ x]));
+ vMap->at (x, y).r = val;
+ vMap->at (x, y).g = val;
+ vMap->at (x, y).b = val;
+ }
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+void
+pcl::StereoMatching::leftRightCheck ()
+{
+ short int p1, p2, p2i;
+
+ for (int y = 0; y < height_; y++)
+ {
+ for (int x = 0; x < width_; x++)
+ {
+ if (disp_map_[y * width_ + x] > 0)
+ {
+ p1 = disp_map_[y * width_ + x] / 16;
+
+ p2i = static_cast<short int> (x - p1 - x_off_);
+
+ if (p2i >= 0)
+ {
+ p2 = disp_map_trg_[y * width_ + p2i] / 16;
+
+ if (abs (p1 - p2) > lr_check_th_)
+ disp_map_[y* width_ + x] = -8;
+ }
+ }
+ }
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+bool
+pcl::StereoMatching::getPointCloud (
+ float u_c, float v_c, float focal, float baseline,
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
+ pcl::PointCloud<pcl::RGB>::Ptr texture)
+{
+ //disp map has not been computed yet..
+ if (disp_map_ == NULL)
+ {
+ PCL_ERROR ("[pcl::StereoMatching::getPointCloud] Error: a disparity map has not been computed yet. The resulting cloud can not be computed..\n");
+
+ return (false);
+ }
+
+ if (static_cast<int> (texture->width) != width_ || static_cast<int> (texture->height) != height_)
+ {
+ PCL_ERROR("[pcl::StereoMatching::getPointCloud] Error: the size of the texture cloud does not match that of the computed range map. The resulting cloud can not be computed..\n");
+ return (false);
+ }
+
+ //cloud needs to be re-allocated
+ if (static_cast<int> (cloud->width) != width_ || static_cast<int> (cloud->height) != height_)
+ {
+ //cloud.reset(new pcl::PointCloud<pcl::PointXYZRGBA>(width_, height_) );
+ cloud->resize (width_ * height_);
+ cloud->width = width_;
+ cloud->height = height_;
+ cloud->is_dense = false;
+ }
+
+ //Loop
+ pcl::PointXYZRGB temp_point;
+ /*pcl::PointXYZRGB nan_point;
+ nan_point.x = std::numeric_limits<float>::quiet_NaN();
+ nan_point.y = std::numeric_limits<float>::quiet_NaN();
+ nan_point.z = std::numeric_limits<float>::quiet_NaN();
+ nan_point.r = std::numeric_limits<unsigned char>::quiet_NaN();
+ nan_point.g = std::numeric_limits<unsigned char>::quiet_NaN();
+ nan_point.b = std::numeric_limits<unsigned char>::quiet_NaN();*/
+
+ //all disparities are multiplied by a constant equal to 16;
+ //this must be taken into account when computing z values
+ float depth_scale = baseline * focal * 16.0f;
+
+ for (int j = 0; j < height_; j++)
+ {
+ for (int i = 0; i < width_; i++)
+ {
+ if (disp_map_[ j*width_ + i] > 0)
+ {
+ temp_point.z = (depth_scale) / (disp_map_[ j*width_ + i]);
+ temp_point.x = ((static_cast<float> (i) - u_c) * temp_point.z) / focal;
+ temp_point.y = ((static_cast<float> (j) - v_c) * temp_point.z) / focal;
+
+ //temp_point.intensity = ( texture->at(j*width_+i).r +texture->at(j*width_+i).g + texture->at(j*width_+i).b) / 3.0f;
+ temp_point.r = texture->at (j * width_ + i).r;
+ temp_point.g = texture->at (j * width_ + i).g;
+ temp_point.b = texture->at (j * width_ + i).b;
+
+ (*cloud)[j*width_ + i] = temp_point;
+ }
+ //adding NaN value
+ else
+ {
+ temp_point.x = std::numeric_limits<float>::quiet_NaN();
+ temp_point.y = std::numeric_limits<float>::quiet_NaN();
+ temp_point.z = std::numeric_limits<float>::quiet_NaN();
+ temp_point.r = texture->at (j * width_ + i).r;
+ temp_point.g = texture->at (j * width_ + i).g;
+ temp_point.b = texture->at (j * width_ + i).b;
+ (*cloud)[j*width_ + i] = temp_point;
+ }
+ }
+ }
+
+ return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+//const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pcl::StereoMatching::getPointCloud(float uC, float vC, float focal, float baseline)
+bool
+pcl::StereoMatching::getPointCloud (
+ float u_c, float v_c, float focal, float baseline,
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
+{
+
+ //disp map has not been computed yet..
+ if ( disp_map_ == NULL)
+ {
+
+ PCL_ERROR(
+ "[pcl::StereoMatching::getPointCloud] Error: a disparity map has not been computed yet. The resulting cloud can not be computed..\n"
+ );
+
+ return false;
+ }
+
+ //cloud needs to be re-allocated
+ if (static_cast<int> (cloud->width) != width_ || static_cast<int> (cloud->height) != height_)
+ {
+ cloud->resize(width_*height_);
+ cloud->width = width_;
+ cloud->height = height_;
+ cloud->is_dense = false;
+ }
+
+ if ( cloud->is_dense)
+ cloud->is_dense = false;
+
+ //Loop
+ pcl::PointXYZ temp_point;
+ pcl::PointXYZ nan_point;
+ nan_point.x = std::numeric_limits<float>::quiet_NaN();
+ nan_point.y = std::numeric_limits<float>::quiet_NaN();
+ nan_point.z = std::numeric_limits<float>::quiet_NaN();
+ //nan_point.intensity = std::numeric_limits<float>::quiet_NaN();
+
+ //all disparities are multiplied by a constant equal to 16;
+ //this must be taken into account when computing z values
+ float depth_scale = baseline * focal * 16.0f;
+
+ for ( int j=0; j<height_; j++)
+ {
+ for ( int i=0; i<width_; i++)
+ {
+ if ( disp_map_[ j*width_ + i] > 0 )
+ {
+
+ temp_point.z = depth_scale / disp_map_[j * width_ + i];
+ temp_point.x = ((static_cast<float> (i) - u_c) * temp_point.z) / focal;
+ temp_point.y = ((static_cast<float> (j) - v_c) * temp_point.z) / focal;
+ //temp_point.intensity = 255;
+
+ (*cloud)[j * width_ + i] = temp_point;
+ }
+ //adding NaN value
+ else
+ {
+ (*cloud)[j * width_ + i] = nan_point;
+ }
+ }
+ }
+
+ return (true);
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+pcl::GrayStereoMatching::GrayStereoMatching ()
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////
+pcl::GrayStereoMatching::~GrayStereoMatching ()
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////
+void
+pcl::GrayStereoMatching::preProcessing (unsigned char *img, unsigned char *pp_img)
+{
+ int radius = 4; //default value, could be exported
+ int n = 2 * radius + 1;
+ int area = n * n;
+ int threshold = 31;
+
+ int sum = 0;
+ int *v = new int[width_];
+ memset (v, 0, sizeof (int) * width_);
+
+ for (int x = 0; x < n; x++)
+ for (int y = 0; y < n; y++)
+ v[x] += img[y*width_+x];
+
+ for (int x = radius + 1; x < width_ - radius; x++)
+ for (int y = 0; y<n; y++)
+ v[x+radius] += img[y*width_ + x+radius];
+
+ for (int y = 0; y <= radius; y++)
+ for (int x = 0; x < width_; x++)
+ pp_img[y*width_+x] = img[y*width_+x];
+
+ for (int y = radius + 1; y < height_ - radius; y++)
+ {
+ for (int x = 0; x <= radius; x++)
+ pp_img[y*width_+x] = img[y*width_+x];
+
+ sum = 0;
+ for (int x = 0; x<n; x++)
+ {
+ v[x] = v[x] + img[ (y+radius)*width_+x] - img[ (y-radius-1)*width_+x];
+ sum += v[x];
+ }
+
+ for (int x = radius + 1; x < width_ - radius; x++)
+ {
+ v[x+radius] = v[x+radius] + img[ (y+radius)*width_ + x+radius] - img[ (y-radius-1)*width_+ x+radius];
+ sum = sum + v[x+radius] - v[x-radius-1];
+
+ short int temp = static_cast<short int> (img[y*width_+x] - (sum / area));
+
+ if(temp < -threshold)
+ pp_img[y*width_+x] = 0;
+ else if(temp > threshold)
+ pp_img[y*width_+x] = static_cast<unsigned char> (threshold + threshold);
+ else
+ pp_img[y*width_+x] = static_cast<unsigned char> (temp + threshold);
+
+ }
+
+ for (int x = width_-radius; x<width_; x++)
+ {
+ pp_img[y*width_+x] = img[y*width_+x];
+ }
+ }
+
+ for (int y = height_ - radius; y < height_; y++)
+ {
+ for (int x = 0; x < width_; x++)
+ {
+ pp_img[y*width_+x] = img[y*width_+x];
+ }
+ }
+
+ delete [] v;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+void
+pcl::GrayStereoMatching::imgFlip (unsigned char * & img)
+{
+ unsigned char *temp_row = new unsigned char[width_];
+
+ for (int j = 0; j < height_; j++)
+ {
+ memcpy(temp_row, img + j*width_, sizeof(unsigned char) * width_);
+ for (int i = 0; i < width_; i++)
+ {
+ img[j * width_ + i] = temp_row[width_ - 1 - i];
+ }
+ }
+
+ delete [] temp_row;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+void
+pcl::GrayStereoMatching::compute (pcl::PointCloud<pcl::RGB> &ref, pcl::PointCloud<pcl::RGB> &trg)
+{
+
+ if (ref.width != trg.width || ref.height != trg.height)
+ {
+
+ PCL_ERROR(
+ "[pcl::GrayStereoMatching::compute] Error. The two input clouds have different sizes. Aborting..\n"
+ );
+ return;
+ }
+
+ if ( (ref_img_ != NULL) && (width_ != static_cast<int> (ref.width) || height_ != static_cast<int> (ref.height)) )
+ {
+ delete [] ref_img_;
+ delete [] trg_img_;
+
+ ref_img_ = NULL;
+ trg_img_ = NULL;
+ }
+
+ if ( ref_img_ == NULL)
+ {
+ ref_img_ = new unsigned char[ref.width * ref.height];
+ trg_img_ = new unsigned char[ref.width * ref.height];
+ }
+
+ float divider = 1.0f / 3.0f;
+ for (unsigned int j = 0; j < ref.height; j++)
+ {
+ for (unsigned int i = 0; i < ref.width; i++)
+ {
+ ref_img_[j*ref.width + i] = static_cast<unsigned char> (static_cast<float> (ref[j*ref.width + i].r + ref[j*ref.width + i].g + ref[j*ref.width + i].b) * divider);
+ trg_img_[j*ref.width + i] = static_cast<unsigned char> (static_cast<float> (trg[j*ref.width + i].r + trg[j*ref.width + i].g + trg[j*ref.width + i].b) * divider);
+ //ref_img_[ j*ref.width + i] = ( ref(j,i).r + ref(j,i).g + ref(j,i).b) / 3;
+ //trg_img_[ j*ref.width + i] = ( trg(j,i).r + trg(j,i).g + trg(j,i).b) / 3;
+
+ }
+ }
+
+ compute(ref_img_, trg_img_, ref.width, ref.height);
+
+}
+
+//////////////////////////////////////////////////////////////////////////////
+void
+pcl::GrayStereoMatching::compute (unsigned char* ref_img, unsigned char* trg_img, int width, int height)
+{
+
+ //Check that a suitable value of max_disp has been selected
+ if ( max_disp_ <= 0)
+ {
+ PCL_ERROR(
+ "[pcl::StereoMatching::compute] Error. A positive max_disparity value has not be correctly inserted. Aborting..\n"
+ );
+ return;
+ }
+
+ if ( (disp_map_ != NULL) && (width_ != width || height_ != height) )
+ {
+ delete [] disp_map_;
+ disp_map_ = NULL;
+
+ if ( disp_map_trg_ != NULL)
+ {
+ delete [] disp_map_trg_;
+ disp_map_trg_ = NULL;
+ }
+
+ if ( pp_ref_img_ != NULL)
+ {
+ delete [] pp_ref_img_;
+ delete [] pp_trg_img_;
+ pp_ref_img_ = NULL;
+ pp_trg_img_ = NULL;
+ }
+ }
+
+ if ( disp_map_ == NULL)
+ {
+ disp_map_ = new short int[width * height];
+
+ width_ = width;
+ height_ = height;
+ }
+
+
+ if ( is_lr_check_ && disp_map_trg_ == NULL)
+ {
+ disp_map_trg_ = new short int[width * height];
+ }
+
+ if ( !is_lr_check_ && disp_map_trg_ != NULL)
+ {
+ delete [] disp_map_trg_;
+ disp_map_trg_ = NULL;
+ }
+
+ if ( is_pre_proc_ && pp_ref_img_ == NULL)
+ {
+ pp_ref_img_ = new unsigned char[width_*height_];
+ pp_trg_img_ = new unsigned char[width_*height_];
+ }
+
+ if ( !is_pre_proc_ && pp_ref_img_ != NULL)
+ {
+ delete [] pp_ref_img_;
+ delete [] pp_trg_img_;
+ pp_ref_img_ = NULL;
+ pp_trg_img_ = NULL;
+ }
+
+ memset(disp_map_, 0, sizeof(short int)*height_*width_);
+
+ if ( is_pre_proc_)
+ {
+ preProcessing(ref_img, pp_ref_img_);
+ preProcessing(trg_img, pp_trg_img_);
+ }
+
+ if (is_lr_check_)
+ {
+
+ if ( is_pre_proc_)
+ {
+ imgFlip(pp_ref_img_);
+ imgFlip(pp_trg_img_);
+
+ compute_impl(pp_trg_img_, pp_ref_img_);
+
+ imgFlip(pp_ref_img_);
+ imgFlip(pp_trg_img_);
+ }
+ else
+ {
+ imgFlip(ref_img);
+ imgFlip(trg_img);
+
+ compute_impl(trg_img, ref_img);
+
+ imgFlip(ref_img);
+ imgFlip(trg_img);
+ }
+
+ for (int j = 0; j < height_; j++)
+ for (int i = 0; i < width_; i++)
+ disp_map_trg_[j * width_ + i] = disp_map_[j * width_ + width_ - 1 - i];
+
+ }
+
+ if ( is_pre_proc_)
+ compute_impl(pp_ref_img_, pp_trg_img_);
+ else
+ compute_impl(ref_img, trg_img);
+
+ if ( is_lr_check_)
+ {
+ leftRightCheck();
+ }
+
+ //at the end, x_offset (*16) needs to be added to all computed disparities,
+ //so that each fixed point value of the disparity map represents the true disparity value multiplied by 16
+ for (int j = 0; j < height_; j++)
+ for (int i = 0; i < width_; i++)
+ if ( disp_map_[j * width_ + i] > 0)
+ disp_map_[j * width_ + i] += static_cast<short int> (x_off_ * 16);
+
+}
+
if(build)
if(QHULL_FOUND)
- include_directories(${QHULL_INCLUDE_DIRS})
set(HULL_INCLUDES
"include/pcl/${SUBSYS_NAME}/concave_hull.h"
"include/pcl/${SUBSYS_NAME}/convex_hull.h"
#if !defined(OPENNURBS_DEFINES_INC_)
#define OPENNURBS_DEFINES_INC_
+#include <pcl/pcl_exports.h>
+
#if defined (cplusplus) || defined(_cplusplus) || defined(__cplusplus) || defined(ON_CPLUSPLUS)
// C++ extern "C" declaration for C linkage
#if defined(ON_CPLUSPLUS)
// OpenNurbs enums
-class ON_CLASS ON
+class PCL_EXPORTS ON_CLASS ON
{
public:
/*
#if !defined(OPENNURBS_EXTENSIONS_INC_)
#define OPENNURBS_EXTENSIONS_INC_
+#include <pcl/pcl_exports.h>
+
/*
Description:
Used to store user data information in an ONX_Model.
*/
-class ON_CLASS ONX_Model_UserData
+class PCL_EXPORTS ON_CLASS ONX_Model_UserData
{
public:
#if defined(ON_DLL_EXPORTS) || defined(ON_DLL_IMPORTS)
Description:
Used to store geometry table object definition and attributes in an ONX_Model.
*/
-class ON_CLASS ONX_Model_Object
+class PCL_EXPORTS ON_CLASS ONX_Model_Object
{
public:
#if defined(ON_DLL_EXPORTS) || defined(ON_DLL_IMPORTS)
Description:
Used to store render light table light definition and attributes in an ONX_Model.
*/
-class ON_CLASS ONX_Model_RenderLight
+class PCL_EXPORTS ON_CLASS ONX_Model_RenderLight
{
public:
#if defined(ON_DLL_EXPORTS) || defined(ON_DLL_IMPORTS)
read from 3dm archives. Please study example_read.cpp for
details.
*/
-class ON_CLASS ONX_Model
+class PCL_EXPORTS ON_CLASS ONX_Model
{
public:
#if defined(ON_DLL_EXPORTS) || defined(ON_DLL_IMPORTS)
#if !defined(ON_STRING_INC_)
#define ON_STRING_INC_
-
+#include <pcl/pcl_exports.h>
/*
Description:
// ON_wString
//
-class ON_CLASS ON_wString
+class PCL_EXPORTS ON_CLASS ON_wString
{
public:
1: The input parameters were invalid.
This error cannot be masked.
- 2: The ouput buffer was not large enough to hold the converted
+ 2: The output buffer was not large enough to hold the converted
string. As much conversion as possible is performed in this
case and the error cannot be masked.
#ifndef HASH_INCLUDED
#define HASH_INCLUDED
#if defined _WIN32 && !defined __MINGW32__
+#define _SILENCE_STDEXT_HASH_DEPRECATION_WARNINGS
#include <hash_map>
using namespace stdext;
#else // !_WIN32 || __MINGW32__
PCL_ERROR ("[pcl::%s::setDimension] Invalid input dimension specified!\n", getClassName ().c_str ());
}
+ /** \brief Retrieve the indices of the input point cloud that for the convex hull.
+ *
+ * \note Should only be called after reconstruction was performed and if the ConcaveHull is
+ * set to preserve information via setKeepInformation ().
+ *
+ * \param[out] hull_point_indices The indices of the points forming the point cloud
+ */
+ void
+ getHullPointIndices (pcl::PointIndices &hull_point_indices) const;
+
protected:
/** \brief Class get name method. */
std::string
/** \brief the dimensionality of the concave hull */
int dim_;
+
+ /** \brief vector containing the point cloud indices of the convex hull points. */
+ pcl::PointIndices hull_indices_;
};
}
/** \brief If set to true, the qhull library is called to compute the total area and volume of the convex hull.
* NOTE: When this option is activated, the qhull library produces output to the console.
- * \param[in] value wheter to compute the area and the volume, default is false
+ * \param[in] value whether to compute the area and the volume, default is false
*/
void
setComputeAreaVolume (bool value)
return (dimension_);
}
+ /** \brief Retrieve the indices of the input point cloud that for the convex hull.
+ *
+ * \note Should only be called after reconstruction was performed.
+ * \param[out] hull_point_indices The indices of the points forming the point cloud
+ */
+ void
+ getHullPointIndices (pcl::PointIndices &hull_point_indices) const;
+
protected:
/** \brief The actual reconstruction method.
*
/* \brief z-axis - for checking valid projections. */
const Eigen::Vector3d z_axis_;
+ /* \brief vector containing the point cloud indices of the convex hull points. */
+ pcl::PointIndices hull_indices_;
+
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/** \brief Temporary variable to store a triangle (as a set of point indices) **/
pcl::Vertices triangle_;
/** \brief Temporary variable to store point coordinates **/
- std::vector<Eigen::Vector3f> coords_;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > coords_;
/** \brief A list of angles to neighbors **/
std::vector<nnAngle> angles_;
distances.resize (1);
// for each point in the concave hull, search for the nearest neighbor in the original point cloud
-
- std::vector<int> indices;
- indices.resize (alpha_shape.points.size ());
+ hull_indices_.header = input_->header;
+ hull_indices_.indices.clear ();
+ hull_indices_.indices.reserve (alpha_shape.points.size ());
for (size_t i = 0; i < alpha_shape.points.size (); i++)
{
tree.nearestKSearch (alpha_shape.points[i], 1, neighbor, distances);
- indices[i] = neighbor[0];
+ hull_indices_.indices.push_back (neighbor[0]);
}
// replace point with the closest neighbor in the original point cloud
- pcl::copyPointCloud (*input_, indices, alpha_shape);
+ pcl::copyPointCloud (*input_, hull_indices_.indices, alpha_shape);
}
}
#ifdef __GNUC__
performReconstruction (hull_points, polygons);
}
+//////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointInT> void
+pcl::ConcaveHull<PointInT>::getHullPointIndices (pcl::PointIndices &hull_point_indices) const
+{
+ hull_point_indices = hull_indices_;
+}
#define PCL_INSTANTIATE_ConcaveHull(T) template class PCL_EXPORTS pcl::ConcaveHull<T>;
polygons.resize (1);
polygons[0].vertices.resize (hull.points.size ());
+ hull_indices_.header = input_->header;
+ hull_indices_.indices.clear ();
+ hull_indices_.indices.reserve (hull.points.size ());
+
for (int j = 0; j < static_cast<int> (hull.points.size ()); j++)
{
+ hull_indices_.indices.push_back ((*indices_)[idx_points[j].first]);
hull.points[j] = input_->points[(*indices_)[idx_points[j].first]];
polygons[0].vertices[j] = static_cast<unsigned int> (j);
}
++max_vertex_id;
std::vector<int> qhid_to_pcidx (max_vertex_id);
+ hull_indices_.header = input_->header;
+ hull_indices_.indices.clear ();
+ hull_indices_.indices.reserve (num_vertices);
+
FORALLvertices
{
- // Add vertices to hull point_cloud
- hull.points[i] = input_->points[(*indices_)[qh_pointid (vertex->point)]];
+ // Add vertices to hull point_cloud and store index
+ hull_indices_.indices.push_back ((*indices_)[qh_pointid (vertex->point)]);
+ hull.points[i] = input_->points[(*indices_)[hull_indices_.indices.back ()]];
qhid_to_pcidx[vertex->id] = i; // map the vertex id of qhull to the point cloud index
++i;
deinitCompute ();
}
+//////////////////////////////////////////////////////////////////////////
+template <typename PointInT> void
+pcl::ConvexHull<PointInT>::getHullPointIndices (pcl::PointIndices &hull_point_indices) const
+{
+ hull_point_indices = hull_indices_;
+}
#define PCL_INSTANTIATE_ConvexHull(T) template class PCL_EXPORTS pcl::ConvexHull<T>;
int is_free=0, nr_parts=0, increase_nnn4fn=0, increase_nnn4s=0, increase_dist=0, nr_touched = 0;
bool is_fringe;
angles_.resize(nnn_);
- std::vector<Eigen::Vector2f> uvn_nn (nnn_);
+ std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > uvn_nn (nnn_);
Eigen::Vector2f uvn_s;
// iterating through fringe points and finishing them until everything is done
center[1] = min_p_[1] + (max_p_[1] - min_p_[1]) * float (index_3d[1]) / float (res_y_);
center[2] = min_p_[2] + (max_p_[2] - min_p_[2]) * float (index_3d[2]) / float (res_z_);
- std::vector<Eigen::Vector3f> p;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > p;
p.resize (8);
for (int i = 0; i < 8; ++i)
{
w = M.fullPivLu ().solve (d);
std::vector<double> weights (2*N);
- std::vector<Eigen::Vector3d> centers (2*N);
+ std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > centers (2*N);
for (unsigned int i = 0; i < N; ++i)
{
centers[i] = Eigen::Vector3f (input_->points[i].getVector3fMap ()).cast<double> ();
double f = 0.0;
std::vector<double>::const_iterator w_it (weights.begin());
- for (std::vector<Eigen::Vector3d>::const_iterator c_it = centers.begin ();
+ for (std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >::const_iterator c_it = centers.begin ();
c_it != centers.end (); ++c_it, ++w_it)
f += *w_it * kernel (*c_it, point);
{
// Update neighborhood, since point was projected, and computing relative
// positions. Note updating only distances for the weights for speed
- std::vector<Eigen::Vector3d> de_meaned (nn_indices.size ());
+ std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > de_meaned (nn_indices.size ());
for (size_t ni = 0; ni < nn_indices.size (); ++ni)
{
de_meaned[ni][0] = input_->points[nn_indices[ni]].x - point[0];
#include <pcl/surface/texture_mapping.h>
///////////////////////////////////////////////////////////////////////////////////////////////
-template<typename PointInT> std::vector<Eigen::Vector2f>
+template<typename PointInT> std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >
pcl::TextureMapping<PointInT>::mapTexture2Face (
const Eigen::Vector3f &p1,
const Eigen::Vector3f &p2,
const Eigen::Vector3f &p3)
{
- std::vector<Eigen::Vector2f> tex_coordinates;
+ std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > tex_coordinates;
// process for each face
Eigen::Vector3f p1p2 (p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]);
Eigen::Vector3f p1p3 (p3[0] - p1[0], p3[1] - p1[1], p3[2] - p1[2]);
Eigen::Vector3f facet[3];
// texture coordinates for each mesh
- std::vector<std::vector<Eigen::Vector2f> > texture_map;
+ std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >texture_map;
for (size_t m = 0; m < tex_mesh.tex_polygons.size (); ++m)
{
// texture coordinates for each mesh
- std::vector<Eigen::Vector2f> texture_map_tmp;
+ std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
// processing for each face
for (size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i)
}
// get texture coordinates of each face
- std::vector<Eigen::Vector2f> tex_coordinates = mapTexture2Face (facet[0], facet[1], facet[2]);
+ std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > tex_coordinates = mapTexture2Face (facet[0], facet[1], facet[2]);
for (size_t n = 0; n < tex_coordinates.size (); ++n)
texture_map_tmp.push_back (tex_coordinates[n]);
}// end faces
float z_offset = 0 - z_lowest;
// texture coordinates for each mesh
- std::vector<std::vector<Eigen::Vector2f> > texture_map;
+ std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >texture_map;
for (size_t m = 0; m < tex_mesh.tex_polygons.size (); ++m)
{
// texture coordinates for each mesh
- std::vector<Eigen::Vector2f> texture_map_tmp;
+ std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
// processing for each face
for (size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i)
PCL_INFO ("You provided %d cameras and a mesh containing %d sub-meshes.\n", cams.size (), tex_mesh.tex_polygons.size ());
- pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud (new pcl::PointCloud<pcl::PointXYZ>);
- pcl::PointCloud<pcl::PointXYZ>::Ptr camera_transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ typename pcl::PointCloud<PointInT>::Ptr originalCloud (new pcl::PointCloud<PointInT>);
+ typename pcl::PointCloud<PointInT>::Ptr camera_transformed_cloud (new pcl::PointCloud<PointInT>);
// convert mesh's cloud to pcl format for ease
pcl::fromPCLPointCloud2 (tex_mesh.cloud, *originalCloud);
// texture coordinates for each mesh
- std::vector<std::vector<Eigen::Vector2f> > texture_map;
+ std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texture_map;
for (size_t m = 0; m < cams.size (); ++m)
{
pcl::transformPointCloud (*originalCloud, *camera_transformed_cloud, cam_trans.inverse ());
// vector of texture coordinates for each face
- std::vector<Eigen::Vector2f> texture_map_tmp;
+ std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
// processing each face visible by this camera
- pcl::PointXYZ pt;
+ PointInT pt;
size_t idx;
for (size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i)
{
}// end cameras
// push on extra empty UV map (for unseen faces) so that obj writer does not crash!
- std::vector<Eigen::Vector2f> texture_map_tmp;
+ std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
for (size_t i = 0; i < tex_mesh.tex_polygons[cams.size ()].size (); ++i)
for (size_t j = 0; j < tex_mesh.tex_polygons[cams.size ()][i].vertices.size (); ++j)
{
///////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT> bool
-pcl::TextureMapping<PointInT>::isPointOccluded (const pcl::PointXYZ &pt, OctreePtr octree)
+pcl::TextureMapping<PointInT>::isPointOccluded (const PointInT &pt, OctreePtr octree)
{
Eigen::Vector3f direction;
direction (0) = pt.x;
// copy mesh
cleaned_mesh = tex_mesh;
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
- pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ typename pcl::PointCloud<PointInT>::Ptr cloud (new pcl::PointCloud<PointInT>);
+ typename pcl::PointCloud<PointInT>::Ptr filtered_cloud (new pcl::PointCloud<PointInT>);
// load points into a PCL format
pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud);
// clear polygons from cleaned_mesh
sorted_mesh.tex_polygons.clear ();
- pcl::PointCloud<pcl::PointXYZ>::Ptr original_cloud (new pcl::PointCloud<pcl::PointXYZ>);
- pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
- pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ typename pcl::PointCloud<PointInT>::Ptr original_cloud (new pcl::PointCloud<PointInT>);
+ typename pcl::PointCloud<PointInT>::Ptr transformed_cloud (new pcl::PointCloud<PointInT>);
+ typename pcl::PointCloud<PointInT>::Ptr filtered_cloud (new pcl::PointCloud<PointInT>);
// load points into a PCL format
pcl::fromPCLPointCloud2 (tex_mesh.cloud, *original_cloud);
{
// point is not occluded
// does it land on the camera's image plane?
- pcl::PointXYZ pt = transformed_cloud->points[tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]];
+ PointInT pt = transformed_cloud->points[tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]];
Eigen::Vector2f dummy_UV;
if (!getPointUVCoordinates (pt, cameras[cam], dummy_UV))
{
double octree_voxel_size, bool show_nb_occlusions, int max_occlusions)
{
// load points into a PCL format
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ typename pcl::PointCloud<PointInT>::Ptr cloud (new pcl::PointCloud<PointInT>);
pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud);
showOcclusions (cloud, colored_cloud, octree_voxel_size, show_nb_occlusions, max_occlusions);
if (mesh.tex_polygons.size () != 1)
return;
- pcl::PointCloud<pcl::PointXYZ>::Ptr mesh_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ typename pcl::PointCloud<PointInT>::Ptr mesh_cloud (new pcl::PointCloud<PointInT>);
pcl::fromPCLPointCloud2 (mesh.cloud, *mesh_cloud);
PCL_INFO ("Processing camera %d of %d.\n", current_cam+1, cameras.size ());
// transform mesh into camera's frame
- pcl::PointCloud<pcl::PointXYZ>::Ptr camera_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ typename pcl::PointCloud<PointInT>::Ptr camera_cloud (new pcl::PointCloud<PointInT>);
pcl::transformPointCloud (*mesh_cloud, *camera_cloud, cameras[current_cam].pose.inverse ());
// CREATE UV MAP FOR CURRENT FACES
if (static_cast<int> (mesh.tex_coordinates.size ()) <= current_cam)
{
- std::vector<Eigen::Vector2f> dummy_container;
+ std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > dummy_container;
mesh.tex_coordinates.push_back (dummy_container);
}
mesh.tex_coordinates[current_cam].resize (3 * visibility.size ());
if (mesh.tex_coordinates.size() <= cameras.size ())
{
- std::vector<Eigen::Vector2f> dummy_container;
+ std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > dummy_container;
mesh.tex_coordinates.push_back(dummy_container);
}
///////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT> inline bool
-pcl::TextureMapping<PointInT>::getPointUVCoordinates(const pcl::PointXYZ &pt, const Camera &cam, pcl::PointXY &UV_coordinates)
+pcl::TextureMapping<PointInT>::getPointUVCoordinates(const PointInT &pt, const Camera &cam, pcl::PointXY &UV_coordinates)
{
if (pt.z > 0)
{
///////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT> inline bool
-pcl::TextureMapping<PointInT>::isFaceProjected (const Camera &camera, const pcl::PointXYZ &p1, const pcl::PointXYZ &p2, const pcl::PointXYZ &p3, pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3)
+pcl::TextureMapping<PointInT>::isFaceProjected (const Camera &camera, const PointInT &p1, const PointInT &p2, const PointInT &p3, pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3)
{
return (getPointUVCoordinates(p1, camera, proj1)
&&
* In Proceedings of the 12th International Conference on Intelligent Autonomous Systems (IAS),
* Jeju Island, Korea, June 26-29 2012.
* <a href="http://purl.org/holz/papers/holz_2012_ias.pdf">http://purl.org/holz/papers/holz_2012_ias.pdf</a>
- *
+ *
* \author Dirk Holz, Radu B. Rusu
* \ingroup surface
*/
/** \brief Constructor. Triangulation type defaults to \a QUAD_MESH. */
OrganizedFastMesh ()
- : max_edge_length_squared_ (0.025f)
+ : max_edge_length_a_ (0.0f)
+ , max_edge_length_b_ (0.0f)
+ , max_edge_length_c_ (0.0f)
+ , max_edge_length_set_ (false)
+ , max_edge_length_dist_dependent_ (false)
, triangle_pixel_size_rows_ (1)
, triangle_pixel_size_columns_ (1)
, triangulation_type_ (QUAD_MESH)
+ , viewpoint_ (Eigen::Vector3f::Zero ())
, store_shadowed_faces_ (false)
, cos_angle_tolerance_ (fabsf (cosf (pcl::deg2rad (12.5f))))
+ , distance_tolerance_ (-1.0f)
+ , distance_dependent_ (false)
+ , use_depth_as_distance_(false)
{
check_tree_ = false;
};
/** \brief Destructor. */
virtual ~OrganizedFastMesh () {};
- /** \brief Set a maximum edge length. TODO: Implement!
- * \param[in] max_edge_length the maximum edge length
+ /** \brief Set a maximum edge length.
+ * Using not only the scalar \a a, but also \a b and \a c, allows for using a distance threshold in the form of:
+ * threshold(x) = c*x*x + b*x + a
+ * \param[in] a scalar coefficient of the (distance-dependent polynom) threshold
+ * \param[in] b linear coefficient of the (distance-dependent polynom) threshold
+ * \param[in] c quadratic coefficient of the (distance-dependent polynom) threshold
*/
inline void
- setMaxEdgeLength (float max_edge_length)
+ setMaxEdgeLength (float a, float b = 0.0f, float c = 0.0f)
{
- max_edge_length_squared_ = max_edge_length * max_edge_length;
+ max_edge_length_a_ = a;
+ max_edge_length_b_ = b;
+ max_edge_length_c_ = c;
+ if ((max_edge_length_a_ + max_edge_length_b_ + max_edge_length_c_) > std::numeric_limits<float>::min())
+ max_edge_length_set_ = true;
+ else
+ max_edge_length_set_ = false;
};
+ inline void
+ unsetMaxEdgeLength ()
+ {
+ max_edge_length_set_ = false;
+ }
+
/** \brief Set the edge length (in pixels) used for constructing the fixed mesh.
* \param[in] triangle_size edge length in pixels
* (Default: 1 = neighboring pixels are connected)
triangulation_type_ = type;
}
+ /** \brief Set the viewpoint from where the input point cloud has been acquired.
+ * \param[in] viewpoint Vector containing the viewpoint coordinates (in the coordinate system of the data)
+ */
+ inline void setViewpoint (const Eigen::Vector3f& viewpoint)
+ {
+ viewpoint_ = viewpoint;
+ }
+
+ /** \brief Get the viewpoint from where the input point cloud has been acquired. */
+ const inline Eigen::Vector3f& getViewpoint () const
+ {
+ return viewpoint_;
+ }
+
/** \brief Store shadowed faces or not.
- * \param[in] enable set to true to store shadowed faces
- */
+ * \param[in] enable set to true to store shadowed faces
+ */
inline void
storeShadowedFaces (bool enable)
{
store_shadowed_faces_ = enable;
}
+ /** \brief Set the angle tolerance used for checking whether or not an edge is occluded.
+ * Standard values are 5deg to 15deg (input in rad!). Set a value smaller than zero to
+ * disable the check for shadowed edges.
+ * \param[in] angle_tolerance Angle tolerance (in rad). Set a value <0 to disable.
+ */
+ inline void
+ setAngleTolerance(float angle_tolerance)
+ {
+ if (angle_tolerance > 0)
+ cos_angle_tolerance_ = fabsf (cosf (angle_tolerance));
+ else
+ cos_angle_tolerance_ = -1.0f;
+ }
+
+
+ inline void setDistanceTolerance(float distance_tolerance, bool depth_dependent = false)
+ {
+ distance_tolerance_ = distance_tolerance;
+ if (distance_tolerance_ < 0)
+ return;
+
+ distance_dependent_ = depth_dependent;
+ if (!distance_dependent_)
+ distance_tolerance_ *= distance_tolerance_;
+ }
+
+ /** \brief Use the points' depths (z-coordinates) instead of measured distances (points' distances to the viewpoint).
+ * \param[in] enable Set to true skips comptations and further speeds up computation by using depth instead of computing distance. false to disable. */
+ inline void useDepthAsDistance(bool enable)
+ {
+ use_depth_as_distance_ = enable;
+ }
+
protected:
- /** \brief max (squared) length of edge */
- float max_edge_length_squared_;
+ /** \brief max length of edge, scalar component */
+ float max_edge_length_a_;
+ /** \brief max length of edge, scalar component */
+ float max_edge_length_b_;
+ /** \brief max length of edge, scalar component */
+ float max_edge_length_c_;
+ /** \brief flag whether or not edges are limited in length */
+ bool max_edge_length_set_;
+
+ /** \brief flag whether or not max edge length is distance dependent. */
+ bool max_edge_length_dist_dependent_;
/** \brief size of triangle edges (in pixels) for iterating over rows. */
int triangle_pixel_size_rows_;
/** \brief Type of meshing scheme (quads vs. triangles, left cut vs. right cut ... */
TriangulationType triangulation_type_;
+ /** \brief Viewpoint from which the point cloud has been acquired (in the same coordinate frame as the data). */
+ Eigen::Vector3f viewpoint_;
+
/** \brief Whether or not shadowed faces are stored, e.g., for exploration */
bool store_shadowed_faces_;
/** \brief (Cosine of the) angle tolerance used when checking whether or not an edge between two points is shadowed. */
float cos_angle_tolerance_;
+ /** \brief distance tolerance for filtering out shadowed/occluded edges */
+ float distance_tolerance_;
+
+ /** \brief flag whether or not \a distance_tolerance_ is distance dependent (multiplied by the squared distance to the point) or not. */
+ bool distance_dependent_;
+
+ /** \brief flag whether or not the points' depths are used instead of measured distances (points' distances to the viewpoint).
+ This flag may be set using useDepthAsDistance(true) for (RGB-)Depth cameras to skip computations and gain additional speed up. */
+ bool use_depth_as_distance_;
+
+
/** \brief Perform the actual polygonal reconstruction.
* \param[out] polygons the resultant polygons
*/
inline bool
isShadowed (const PointInT& point_a, const PointInT& point_b)
{
- Eigen::Vector3f viewpoint = Eigen::Vector3f::Zero (); // TODO: allow for passing viewpoint information
- Eigen::Vector3f dir_a = viewpoint - point_a.getVector3fMap ();
+ bool valid = true;
+
+ Eigen::Vector3f dir_a = viewpoint_ - point_a.getVector3fMap ();
Eigen::Vector3f dir_b = point_b.getVector3fMap () - point_a.getVector3fMap ();
float distance_to_points = dir_a.norm ();
float distance_between_points = dir_b.norm ();
- float cos_angle = dir_a.dot (dir_b) / (distance_to_points*distance_between_points);
- if (cos_angle != cos_angle)
- cos_angle = 1.0f;
- return (fabs (cos_angle) >= cos_angle_tolerance_);
- // TODO: check for both: angle almost 0/180 _and_ distance between points larger than noise level
+
+ if (cos_angle_tolerance_ > 0)
+ {
+ float cos_angle = dir_a.dot (dir_b) / (distance_to_points*distance_between_points);
+ if (cos_angle != cos_angle)
+ cos_angle = 1.0f;
+ bool check_angle = fabs (cos_angle) >= cos_angle_tolerance_;
+
+ bool check_distance = true;
+ if (check_angle && (distance_tolerance_ > 0))
+ {
+ float dist_thresh = distance_tolerance_;
+ if (distance_dependent_)
+ {
+ float d = distance_to_points;
+ if (use_depth_as_distance_)
+ d = std::max(point_a.z, point_b.z);
+ dist_thresh *= d*d;
+ dist_thresh *= dist_thresh; // distance_tolerance_ is already squared if distance_dependent_ is false.
+ }
+ check_distance = (distance_between_points > dist_thresh);
+ }
+ valid = !(check_angle && check_distance);
+ }
+
+ // check if max. edge length is not exceeded
+ if (max_edge_length_set_)
+ {
+ float dist = (use_depth_as_distance_ ? std::max(point_a.z, point_b.z) : distance_to_points);
+ float dist_thresh = max_edge_length_a_;
+ if (fabs(max_edge_length_b_) > std::numeric_limits<float>::min())
+ dist_thresh += max_edge_length_b_ * dist;
+ if (fabs(max_edge_length_c_) > std::numeric_limits<float>::min())
+ dist_thresh += max_edge_length_c_ * dist * dist;
+ valid = (distance_between_points <= dist_thresh);
+ }
+
+ return !valid;
}
/** \brief Check if a triangle is valid.
{
public:
- typedef boost::shared_ptr< PointInT > Ptr;
- typedef boost::shared_ptr< const PointInT > ConstPtr;
+ typedef boost::shared_ptr< TextureMapping < PointInT > > Ptr;
+ typedef boost::shared_ptr< const TextureMapping < PointInT > > ConstPtr;
typedef pcl::PointCloud<PointInT> PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
* \returns false if the point is not visible by the camera
*/
inline bool
- getPointUVCoordinates (const pcl::PointXYZ &pt, const Camera &cam, Eigen::Vector2f &UV_coordinates)
+ getPointUVCoordinates (const PointInT &pt, const Camera &cam, Eigen::Vector2f &UV_coordinates)
{
// if the point is in front of the camera
if (pt.z > 0)
* \returns true if the point is occluded.
*/
inline bool
- isPointOccluded (const pcl::PointXYZ &pt, const OctreePtr octree);
+ isPointOccluded (const PointInT &pt, const OctreePtr octree);
/** \brief Remove occluded points from a point cloud
* \param[in] input_cloud the cloud on which to perform occlusion detection
* \param[in] p2 the second point
* \param[in] p3 the third point
*/
- std::vector<Eigen::Vector2f>
+ std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >
mapTexture2Face (const Eigen::Vector3f &p1, const Eigen::Vector3f &p2, const Eigen::Vector3f &p3);
/** \brief Returns the circumcenter of a triangle and the circle's radius.
* \returns false if the point is not visible by the camera
*/
inline bool
- getPointUVCoordinates (const pcl::PointXYZ &pt, const Camera &cam, pcl::PointXY &UV_coordinates);
+ getPointUVCoordinates (const PointInT &pt, const Camera &cam, pcl::PointXY &UV_coordinates);
/** \brief Returns true if all the vertices of one face are projected on the camera's image plane.
* \param[in] camera camera on which to project the face.
*/
inline bool
isFaceProjected (const Camera &camera,
- const pcl::PointXYZ &p1, const pcl::PointXYZ &p2, const pcl::PointXYZ &p3,
+ const PointInT &p1, const PointInT &p2, const PointInT &p3,
pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3);
/** \brief Returns True if a point lays within a triangle
while(pObject)
{
- rc = archive.Write3dmStartSection( version, "Archive created by ON_WriteOneObjectArchive "__DATE__" "__TIME__ );
+ rc = archive.Write3dmStartSection( version, "Archive created by ON_WriteOneObjectArchive " __DATE__ " " __TIME__ );
if ( !rc )
break;
0, // null output error status
(4|8|16), // mask common conversion errors
0, // error_code_point = null terminator inserted at point of conversion error
- 0 // null ouput end-of-string pointer
+ 0 // null output end-of-string pointer
);
// TODO
// Test m_dirent.d_name to make sure it passes m_ws/utf8_file_name_filter
#include <pcl/surface/impl/mls.hpp>
// Instantiations of specific point types
-PCL_INSTANTIATE_PRODUCT(MovingLeastSquares, ((pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA))
+PCL_INSTANTIATE_PRODUCT(MovingLeastSquares, ((pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)(pcl::PointXYZRGBNormal)(pcl::PointNormal))
((pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)(pcl::PointXYZRGBNormal)(pcl::PointNormal)))
#ifdef _OPENMP
-PCL_INSTANTIATE_PRODUCT(MovingLeastSquaresOMP, ((pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA))
+PCL_INSTANTIATE_PRODUCT(MovingLeastSquaresOMP, ((pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)(pcl::PointXYZRGBNormal)(pcl::PointNormal))
((pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)(pcl::PointXYZRGBNormal)(pcl::PointNormal)))
#endif
/// Ideally, we should instantiate like below, but it takes large amounts of main memory for compilation
*/
#include <pcl/surface/on_nurbs/fitting_curve_2d_apdm.h>
+#include <pcl/pcl_macros.h>
#include <stdexcept>
using namespace pcl;
void
FittingCurve2dAPDM::fitting (FitParameter ¶m)
{
- double avgerr (DBL_MAX);
- double maxerr (DBL_MAX);
bool stop (false);
for (unsigned j = 0; j < param.iterations && !stop; j++)
{
*/
#include <pcl/surface/on_nurbs/fitting_curve_2d_pdm.h>
+#include <pcl/pcl_macros.h>
#include <stdexcept>
using namespace pcl;
*/
#include <pcl/surface/on_nurbs/fitting_curve_pdm.h>
+#include <pcl/pcl_macros.h>
#include <stdexcept>
using namespace pcl;
#include <stdexcept>
#include <pcl/surface/on_nurbs/fitting_cylinder_pdm.h>
+#include <pcl/pcl_macros.h>
using namespace pcl;
using namespace on_nurbs;
#include <stdexcept>
#include <pcl/surface/on_nurbs/fitting_sphere_pdm.h>
+#include <pcl/pcl_macros.h>
using namespace pcl;
using namespace on_nurbs;
}
ON_NurbsSurface
-FittingSphere::initNurbsSphere (int order, NurbsDataSurface *data, Eigen::Vector3d pole_axis)
+FittingSphere::initNurbsSphere (int order, NurbsDataSurface *data, Eigen::Vector3d)
{
Eigen::Vector3d mean = NurbsTools::computeMean (data->interior);
i.push_back (it_row->first);
j.push_back (it_col->first);
v.push_back (it_col->second);
- it_col++;
+ ++it_col;
}
- it_row++;
+ ++it_row;
}
}
it_col = it_row->second.find (j);
if (it_col != it_row->second.end ())
it_row->second.erase (it_col);
- it_row++;
+ ++it_row;
}
}
while (it_row != m_mat.end ())
{
it_col = it_row->second.end ();
- it_col--;
+ --it_col;
if (sj < ((*it_col).first + 1))
sj = (*it_col).first + 1;
- it_row++;
+ ++it_row;
}
it_row = m_mat.end ();
- it_row--;
+ --it_row;
si = (*it_row).first + 1;
}
{
s += int (it_row->second.size ());
- it_row++;
+ ++it_row;
}
return s;
while (it_col != it_row->second.end ())
{
printf ("[%d,%d] %f ", it_row->first, it_col->first, it_col->second);
- it_col++;
+ ++it_col;
}
printf ("\n");
- it_row++;
+ ++it_row;
}
}
// Instantiations of specific point types
-PCL_INSTANTIATE(TextureMapping, (pcl::PointXYZ))
+#ifdef PCL_ONLY_CORE_POINT_TYPES
+ PCL_INSTANTIATE(TextureMapping, (pcl::PointXYZ))
+#else
+ PCL_INSTANTIATE(TextureMapping, PCL_XYZ_POINT_TYPES)
+#endif
VTKUtils::convertToVTK (*input_mesh_, vtk_polygons_);
// Apply the VTK algorithm
- vtkSmartPointer<vtkSmoothPolyDataFilter> vtk_smoother = vtkSmoothPolyDataFilter::New ();
+ vtkSmartPointer<vtkSmoothPolyDataFilter> vtk_smoother = vtkSmartPointer<vtkSmoothPolyDataFilter>::New ();
#if VTK_MAJOR_VERSION < 6
vtk_smoother->SetInput (vtk_polygons_);
#else
vtkSmartPointer<vtkPolyData> vtk_polygons;
mesh2vtk (triangles, vtk_polygons);
- vtkSmartPointer<vtkTriangleFilter> vtk_triangles = vtkTriangleFilter::New ();
+ vtkSmartPointer<vtkTriangleFilter> vtk_triangles = vtkSmartPointer<vtkTriangleFilter>::New ();
#if VTK_MAJOR_VERSION < 6
vtk_triangles->SetInput (vtk_polygons);
#else
--- /dev/null
+PCL_ADD_TEST(test_2d test_2d FILES test_2d.cpp
+ LINK_WITH pcl_io pcl_gtest
+ ARGUMENTS "${PCL_SOURCE_DIR}/test/2d/lena.pcd"
+ "${PCL_SOURCE_DIR}/test/2d/gauss_smooth.pcd"
+ "${PCL_SOURCE_DIR}/test/2d/erosion.pcd"
+ "${PCL_SOURCE_DIR}/test/2d/dilation.pcd"
+ "${PCL_SOURCE_DIR}/test/2d/opening.pcd"
+ "${PCL_SOURCE_DIR}/test/2d/closing.pcd"
+ "${PCL_SOURCE_DIR}/test/2d/erosion_binary.pcd"
+ "${PCL_SOURCE_DIR}/test/2d/dilation_binary.pcd"
+ "${PCL_SOURCE_DIR}/test/2d/opening_binary.pcd"
+ "${PCL_SOURCE_DIR}/test/2d/closing_binary.pcd"
+ "${PCL_SOURCE_DIR}/test/2d/canny.pcd")
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: test_convolution.cpp nsomani $
+ *
+ */
+
+#include <pcl/2d/convolution.h>
+#include <pcl/2d/kernel.h>
+#include <pcl/2d/edge.h>
+#include <pcl/2d/morphology.h>
+
+#include <gtest/gtest.h>
+#include <fstream>
+
+#include <pcl/point_types.h>
+#include <pcl/pcl_base.h>
+#include <pcl/io/pcd_io.h>
+
+char *lena;
+char *gaus;
+char *erosion_ref;
+char *dilation_ref;
+char *opening_ref;
+char *closing_ref;
+char *erosion_binary_ref;
+char *dilation_binary_ref;
+char *opening_binary_ref;
+char *closing_binary_ref;
+char *canny_ref;
+
+using namespace pcl;
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+TEST (Convolution, borderOptions)
+{
+ kernel<pcl::PointXYZI> *k = new kernel<pcl::PointXYZI> ();
+ Convolution<pcl::PointXYZI> *conv = new Convolution<pcl::PointXYZI> ();
+
+ /*dummy clouds*/
+ pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr kernel_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+
+ pcl::io::loadPCDFile(lena, *input_cloud);
+
+ int height = input_cloud->height;
+ int width = input_cloud->width;
+
+ k->setKernelType(kernel<pcl::PointXYZI>::DERIVATIVE_CENTRAL_X);
+ k->fetchKernel (*kernel_cloud);
+
+ conv->setKernel(*kernel_cloud);
+ conv->setInputCloud(input_cloud);
+
+ conv->setBoundaryOptions(Convolution<pcl::PointXYZI>::BOUNDARY_OPTION_MIRROR);
+ conv->filter (*output_cloud);
+ for (int i = 1; i < height - 1; i++)
+ for (int j = 1; j < width - 1; j++)
+ EXPECT_NEAR ((*output_cloud)(j,i).intensity, ((*input_cloud)(j+1,i).intensity-(*input_cloud)(j-1,i).intensity), 1);
+
+ for (int i = 1; i < height - 1; i++)
+ {
+ EXPECT_NEAR ((*output_cloud)(0,i).intensity, ((*input_cloud)(1,i).intensity-(*input_cloud)(0,i).intensity), 1);
+ EXPECT_NEAR ((*output_cloud)(width-1,i).intensity, ((*input_cloud)(width-1,i).intensity-(*input_cloud)(width-2,i).intensity), 1);
+ }
+
+ conv->setBoundaryOptions(Convolution<pcl::PointXYZI>::BOUNDARY_OPTION_CLAMP);
+ conv->filter (*output_cloud);
+ for (int i = 1; i < height - 1; i++)
+ for (int j = 1; j < width - 1; j++)
+ EXPECT_NEAR ((*output_cloud)(j,i).intensity, ((*input_cloud)(j+1,i).intensity-(*input_cloud)(j-1,i).intensity), 1);
+
+ for (int i = 1; i < height - 1; i++)
+ {
+ EXPECT_NEAR ((*output_cloud)(0,i).intensity, ((*input_cloud)(1,i).intensity-(*input_cloud)(0,i).intensity), 1);
+ // EXPECT_NEAR ((*output_cloud)(width-1,i).intensity, ((*input_cloud)(width-1,i).intensity-(*input_cloud)(width-2,i).intensity), 1);
+ }
+
+ conv->setBoundaryOptions(Convolution<pcl::PointXYZI>::BOUNDARY_OPTION_ZERO_PADDING);
+ conv->filter (*output_cloud);
+ for (int i = 1; i < height - 1; i++)
+ for (int j = 1; j < width - 1; j++)
+ EXPECT_NEAR ((*output_cloud)(j,i).intensity, ((*input_cloud)(j+1,i).intensity-(*input_cloud)(j-1,i).intensity), 1);
+
+ for (int i = 1; i < height - 1; i++)
+ {
+ EXPECT_NEAR ((*output_cloud)(0,i).intensity, ((*input_cloud)(1,i).intensity), 1);
+ EXPECT_NEAR ((*output_cloud)(width-1,i).intensity, (-(*input_cloud)(width-2,i).intensity), 1);
+ }
+}
+
+TEST (Convolution, gaussianSmooth)
+{
+ kernel<pcl::PointXYZI> *k = new kernel<pcl::PointXYZI> ();
+ Convolution<pcl::PointXYZI> *conv = new Convolution<pcl::PointXYZI> ();
+
+ /*dummy clouds*/
+ pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr gt_output_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr kernel_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+
+ pcl::io::loadPCDFile(lena, *input_cloud);
+ pcl::io::loadPCDFile(gaus, *gt_output_cloud);
+
+ int height = input_cloud->height;
+ int width = input_cloud->width;
+
+ k->setKernelType(kernel<pcl::PointXYZI>::GAUSSIAN);
+ k->setKernelSize(3);
+ k->setKernelSigma(1.0f);
+ k->fetchKernel (*kernel_cloud);
+
+ conv->setKernel(*kernel_cloud);
+ conv->setInputCloud(input_cloud);
+
+ conv->setBoundaryOptions(Convolution<pcl::PointXYZI>::BOUNDARY_OPTION_MIRROR);
+ conv->filter (*output_cloud);
+ for (int i = 1; i < height - 1; i++)
+ for (int j = 1; j < width - 1; j++)
+ EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity, 1);
+
+}
+
+TEST(Edge, sobel)
+{
+ Edge<pcl::PointXYZI, PointXYZIEdge>::Ptr edge_ (new Edge<pcl::PointXYZI, PointXYZIEdge> ());
+
+ pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<PointXYZIEdge>::Ptr output_cloud (new pcl::PointCloud<PointXYZIEdge>);
+
+ pcl::io::loadPCDFile(lena, *input_cloud);
+
+ int height = input_cloud->height;
+ int width = input_cloud->width;
+
+ edge_->setInputCloud(input_cloud);
+ edge_->setOutputType (Edge<pcl::PointXYZI, PointXYZIEdge>::OUTPUT_X_Y);
+ edge_->detectEdgeSobel (*output_cloud);
+
+ float gt_x, gt_y;
+ for (int i = 1; i < height - 1; i++){
+ for (int j = 1; j < width - 1; j++){
+ gt_x = (*input_cloud)(j+1,i-1).intensity - (*input_cloud)(j-1,i-1).intensity +
+ 2*(*input_cloud)(j+1,i).intensity - 2*(*input_cloud)(j-1,i).intensity +
+ (*input_cloud)(j+1,i+1).intensity - (*input_cloud)(j-1,i+1).intensity;
+ EXPECT_NEAR ((*output_cloud)(j,i).magnitude_x, gt_x, 1);
+
+ gt_y = (*input_cloud)(j-1,i+1).intensity - (*input_cloud)(j-1,i-1).intensity +
+ 2*(*input_cloud)(j,i+1).intensity - 2*(*input_cloud)(j,i-1).intensity +
+ (*input_cloud)(j+1,i+1).intensity - (*input_cloud)(j+1,i-1).intensity;
+ EXPECT_NEAR ((*output_cloud)(j,i).magnitude_y, gt_y, 1);
+ }
+ }
+}
+
+TEST(Edge, prewitt)
+{
+ Edge<pcl::PointXYZI, PointXYZIEdge>::Ptr edge_ (new Edge<pcl::PointXYZI, PointXYZIEdge> ());
+
+ pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<PointXYZIEdge>::Ptr output_cloud (new pcl::PointCloud<PointXYZIEdge>);
+
+ pcl::io::loadPCDFile(lena, *input_cloud);
+
+ int height = input_cloud->height;
+ int width = input_cloud->width;
+
+ edge_->setInputCloud(input_cloud);
+ edge_->setOutputType (Edge<pcl::PointXYZI, PointXYZIEdge>::OUTPUT_X_Y);
+ edge_->detectEdgePrewitt (*output_cloud);
+
+ float gt_x, gt_y;
+ for (int i = 1; i < height - 1; i++){
+ for (int j = 1; j < width - 1; j++){
+ gt_x = (*input_cloud)(j+1,i-1).intensity - (*input_cloud)(j-1,i-1).intensity +
+ (*input_cloud)(j+1,i).intensity - (*input_cloud)(j-1,i).intensity +
+ (*input_cloud)(j+1,i+1).intensity - (*input_cloud)(j-1,i+1).intensity;
+ EXPECT_NEAR ((*output_cloud)(j,i).magnitude_x, gt_x, 1);
+
+ gt_y = (*input_cloud)(j-1,i-1).intensity - (*input_cloud)(j-1,i+1).intensity +
+ (*input_cloud)(j,i-1).intensity - (*input_cloud)(j,i+1).intensity +
+ (*input_cloud)(j+1,i-1).intensity - (*input_cloud)(j+1,i+1).intensity;
+ EXPECT_NEAR ((*output_cloud)(j,i).magnitude_y, gt_y, 1);
+ }
+ }
+}
+
+TEST(Edge, canny)
+{
+ Edge<pcl::PointXYZI, PointXYZIEdge>::Ptr edge_ (new Edge<pcl::PointXYZI, PointXYZIEdge> ());
+
+ pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr gt_output_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<PointXYZIEdge>::Ptr output_cloud (new pcl::PointCloud<PointXYZIEdge>);
+
+ pcl::io::loadPCDFile(lena, *input_cloud);
+ //pcl::io::loadPCDFile("canny.pcd", *gt_output_cloud);
+ pcl::io::loadPCDFile(canny_ref, *gt_output_cloud);
+
+ int height = input_cloud->height;
+ int width = input_cloud->width;
+
+ edge_->setInputCloud(input_cloud);
+ edge_->setOutputType (Edge<pcl::PointXYZI, PointXYZIEdge>::OUTPUT_X_Y);
+ edge_->setHysteresisThresholdLow (10);
+ edge_->setHysteresisThresholdHigh (50);
+ edge_->detectEdgeCanny (*output_cloud);
+
+ int count = 0;
+ for (int i = 1; i < height - 1; i++)
+ {
+ for (int j = 1; j < width - 1; j++)
+ {
+ if (fabs ((*output_cloud)(j,i).magnitude - (*gt_output_cloud)(j,i).intensity) > 50)
+ count++;
+ EXPECT_NEAR ((*output_cloud)(j,i).magnitude, (*gt_output_cloud)(j,i).intensity, 255);
+ }
+ }
+ //EXPECT_LE(count, 0.1*height*width);
+}
+
+void threshold(pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud, float thresh){
+ for(size_t i = 0;i < cloud->height;i++){
+ for(size_t j = 0;j < cloud->width;j++){
+ if((*cloud)(j,i).intensity > thresh)
+ (*cloud)(j,i).intensity = 1;
+ else
+ (*cloud)(j,i).intensity = 0;
+ }
+ }
+}
+
+TEST(Morphology, erosion)
+{
+ /*dummy clouds*/
+ pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr gt_output_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr structuring_element_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+
+ pcl::io::loadPCDFile(lena, *input_cloud);
+ pcl::io::loadPCDFile(erosion_ref, *gt_output_cloud);
+
+ Morphology<pcl::PointXYZI> *morph = new Morphology<pcl::PointXYZI>();
+ morph->setInputCloud(input_cloud);
+ morph->structuringElementRectangle(*structuring_element_cloud, 3, 3);
+ morph->setStructuringElement(structuring_element_cloud);
+ morph->erosionGray(*output_cloud);
+
+ int height = input_cloud->height;
+ int width = input_cloud->width;
+
+ for (int i = 1; i < height - 1; i++)
+ for (int j = 1; j < width - 1; j++)
+ EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity, 1);
+
+ pcl::io::loadPCDFile(erosion_binary_ref, *gt_output_cloud);
+ threshold(input_cloud, 100);
+ morph->setInputCloud(input_cloud);
+ morph->erosionBinary(*output_cloud);
+ for (int i = 1; i < height - 1; i++)
+ for (int j = 1; j < width - 1; j++)
+ EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity/255.0, 1);
+
+}
+
+TEST(Morphology, dilation)
+{
+ /*dummy clouds*/
+ pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr gt_output_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr structuring_element_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+
+ pcl::io::loadPCDFile(lena, *input_cloud);
+ pcl::io::loadPCDFile(dilation_ref, *gt_output_cloud);
+
+ Morphology<pcl::PointXYZI> *morph = new Morphology<pcl::PointXYZI>();
+ morph->setInputCloud(input_cloud);
+ morph->structuringElementRectangle(*structuring_element_cloud, 3, 3);
+ morph->setStructuringElement(structuring_element_cloud);
+ morph->dilationGray(*output_cloud);
+
+ int height = input_cloud->height;
+ int width = input_cloud->width;
+
+ for (int i = 1; i < height - 1; i++)
+ for (int j = 1; j < width - 1; j++)
+ EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity, 1);
+
+ pcl::io::loadPCDFile(dilation_binary_ref, *gt_output_cloud);
+ threshold(input_cloud, 100);
+ morph->setInputCloud(input_cloud);
+ morph->dilationBinary(*output_cloud);
+ for (int i = 1; i < height - 1; i++)
+ for (int j = 1; j < width - 1; j++)
+ EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity/255.0, 1);
+}
+
+TEST(Morphology, opening)
+{
+ /*dummy clouds*/
+ pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr gt_output_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr structuring_element_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+
+ pcl::io::loadPCDFile(lena, *input_cloud);
+ pcl::io::loadPCDFile(opening_ref, *gt_output_cloud);
+
+ Morphology<pcl::PointXYZI> *morph = new Morphology<pcl::PointXYZI>();
+ morph->setInputCloud(input_cloud);
+ morph->structuringElementRectangle(*structuring_element_cloud, 3, 3);
+ morph->setStructuringElement(structuring_element_cloud);
+ morph->openingGray(*output_cloud);
+
+ int height = input_cloud->height;
+ int width = input_cloud->width;
+
+ for (int i = 1; i < height - 1; i++)
+ for (int j = 1; j < width - 1; j++)
+ EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity, 1);
+
+ pcl::io::loadPCDFile(opening_binary_ref, *gt_output_cloud);
+ threshold(input_cloud, 100);
+ morph->setInputCloud(input_cloud);
+ morph->openingBinary(*output_cloud);
+ for (int i = 1; i < height - 1; i++)
+ for (int j = 1; j < width - 1; j++)
+ EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity/255.0, 1);
+}
+
+TEST(Morphology, closing)
+{
+ /*dummy clouds*/
+ pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr gt_output_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr structuring_element_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+ pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZI>);
+
+ pcl::io::loadPCDFile(lena, *input_cloud);
+ pcl::io::loadPCDFile(closing_ref, *gt_output_cloud);
+
+ Morphology<pcl::PointXYZI> *morph = new Morphology<pcl::PointXYZI>();
+ morph->setInputCloud(input_cloud);
+ morph->structuringElementRectangle(*structuring_element_cloud, 3, 3);
+ morph->setStructuringElement(structuring_element_cloud);
+ morph->closingGray(*output_cloud);
+
+ int height = input_cloud->height;
+ int width = input_cloud->width;
+
+ for (int i = 1; i < height - 1; i++)
+ for (int j = 1; j < width - 1; j++)
+ EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity, 1);
+
+ pcl::io::loadPCDFile(closing_binary_ref, *gt_output_cloud);
+ threshold(input_cloud, 100);
+ morph->setInputCloud(input_cloud);
+ morph->closingBinary(*output_cloud);
+ for (int i = 1; i < height - 1; i++)
+ for (int j = 1; j < width - 1; j++)
+ EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity/255.0, 1);
+}
+
+/** --[ */
+int
+main (int argc, char** argv)
+{
+ testing::InitGoogleTest (&argc, argv);
+ lena = argv[1]; //lena.pcd
+ gaus = argv[2]; //gauss_smooth.pcd
+ erosion_ref = argv[3];
+ dilation_ref = argv[4];
+ opening_ref = argv[5];
+ closing_ref = argv[6];
+ erosion_binary_ref = argv[7];
+ dilation_binary_ref = argv[8];
+ opening_binary_ref = argv[9];
+ closing_binary_ref = argv[10];
+ canny_ref = argv[11];
+ return (RUN_ALL_TESTS ());
+}
+/* ]-- */
+
if(BUILD_visualization)
include("${VTK_USE_FILE}")
- set(SUBSYS_DEPS common sample_consensus io kdtree features filters geometry keypoints search surface registration segmentation octree recognition people outofcore visualization)
+ set(SUBSYS_DEPS 2d common sample_consensus io kdtree features filters geometry keypoints search surface registration segmentation octree recognition people outofcore visualization)
set(OPT_DEPS vtk)
else()
- set(SUBSYS_DEPS common sample_consensus io kdtree features filters geometry keypoints search surface registration segmentation octree recognition people outofcore)
+ set(SUBSYS_DEPS 2d common sample_consensus io kdtree features filters geometry keypoints search surface registration segmentation octree recognition people outofcore)
endif()
set(DEFAULT OFF)
add_custom_target(tests "${CMAKE_CTEST_COMMAND}" "-V" VERBATIM)
+ add_subdirectory(2d)
add_subdirectory(common)
add_subdirectory(features)
add_subdirectory(filters)
add_subdirectory(search)
add_subdirectory(keypoints)
add_subdirectory(surface)
+ add_subdirectory(segmentation)
add_subdirectory(sample_consensus)
- PCL_ADD_TEST(a_bearing_angle_image_test test_bearing_angle_image
- FILES test_bearing_angle_image.cpp
- LINK_WITH pcl_gtest pcl_common pcl_io)
-
PCL_ADD_TEST(a_recognition_ism_test test_recognition_ism
FILES test_recognition_ism.cpp
LINK_WITH pcl_gtest pcl_io pcl_features
PCL_ADD_TEST(common_test_macros test_macros FILES test_macros.cpp LINK_WITH pcl_gtest pcl_common)
PCL_ADD_TEST(common_vector_average test_vector_average FILES test_vector_average.cpp LINK_WITH pcl_gtest)
PCL_ADD_TEST(common_common test_common FILES test_common.cpp LINK_WITH pcl_gtest pcl_common)
+PCL_ADD_TEST(common_geometry test_geometry FILES test_geometry.cpp LINK_WITH pcl_gtest pcl_common)
PCL_ADD_TEST(common_copy_point test_copy_point FILES test_copy_point.cpp LINK_WITH pcl_gtest pcl_common)
PCL_ADD_TEST(common_centroid test_centroid FILES test_centroid.cpp LINK_WITH pcl_gtest pcl_common)
PCL_ADD_TEST(common_int test_plane_intersection FILES test_plane_intersection.cpp LINK_WITH pcl_gtest pcl_common)
PCL_ADD_TEST(common_generator test_generator FILES test_generator.cpp LINK_WITH pcl_gtest pcl_common)
PCL_ADD_TEST(common_io test_common_io FILES test_io.cpp LINK_WITH pcl_gtest pcl_common)
PCL_ADD_TEST(common_copy_make_borders test_copy_make_borders FILES test_copy_make_borders.cpp LINK_WITH pcl_gtest pcl_common)
+PCL_ADD_TEST(common_bearing_angle_image test_bearing_angle_image FILES test_bearing_angle_image.cpp LINK_WITH pcl_gtest pcl_common)
PCL_ADD_TEST(common_point_type_conversion test_common_point_type_conversion FILES test_point_type_conversion.cpp LINK_WITH pcl_gtest pcl_common)
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2013, Intelligent Robotics Lab, DLUT.
+ * Author: Qinghua Li, Yan Zhuang, Fei Yan
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Intelligent Robotics Lab, DLUT. nor the names
+ * of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ * \file test_bearing_angle_image.cpp
+ * \created on: July 07, 2013
+ * \author: Qinghua Li (qinghua__li@163.com)
+ */
+
+#include <gtest/gtest.h>
+#include <iostream>
+#include <pcl/range_image/bearing_angle_image.h>
+
+pcl::BearingAngleImage bearing_angle_image;
+pcl::PointCloud<pcl::PointXYZ> point_cloud (3, 2);
+
+/////////////////////////////////////////////////////////////////////
+TEST (BearingAngleImageTest, GetAngle)
+{
+ pcl::PointXYZ point1 (1.0, 2.0, 3.0);
+ pcl::PointXYZ point2 (2.0, 1.0, 1.0);
+
+ double angle = bearing_angle_image.getAngle (point1, point2);
+ EXPECT_NEAR (40.203, angle, 1e-3);
+}
+
+/////////////////////////////////////////////////////////////////////
+TEST (BearingAngleImageTest, GenerateBAImage)
+{
+ point_cloud.points[0] = pcl::PointXYZ (3.0, 1.5, -2.0);
+ point_cloud.points[1] = pcl::PointXYZ (1.0, 3.0, 2.0);
+ point_cloud.points[2] = pcl::PointXYZ (2.0, 3.0, 2.0);
+
+ point_cloud.points[3] = pcl::PointXYZ (2.0, 3.0, 1.0);
+ point_cloud.points[4] = pcl::PointXYZ (4.0, 2.0, 2.0);
+ point_cloud.points[5] = pcl::PointXYZ (-1.5, 3.0, 1.0);
+
+ bearing_angle_image.generateBAImage (point_cloud);
+
+ uint8_t grays[6];
+ for (int i = 0; i < 3 * 2; ++i)
+ {
+ grays[i] = (bearing_angle_image.points[i].rgba >> 8) & 0xff;
+ }
+
+ EXPECT_EQ (0, grays[0]);
+ EXPECT_EQ (0, grays[1]);
+ EXPECT_EQ (0, grays[2]);
+ EXPECT_EQ (112, grays[3]);
+ EXPECT_EQ (80, grays[4]);
+ EXPECT_EQ (0, grays[5]);
+}
+
+
+/* ---[ */
+int
+main (int argc, char** argv)
+{
+ testing::InitGoogleTest (&argc, argv);
+ return (RUN_ALL_TESTS ());
+}
EXPECT_FALSE ((pcl::traits::has_label<pcl::Normal>::value));
}
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST (PCL, GetMaxDistance)
+{
+ PointCloud<PointXYZ> cloud;
+ Eigen::Vector4f max_pt, max_exp_pt;
+ const Eigen::Vector4f pivot_pt (Eigen::Vector4f::Zero ());
+
+ // populate cloud
+ cloud.points.resize (3);
+ cloud[0].data[0] = 4.f; cloud[0].data[1] = 3.f;
+ cloud[0].data[2] = 0.f; cloud[0].data[3] = 0.f;
+ cloud[1].data[0] = 0.f; cloud[1].data[1] = 0.f;
+ cloud[1].data[2] = 0.f; cloud[1].data[3] = 1000.f; //This term should not influence max dist
+ cloud[2].data[0] = -1.5f; cloud[2].data[1] = 1.5f;
+ cloud[2].data[2] = -.5f; cloud[2].data[3] = 0.f;
+
+ // No indices specified
+ max_exp_pt = cloud[0].getVector4fMap ();
+ getMaxDistance (cloud, pivot_pt, max_pt);
+ EXPECT_EQ (max_exp_pt, max_pt);
+
+ // Specifying indices
+ std::vector<int> idx (2);
+ idx[0] = 1; idx[1] = 2;
+ max_exp_pt = cloud[2].getVector4fMap ();
+ getMaxDistance (cloud, idx, pivot_pt, max_pt);
+ EXPECT_EQ (max_exp_pt, max_pt);
+}
+
/* ---[ */
int
main (int argc, char** argv)
pcl::copyPointCloud (cloud, dst, top, bottom, left, right, pcl::BORDER_CONSTANT, constant);
for (int j = 0; j < top; ++j)
- for (int i = 0; i < dst.width; ++i)
+ for (uint32_t i = 0; i < dst.width; ++i)
EXPECT_XYZ_EQ (dst (i,j), constant);
- for (int j = top; j < cloud.height+top; ++j)
+ for (unsigned int j = top; j < cloud.height+top; ++j)
{
- for (int i = 0; i < dst.width; ++i)
+ for (uint32_t i = 0; i < dst.width; ++i)
{
- if (i < left)
+ if (static_cast<int> (i) < left)
EXPECT_XYZ_EQ (dst (i,j), constant);
else
{
}
}
- for (int j = cloud.height+top; j < dst.height; ++j)
- for (int i = 0; i < dst.width; ++i)
+ for (uint32_t j = cloud.height+top; j < dst.height; ++j)
+ for (uint32_t i = 0; i < dst.width; ++i)
EXPECT_XYZ_EQ (dst (i,j), constant);
}
{
for (int i = 0; i < left; ++i)
EXPECT_XYZ_EQ (dst (i,j), cloud (0,0));
- for (int i = left; i < cloud.width+left; ++i)
+ for (unsigned int i = left; i < cloud.width+left; ++i)
EXPECT_XYZ_EQ (dst (i,j), cloud (i-left,0));
- for (int i = cloud.width+left; i < dst.width; ++i)
+ for (uint32_t i = cloud.width+left; i < dst.width; ++i)
EXPECT_XYZ_EQ (dst (i,j), cloud (cloud.width-1,0));
}
- for (int j = top; j < cloud.height+top; ++j)
+ for (unsigned int j = top; j < cloud.height+top; ++j)
{
- for (int i = 0; i < dst.width; ++i)
+ for (uint32_t i = 0; i < dst.width; ++i)
{
- if (i < left)
+ if (static_cast<int> (i) < left)
EXPECT_XYZ_EQ (dst (i,j), cloud (0,j-top));
else
{
}
}
- for (int j = cloud.height+top; j < dst.height; ++j)
+ for (uint32_t j = cloud.height+top; j < dst.height; ++j)
{
for (int i = 0; i < left; ++i)
EXPECT_XYZ_EQ (dst (i,j), cloud (0,cloud.height-1));
- for (int i = left; i < cloud.width+left; ++i)
+ for (unsigned int i = left; i < cloud.width+left; ++i)
EXPECT_XYZ_EQ (dst (i,j), cloud (i-left,cloud.height-1));
- for (int i = cloud.width+left; i < dst.width; ++i)
+ for (uint32_t i = cloud.width+left; i < dst.width; ++i)
EXPECT_XYZ_EQ (dst (i,j), cloud (cloud.width-1,cloud.height-1));
}
}
for (int i = 0, l = left-1; i < left; ++i, --l)
EXPECT_XYZ_EQ (dst (i,j), cloud (l, k));
- for (int i = left; i < cloud.width+left; ++i)
+ for (unsigned int i = left; i < cloud.width+left; ++i)
EXPECT_XYZ_EQ (dst (i,j), cloud (i-left,k));
for (int i = cloud.width+left, l = cloud.width-left; i < left; ++i, --l)
EXPECT_XYZ_EQ (dst (i,j), cloud (l, k));
}
- for (int j = top; j < cloud.height+top; ++j)
+ for (unsigned int j = top; j < cloud.height+top; ++j)
{
for (int i = 0, l = left-1; i < left; ++i, --l)
EXPECT_XYZ_EQ (dst (i,j), cloud (l, j-top));
- for (int i = left; i < cloud.width + left; ++i)
+ for (unsigned int i = left; i < cloud.width + left; ++i)
EXPECT_XYZ_EQ (dst (i,j), cloud (i-left,j-top));
for (int i = cloud.width+left, l = cloud.width-left; i < left; ++i, --l)
for (int i = 0, l = left-1; i < left; ++i, --l)
EXPECT_XYZ_EQ (dst (i,j), cloud (l, k));
- for (int i = left; i < cloud.width+left; ++i)
+ for (unsigned int i = left; i < cloud.width+left; ++i)
EXPECT_XYZ_EQ (dst (i,j), cloud (i-left,k));
for (int i = cloud.width+left, l = cloud.width-left; i < left; ++i, --l)
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <gtest/gtest.h>
+#include <pcl/common/geometry.h>
+#include <pcl/point_types.h>
+
+using namespace pcl;
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename T> class XYZPointTypesTest : public ::testing::Test { };
+typedef ::testing::Types<BOOST_PP_SEQ_ENUM(PCL_XYZ_POINT_TYPES)> XYZPointTypes;
+TYPED_TEST_CASE(XYZPointTypesTest, XYZPointTypes);
+
+TYPED_TEST(XYZPointTypesTest, Distance)
+{
+ TypeParam p1, p2;
+ p1.x = 3; p1.y = 4; p1.z = 5;
+ p2.y = 1; p2.x = 1; p2.z = 1.5;
+ double distance = geometry::distance (p1, p2);
+ EXPECT_NEAR (distance, 5.024938, 1e-4);
+}
+
+TYPED_TEST(XYZPointTypesTest, SquaredDistance)
+{
+ TypeParam p1, p2;
+ p1.x = 3; p1.y = 4; p1.z = 5;
+ p2.y = 1; p2.x = 1; p2.z = 1.5;
+ double distance = geometry::squaredDistance (p1, p2);
+ EXPECT_NEAR (distance, 25.25, 1e-4);
+}
+
+/* ---[ */
+int
+main (int argc, char** argv)
+{
+ testing::InitGoogleTest (&argc, argv);
+ return (RUN_ALL_TESTS ());
+}
+/* ]--- */
typedef PointCloud <PointXYZRGBA> CloudXYZRGBA;
typedef PointCloud <PointXYZRGB> CloudXYZRGB;
typedef PointCloud <PointXYZRGBNormal> CloudXYZRGBNormal;
+typedef PointCloud <PointXYZ> CloudXYZ;
PointXYZRGBA pt_xyz_rgba, pt_xyz_rgba2;
PointXYZRGB pt_xyz_rgb;
+PointXYZ pt_xyz;
///////////////////////////////////////////////////////////////////////////////////////////
TEST (PCL, concatenateFields)
}
}
+TEST (PCL, CopyPointCloudWithSameTypes)
+{
+ CloudXYZ cloud_in (5, 1, pt_xyz);
+ CloudXYZ cloud_in_empty;
+ CloudXYZ cloud_out;
+
+ pcl::copyPointCloud (cloud_in, cloud_out);
+
+ ASSERT_EQ (cloud_in.size (), cloud_out.size ());
+ for (size_t i = 0; i < cloud_out.size (); ++i)
+ EXPECT_XYZ_EQ (cloud_in[i], cloud_out[i]);
+
+ pcl::copyPointCloud (cloud_in_empty, cloud_out);
+
+ ASSERT_EQ (0, cloud_out.size ());
+}
+
/* ---[ */
int
main (int argc, char** argv)
pt_xyz_rgb.b = 0;
pt_xyz_rgb.a = 255;
+ pt_xyz.x = 4;
+ pt_xyz.y = 1;
+ pt_xyz.z = 5;
+
testing::InitGoogleTest (&argc, argv);
return (RUN_ALL_TESTS ());
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST (PCL, VectorAverage_mean)
{
- std::vector<Eigen::Vector3f> points;
- std::vector<Eigen::Vector3f::Scalar> weights;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > points;
+ std::vector<Eigen::Vector3f::Scalar, Eigen::aligned_allocator<Eigen::Vector3f::Scalar> > weights;
points.push_back (Eigen::Vector3f (-0.558191f, 0.180822f, -0.809769f));
weights.push_back (0.160842f);
points.push_back (Eigen::Vector3f (-0.510641f, 0.290673f, -0.809169f));
FILES test_spin_estimation.cpp
LINK_WITH pcl_gtest pcl_features pcl_io
ARGUMENTS "${PCL_SOURCE_DIR}/test/bun0.pcd")
+PCL_ADD_TEST(feature_rsd_estimation test_rsd_estimation
+ FILES test_rsd_estimation.cpp
+ LINK_WITH pcl_gtest pcl_features pcl_io pcl_kdtree
+ ARGUMENTS "${PCL_SOURCE_DIR}/test/bun0.pcd")
+PCL_ADD_TEST(feature_grsd_estimation test_grsd_estimation
+ FILES test_grsd_estimation.cpp
+ LINK_WITH pcl_gtest pcl_features pcl_io pcl_kdtree
+ ARGUMENTS "${PCL_SOURCE_DIR}/test/bun0.pcd")
PCL_ADD_TEST(feature_invariants_estimation test_invariants_estimation
FILES test_invariants_estimation.cpp
LINK_WITH pcl_gtest pcl_features pcl_io
FILES test_shot_lrf_estimation.cpp
LINK_WITH pcl_gtest pcl_features pcl_io
ARGUMENTS "${PCL_SOURCE_DIR}/test/bun0.pcd")
+PCL_ADD_TEST(feature_brisk test_brisk
+ FILES test_brisk.cpp
+ LINK_WITH pcl_gtest pcl_io pcl_kdtree pcl_filters pcl_keypoints pcl_common pcl_features pcl_search
+ ARGUMENTS "${PCL_SOURCE_DIR}/test/brisk_image_gt.pcd" "${PCL_SOURCE_DIR}/test/brisk_keypoints_gt.pcd" "${PCL_SOURCE_DIR}/test/brisk_descriptors_gt.pcd")
PCL_ADD_TEST(features_narf test_narf
FILES test_narf.cpp
LINK_WITH pcl_gtest pcl_features ${FLANN_LIBRARIES})
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+
+#include <gtest/gtest.h>
+#include <pcl/point_types.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/keypoints/brisk_2d.h>
+#include <pcl/features/brisk_2d.h>
+#include <set>
+
+using namespace pcl;
+using namespace pcl::io;
+using namespace std;
+
+typedef PointXYZRGBA PointT;
+typedef PointWithScale KeyPointT;
+
+
+PointCloud<PointT>::Ptr cloud_image (new PointCloud<PointT>);
+PointCloud<PointWithScale>::Ptr cloud_keypoints (new PointCloud<PointWithScale>);
+PointCloud<BRISKSignature512>::Ptr cloud_descriptors (new PointCloud<BRISKSignature512>);
+PointCloud<PointWithScale>::Ptr cloud_keypoints_gt (new PointCloud<PointWithScale>);
+PointCloud<BRISKSignature512>::Ptr cloud_descriptors_gt (new PointCloud<BRISKSignature512>);
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+TEST (PCL, BRISK_2D)
+{
+#if defined(__SSSE3__) && !defined(__i386__)
+ // Compute BRISK keypoints
+ BriskKeypoint2D<PointT> brisk_keypoint_estimation;
+ brisk_keypoint_estimation.setThreshold (60);
+ brisk_keypoint_estimation.setOctaves (4);
+ brisk_keypoint_estimation.setInputCloud (cloud_image);
+
+ cloud_keypoints.reset (new PointCloud<KeyPointT>);
+ brisk_keypoint_estimation.compute (*cloud_keypoints);
+
+ //io::savePCDFileBinary ("brisk_keypoints.pcd", *cloud_keypoints);
+
+ const int num_of_keypoints = int (cloud_keypoints->size ());
+ const int num_of_keypoints_gt = int (cloud_keypoints_gt->size ());
+ EXPECT_EQ (num_of_keypoints_gt, num_of_keypoints);
+
+
+ for (size_t point_index = 0; point_index < cloud_keypoints->size (); ++point_index)
+ {
+ PointWithScale & point = (*cloud_keypoints) [point_index];
+
+ const float dx = point.x - point.x;
+ const float dy = point.y - point.y;
+ const float dz = point.z - point.z;
+
+ const float sqr_distance = (dx*dx + dy*dy + dz*dz);
+
+ EXPECT_NEAR (0.0f, sqr_distance, 1e-4);
+ }
+
+ BRISK2DEstimation<PointT> brisk_descriptor_estimation;
+ brisk_descriptor_estimation.setInputCloud (cloud_image);
+ brisk_descriptor_estimation.setKeypoints (cloud_keypoints);
+
+
+ cloud_descriptors.reset (new PointCloud<BRISKSignature512>);
+ brisk_descriptor_estimation.compute (*cloud_descriptors);
+
+ const int num_of_descriptors = int (cloud_descriptors->size ());
+ const int num_of_descriptors_gt = int (cloud_descriptors_gt->size ());
+ EXPECT_EQ (num_of_descriptors_gt, num_of_descriptors);
+
+
+ //io::savePCDFileBinary ("brisk_descriptors.pcd", *cloud_descriptors);
+ //for (size_t point_index = 0; point_index < cloud_keypoints->size (); ++point_index)
+ for (size_t point_index = 0; point_index < cloud_descriptors->size (); ++point_index)
+ {
+ BRISKSignature512 & descriptor = (*cloud_descriptors) [point_index];
+ BRISKSignature512 & descriptor_gt = (*cloud_descriptors_gt) [point_index];
+
+ float sqr_dist = 0.0f;
+ for (size_t index = 0; index < 33; ++index)
+ {
+ const float dist = float (descriptor.descriptor[index] - descriptor_gt.descriptor[index]);
+ sqr_dist += dist * dist;
+ }
+
+ EXPECT_NEAR (0.0f, sqr_dist, 1e-4);
+ }
+#else
+ PCL_WARN ("Not compiled with SSSE3, skipping test of Brisk.\n");
+#endif
+}
+
+
+
+/* ---[ */
+int
+ main (int argc, char** argv)
+{
+ if (argc < 2)
+ {
+ std::cerr << "No test file given. Please download `image_gt.pcd`, `keypoints_gt.pcd`, as well as `descriptors_gt.pcd`, and pass its path to the test." << std::endl;
+ return (-1);
+ }
+
+ // Load a sample point cloud
+ if (io::loadPCDFile (argv[1], *cloud_image) < 0)
+ {
+ std::cerr << "Failed to read test file. Please download `image_gt.pcd`, `keypoints_gt.pcd`, as well as `descriptors_gt.pcd`, and pass its path to the test." << std::endl;
+ return (-1);
+ }
+
+ if (io::loadPCDFile (argv[2], *cloud_keypoints_gt) < 0)
+ {
+ std::cerr << "Failed to read test file. Please download `image_gt.pcd`, `keypoints_gt.pcd`, as well as `descriptors_gt.pcd`, and pass its path to the test." << std::endl;
+ return (-1);
+ }
+
+ if (io::loadPCDFile (argv[3], *cloud_descriptors_gt) < 0)
+ {
+ std::cerr << "Failed to read test file. Please download `image_gt.pcd`, `keypoints_gt.pcd`, as well as `descriptors_gt.pcd`, and pass its path to the test." << std::endl;
+ return (-1);
+ }
+
+ testing::InitGoogleTest (&argc, argv);
+ return (RUN_ALL_TESTS ());
+}
+/* ]--- */
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2009-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <gtest/gtest.h>
+#include <pcl/point_cloud.h>
+#include <pcl/features/grsd.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/io/pcd_io.h>
+
+
+using namespace pcl;
+using namespace pcl::io;
+using namespace std;
+
+search::KdTree<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ> ());
+PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ> ());
+vector<int> indices;
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST (PCL, GRSDEstimation)
+{
+ // Estimate normals first
+ double rad = 0.02;
+ NormalEstimation<PointXYZ, Normal> n;
+ PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
+ // set parameters
+ n.setInputCloud (cloud);
+ n.setSearchMethod (tree);
+ n.setRadiusSearch (rad);
+ n.compute (*normals);
+
+ EXPECT_NEAR (normals->points[103].normal_x, 0.694, 0.1);
+ EXPECT_NEAR (normals->points[103].normal_y, -0.562, 0.1);
+ EXPECT_NEAR (normals->points[103].normal_z, -0.448, 0.1);
+
+ // GRSDEstimation
+ double rsd_radius = 0.03;
+ GRSDEstimation<PointXYZ, Normal, GRSDSignature21> grsd;
+ grsd.setInputNormals (normals);
+ PointCloud<GRSDSignature21>::Ptr grsd_desc (new PointCloud<GRSDSignature21> ());
+ grsd.setInputCloud (cloud);
+ grsd.setSearchMethod (tree);
+ grsd.setRadiusSearch (rsd_radius);
+ grsd.compute (*grsd_desc);
+
+ EXPECT_EQ (12, grsd_desc->points[0].histogram[2]);
+ EXPECT_EQ (104, grsd_desc->points[0].histogram[4]);
+ EXPECT_EQ (0, grsd_desc->points[0].histogram[6]);
+ EXPECT_EQ (0, grsd_desc->points[0].histogram[8]);
+ EXPECT_EQ (0, grsd_desc->points[0].histogram[10]);
+ EXPECT_EQ (34, grsd_desc->points[0].histogram[12]);
+ EXPECT_EQ (167, grsd_desc->points[0].histogram[14]);
+ EXPECT_EQ (68, grsd_desc->points[0].histogram[16]);
+ EXPECT_EQ (204, grsd_desc->points[0].histogram[18]);
+ EXPECT_EQ (0, grsd_desc->points[0].histogram[20]);
+
+}
+
+/* ---[ */
+int
+main (int argc, char** argv)
+{
+ if (argc < 2)
+ {
+ std::cerr << "No test file given. Please download `bun0.pcd` and pass its path to the test." << std::endl;
+ return (-1);
+ }
+
+ if (loadPCDFile<PointXYZ> (argv[1], *cloud) < 0)
+ {
+ std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl;
+ return (-1);
+ }
+
+ testing::InitGoogleTest (&argc, argv);
+ return (RUN_ALL_TESTS ());
+}
+/* ]--- */
VFHEstimation<PointXYZ, PointNormal, VFHSignature308>::Ptr vfh (new VFHEstimation<PointXYZ, PointNormal, VFHSignature308> ());
vfh->setViewPoint (1.0f, 1.0f, 1.0f);
- UniqueShapeContext<PointXYZ, ShapeContext1980, ReferenceFrame>::Ptr usc (new UniqueShapeContext<PointXYZ, ShapeContext1980, ReferenceFrame> ());
+ UniqueShapeContext<PointXYZ, UniqueShapeContext1960, ReferenceFrame>::Ptr usc (new UniqueShapeContext<PointXYZ, UniqueShapeContext1960, ReferenceFrame> ());
usc->setMinimalRadius (5);
StatisticalMultiscaleInterestRegionExtraction<PointXYZ>::Ptr smire (new StatisticalMultiscaleInterestRegionExtraction<PointXYZ> ());
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2009-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <gtest/gtest.h>
+#include <pcl/point_cloud.h>
+#include <pcl/features/rsd.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/io/pcd_io.h>
+
+
+using namespace pcl;
+using namespace pcl::io;
+using namespace std;
+
+search::KdTree<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ> ());
+PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ> ());
+vector<int> indices;
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST (PCL, RSDEstimation)
+{
+ // Estimate normals first
+ double rad = 0.02;
+ NormalEstimation<PointXYZ, Normal> n;
+ PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
+ // set parameters
+ n.setInputCloud (cloud);
+ n.setSearchMethod (tree);
+ n.setRadiusSearch (rad);
+ n.compute (*normals);
+
+ EXPECT_NEAR (normals->points[103].normal_x, 0.694, 0.1);
+ EXPECT_NEAR (normals->points[103].normal_y, -0.562, 0.1);
+ EXPECT_NEAR (normals->points[103].normal_z, -0.448, 0.1);
+
+ // RSDEstimation
+ double max_plane_radius = 0.1;
+ double rsd_radius = 0.03;
+ RSDEstimation<PointXYZ, Normal, PrincipalRadiiRSD> rsd;
+ rsd.setInputNormals (normals);
+ PointCloud<PrincipalRadiiRSD>::Ptr rsds (new PointCloud<PrincipalRadiiRSD> ());
+ rsd.setInputCloud (cloud);
+ rsd.setPlaneRadius (max_plane_radius);
+ rsd.setSearchMethod (tree);
+ rsd.setRadiusSearch (rsd_radius);
+ rsd.setSaveHistograms (true);
+ rsd.compute (*rsds);
+
+ typedef std::vector<Eigen::MatrixXf, Eigen::aligned_allocator<Eigen::MatrixXf> > vec_matrixXf;
+ boost::shared_ptr<vec_matrixXf> mat (new vec_matrixXf);
+
+ mat = rsd.getHistograms();
+
+ EXPECT_EQ (1, (*mat)[140](0, 0));
+ EXPECT_EQ (3, (*mat)[140](0, 1));
+ EXPECT_EQ (7, (*mat)[140](0, 2));
+ EXPECT_EQ (2, (*mat)[140](0, 3));
+ EXPECT_EQ (0, (*mat)[140](0, 4));
+ EXPECT_EQ (0, (*mat)[140](1, 0));
+ EXPECT_EQ (0, (*mat)[140](1, 1));
+ EXPECT_EQ (3, (*mat)[140](1, 2));
+ EXPECT_EQ (10, (*mat)[140](1, 3));
+ EXPECT_EQ (12, (*mat)[140](1, 4));
+ EXPECT_EQ (0, (*mat)[140](2, 0));
+ EXPECT_EQ (0, (*mat)[140](2, 1));
+ EXPECT_EQ (0, (*mat)[140](2, 2));
+ EXPECT_EQ (0, (*mat)[140](2, 3));
+ EXPECT_EQ (1, (*mat)[140](2, 4));
+
+ EXPECT_EQ (0, (*mat)[103](0, 0));
+ EXPECT_EQ (4, (*mat)[103](0, 1));
+ EXPECT_EQ (3, (*mat)[103](0, 2));
+ EXPECT_EQ (0, (*mat)[103](0, 3));
+ EXPECT_EQ (0, (*mat)[103](0, 4));
+ EXPECT_EQ (0, (*mat)[103](1, 0));
+ EXPECT_EQ (1, (*mat)[103](1, 1));
+ EXPECT_EQ (7, (*mat)[103](1, 2));
+ EXPECT_EQ (1, (*mat)[103](1, 3));
+ EXPECT_EQ (0, (*mat)[103](1, 4));
+ EXPECT_EQ (0, (*mat)[103](2, 0));
+ EXPECT_EQ (0, (*mat)[103](2, 1));
+ EXPECT_EQ (1, (*mat)[103](2, 2));
+ EXPECT_EQ (5, (*mat)[103](2, 3));
+ EXPECT_EQ (3, (*mat)[103](2, 4));
+
+}
+
+/* ---[ */
+int
+main (int argc, char** argv)
+{
+ if (argc < 2)
+ {
+ std::cerr << "No test file given. Please download `bun0.pcd` and pass its path to the test." << std::endl;
+ return (-1);
+ }
+
+ if (loadPCDFile<PointXYZ> (argv[1], *cloud) < 0)
+ {
+ std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl;
+ return (-1);
+ }
+
+ testing::InitGoogleTest (&argc, argv);
+ return (RUN_ALL_TESTS ());
+}
+/* ]--- */
ASSERT_EQ (d0.points[i].descriptor[j], d1.points[i].descriptor[j]);
}
+///////////////////////////////////////////////////////////////////////////////////
+template <> void
+checkDesc<UniqueShapeContext1960>(const pcl::PointCloud<UniqueShapeContext1960>& d0, const pcl::PointCloud<UniqueShapeContext1960>& d1)
+{
+ ASSERT_EQ (d0.size (), d1.size ());
+ for (size_t i = 0; i < d1.size (); ++i)
+ for (size_t j = 0; j < 1960; ++j)
+ ASSERT_EQ (d0.points[i].descriptor[j], d1.points[i].descriptor[j]);
+}
+
///////////////////////////////////////////////////////////////////////////////////
template <typename FeatureEstimation, typename PointT, typename NormalT, typename OutputT>
struct createSHOTDesc
const bool) const
{
UniqueShapeContext<PointT, OutputT> usc;
- //usc.setAzimuthBins (4);
- //usc.setElevationBins (4);
- //usc.setRadiusBins (4);
usc.setMinimalRadius (0.004);
usc.setPointDensityRadius (0.008);
usc.setLocalRadius (0.04);
TEST (PCL, USCEstimation)
{
float meshRes = 0.002f;
- //size_t nBinsL = 4;
- //size_t nBinsK = 4;
- //size_t nBinsJ = 4;
float radius = 20.0f * meshRes;
float rmin = radius / 10.0f;
float ptDensityRad = radius / 5.0f;
// estimate
- UniqueShapeContext<PointXYZ, ShapeContext1980> uscd;
+ UniqueShapeContext<PointXYZ, UniqueShapeContext1960> uscd;
uscd.setInputCloud (cloud.makeShared ());
uscd.setSearchMethod (tree);
uscd.setRadiusSearch (radius);
- //uscd.setAzimuthBins (nBinsL);
- //uscd.setElevationBins (nBinsK);
- //uscd.setRadiusBins (nBinsJ);
uscd.setMinimalRadius (rmin);
uscd.setPointDensityRadius (ptDensityRad);
uscd.setLocalRadius (radius);
// Compute the features
- PointCloud<ShapeContext1980>::Ptr uscds (new PointCloud<ShapeContext1980>);
+ PointCloud<UniqueShapeContext1960>::Ptr uscds (new PointCloud<UniqueShapeContext1960>);
uscd.compute (*uscds);
EXPECT_EQ (uscds->size (), cloud.size ());
//EXPECT_EQ ((*uscds)[0].descriptor.size (), 64);
- EXPECT_NEAR ((*uscds)[160].descriptor[56], 53.0597f, 1e-4f);
- EXPECT_NEAR ((*uscds)[160].descriptor[734], 80.1063f, 1e-4f);
- EXPECT_NEAR ((*uscds)[160].descriptor[1222], 93.8412f, 1e-4f);
- EXPECT_NEAR ((*uscds)[160].descriptor[1605], 0.f, 1e-4f);
- EXPECT_NEAR ((*uscds)[160].descriptor[1887], 32.6679f, 1e-4f);
+ EXPECT_NEAR ((*uscds)[160].descriptor[355], 123.0733f, 1e-4f);
+ EXPECT_NEAR ((*uscds)[160].descriptor[494], 154.9401f, 1e-4f);
+ EXPECT_NEAR ((*uscds)[160].descriptor[897], 0.f, 1e-4f);
+ EXPECT_NEAR ((*uscds)[160].descriptor[1178], 62.7496f, 1e-4f);
+ EXPECT_NEAR ((*uscds)[160].descriptor[1878], 31.3748f, 1e-4f);
- EXPECT_NEAR ((*uscds)[168].descriptor[72], 65.3358f, 1e-4f);
- EXPECT_NEAR ((*uscds)[168].descriptor[430], 88.8147f, 1e-4f);
- EXPECT_NEAR ((*uscds)[168].descriptor[987], 0.f, 1e-4f);
- EXPECT_NEAR ((*uscds)[168].descriptor[1563], 128.273f, 1e-4f);
- EXPECT_NEAR ((*uscds)[168].descriptor[1915], 59.2098f, 1e-4f);
+ EXPECT_NEAR ((*uscds)[168].descriptor[57], 39.4986f, 1e-4f);
+ EXPECT_NEAR ((*uscds)[168].descriptor[704], 0.f, 1e-4f);
+ EXPECT_NEAR ((*uscds)[168].descriptor[906], 48.8803f, 1e-4f);
+ EXPECT_NEAR ((*uscds)[168].descriptor[1175], 83.4680f, 1e-4f);
+ EXPECT_NEAR ((*uscds)[168].descriptor[1756], 65.1737f, 1e-4f);
// Test results when setIndices and/or setSearchSurface are used
boost::shared_ptr<vector<int> > test_indices (new vector<int> (0));
test_indices->push_back (static_cast<int> (i));
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
- testSHOTIndicesAndSearchSurface<UniqueShapeContext<PointXYZ, ShapeContext1980>, PointXYZ, Normal, ShapeContext1980> (cloud.makeShared (), normals, test_indices);
- testSHOTLocalReferenceFrame<UniqueShapeContext<PointXYZ, ShapeContext1980>, PointXYZ, Normal, ShapeContext1980> (cloud.makeShared (), normals, test_indices);
+ testSHOTIndicesAndSearchSurface<UniqueShapeContext<PointXYZ, UniqueShapeContext1960>, PointXYZ, Normal, UniqueShapeContext1960> (cloud.makeShared (), normals, test_indices);
+ testSHOTLocalReferenceFrame<UniqueShapeContext<PointXYZ, UniqueShapeContext1960>, PointXYZ, Normal, UniqueShapeContext1960> (cloud.makeShared (), normals, test_indices);
}
/* ---[ */
EXPECT_EQ (cloud->points[cloud->points.size () - 2].y, output.points[output.points.size () - 1].y);
EXPECT_EQ (cloud->points[cloud->points.size () - 2].z, output.points[output.points.size () - 1].z);
+ ei2.setNegative (false);
+ ei2.setKeepOrganized (true);
+ ei2.filter (output_blob);
+
+ fromPCLPointCloud2(output_blob, output);
+
+ EXPECT_EQ (output.points.size (), cloud->points.size ());
+ EXPECT_EQ (output.width, cloud->width);
+ EXPECT_EQ (output.height, cloud->height);
+
+ EXPECT_EQ (output.points[0].x, cloud->points[0].x);
+ EXPECT_EQ (output.points[0].y, cloud->points[0].y);
+ EXPECT_EQ (output.points[0].z, cloud->points[0].z);
+ EXPECT_TRUE (pcl_isnan(output.points[1].x));
+ EXPECT_TRUE (pcl_isnan(output.points[1].y));
+ EXPECT_TRUE (pcl_isnan(output.points[1].z));
+
+ ei2.setNegative (true);
+ ei2.setKeepOrganized (true);
+ ei2.filter (output_blob);
+
+ fromPCLPointCloud2(output_blob, output);
+
+ EXPECT_EQ (output.points.size (), cloud->points.size ());
+ EXPECT_EQ (output.width, cloud->width);
+ EXPECT_EQ (output.height, cloud->height);
+
+ EXPECT_TRUE (pcl_isnan(output.points[0].x));
+ EXPECT_TRUE (pcl_isnan(output.points[0].y));
+ EXPECT_TRUE (pcl_isnan(output.points[0].z));
+ EXPECT_EQ (output.points[1].x, cloud->points[1].x);
+ EXPECT_EQ (output.points[1].y, cloud->points[1].y);
+ EXPECT_EQ (output.points[1].z, cloud->points[1].z);
+
// Test setNegative on empty datasets
PointCloud<PointXYZ> empty, result;
ExtractIndices<PointXYZ> eie;
EXPECT_EQ (output.height, cloud->height);
EXPECT_EQ (bool (output.is_dense), false); // NaN was set as a user filter value
- if (!pcl_isnan (output.points[0].x)) EXPECT_EQ (1, 0);
- if (!pcl_isnan (output.points[0].y)) EXPECT_EQ (1, 0);
- if (!pcl_isnan (output.points[0].z)) EXPECT_EQ (1, 0);
-
- if (!pcl_isnan (output.points[41].x)) EXPECT_EQ (1, 0);
- if (!pcl_isnan (output.points[41].y)) EXPECT_EQ (1, 0);
- if (!pcl_isnan (output.points[41].z)) EXPECT_EQ (1, 0);
+ EXPECT_TRUE (pcl_isnan (output.points[0].x));
+ EXPECT_TRUE (pcl_isnan (output.points[0].y));
+ EXPECT_TRUE (pcl_isnan (output.points[0].z));
+ EXPECT_TRUE (pcl_isnan (output.points[41].x));
+ EXPECT_TRUE (pcl_isnan (output.points[41].y));
+ EXPECT_TRUE (pcl_isnan (output.points[41].z));
pt.setFilterLimitsNegative (true);
pt.filter (output);
EXPECT_EQ (output.height, cloud->height);
EXPECT_EQ (bool (output.is_dense), false); // NaN was set as a user filter value
- if (!pcl_isnan (output.points[0].x)) EXPECT_EQ (1, 0);
- if (!pcl_isnan (output.points[0].y)) EXPECT_EQ (1, 0);
- if (!pcl_isnan (output.points[0].z)) EXPECT_EQ (1, 0);
+ EXPECT_TRUE (pcl_isnan (output.points[0].x));
+ EXPECT_TRUE (pcl_isnan (output.points[0].y));
+ EXPECT_TRUE (pcl_isnan (output.points[0].z));
- if (!pcl_isnan (output.points[41].x)) EXPECT_EQ (1, 0);
- if (!pcl_isnan (output.points[41].y)) EXPECT_EQ (1, 0);
- if (!pcl_isnan (output.points[41].z)) EXPECT_EQ (1, 0);
+ EXPECT_TRUE (pcl_isnan (output.points[41].x));
+ EXPECT_TRUE (pcl_isnan (output.points[41].y));
+ EXPECT_TRUE (pcl_isnan (output.points[41].z));
pt2.setFilterLimitsNegative (true);
pt2.filter (output_blob);
EXPECT_LE (output.points[neighbors2.at (0)].z - output.points[centroidIdx2].z, 0.02 * 2);
}
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST (VoxelGrid_No_DownsampleAllData, Filters)
+{
+ // Test the PointCloud<PointT> method
+ PointCloud<PointXYZ> output;
+ VoxelGrid<PointXYZ> grid;
+
+ grid.setLeafSize (0.02f, 0.02f, 0.02f);
+ grid.setDownsampleAllData(false);
+ grid.setInputCloud (cloud);
+ grid.filter (output);
+
+ EXPECT_EQ (int (output.points.size ()), 103);
+ EXPECT_EQ (int (output.width), 103);
+ EXPECT_EQ (int (output.height), 1);
+ EXPECT_EQ (bool (output.is_dense), true);
+
+ grid.setFilterFieldName ("z");
+ grid.setFilterLimits (0.05, 0.1);
+ grid.filter (output);
+
+ EXPECT_EQ (int (output.points.size ()), 14);
+ EXPECT_EQ (int (output.width), 14);
+ EXPECT_EQ (int (output.height), 1);
+ EXPECT_EQ (bool (output.is_dense), true);
+
+ EXPECT_NEAR (output.points[0].x, -0.026125, 1e-4);
+ EXPECT_NEAR (output.points[0].y, 0.039788, 1e-4);
+ EXPECT_NEAR (output.points[0].z, 0.052827, 1e-4);
+
+ EXPECT_NEAR (output.points[13].x, -0.073202, 1e-4);
+ EXPECT_NEAR (output.points[13].y, 0.1296, 1e-4);
+ EXPECT_NEAR (output.points[13].z, 0.051333, 1e-4);
+
+ grid.setFilterLimitsNegative (true);
+ grid.setSaveLeafLayout(true);
+ grid.filter (output);
+
+ EXPECT_EQ (int (output.points.size ()), 100);
+ EXPECT_EQ (int (output.width), 100);
+ EXPECT_EQ (int (output.height), 1);
+ EXPECT_EQ (bool (output.is_dense), true);
+
+ // centroids should be identified correctly
+ EXPECT_EQ (grid.getCentroidIndex (output.points[0]), 0);
+ EXPECT_EQ (grid.getCentroidIndex (output.points[99]), 99);
+ EXPECT_EQ (grid.getCentroidIndexAt (grid.getGridCoordinates (-1,-1,-1)), -1);
+ //PCL_ERROR ("IGNORE PREVIOUS ERROR: testing it's functionality!\n");
+
+ // input point 195 [0.04872199893, 0.07376000285, 0.01743399911]
+ int centroidIdx = grid.getCentroidIndex (cloud->points[195]);
+
+ // for arbitrary points, the centroid should be close
+ EXPECT_LE (fabs (output.points[centroidIdx].x - cloud->points[195].x), 0.02);
+ EXPECT_LE (fabs (output.points[centroidIdx].y - cloud->points[195].y), 0.02);
+ EXPECT_LE (fabs (output.points[centroidIdx].z - cloud->points[195].z), 0.02);
+
+ // if getNeighborCentroidIndices works then the other helper functions work as well
+ EXPECT_EQ (grid.getNeighborCentroidIndices (output.points[0], Eigen::MatrixXi::Zero(3,1))[0], 0);
+ EXPECT_EQ (grid.getNeighborCentroidIndices (output.points[99], Eigen::MatrixXi::Zero(3,1))[0], 99);
+
+ // neighboring centroid should be in the right position
+ Eigen::MatrixXi directions = Eigen::Vector3i (0, 0, 1);
+ vector<int> neighbors = grid.getNeighborCentroidIndices (cloud->points[195], directions);
+ EXPECT_EQ (neighbors.size (), size_t (directions.cols ()));
+ EXPECT_NE (neighbors.at (0), -1);
+ EXPECT_LE (fabs (output.points[neighbors.at (0)].x - output.points[centroidIdx].x), 0.02);
+ EXPECT_LE (fabs (output.points[neighbors.at (0)].y - output.points[centroidIdx].y), 0.02);
+ EXPECT_LE ( output.points[neighbors.at (0)].z - output.points[centroidIdx].z, 0.02 * 2);
+
+ // Test the pcl::PCLPointCloud2 method
+ VoxelGrid<PCLPointCloud2> grid2;
+
+ PCLPointCloud2 output_blob;
+
+ grid2.setLeafSize (0.02f, 0.02f, 0.02f);
+ grid2.setDownsampleAllData(false);
+ grid2.setInputCloud (cloud_blob);
+ grid2.filter (output_blob);
+
+ fromPCLPointCloud2 (output_blob, output);
+
+ EXPECT_EQ (int (output.points.size ()), 103);
+ EXPECT_EQ (int (output.width), 103);
+ EXPECT_EQ (int (output.height), 1);
+ EXPECT_EQ (bool (output.is_dense), true);
+
+ grid2.setFilterFieldName ("z");
+ grid2.setFilterLimits (0.05, 0.1);
+ grid2.filter (output_blob);
+
+ fromPCLPointCloud2 (output_blob, output);
+
+ EXPECT_EQ (int (output.points.size ()), 14);
+ EXPECT_EQ (int (output.width), 14);
+ EXPECT_EQ (int (output.height), 1);
+ EXPECT_EQ (bool (output.is_dense), true);
+
+ EXPECT_NEAR (output.points[0].x, -0.026125, 1e-4);
+ EXPECT_NEAR (output.points[0].y, 0.039788, 1e-4);
+ EXPECT_NEAR (output.points[0].z, 0.052827, 1e-4);
+
+ EXPECT_NEAR (output.points[13].x, -0.073202, 1e-4);
+ EXPECT_NEAR (output.points[13].y, 0.1296, 1e-4);
+ EXPECT_NEAR (output.points[13].z, 0.051333, 1e-4);
+
+ grid2.setFilterLimitsNegative (true);
+ grid2.setSaveLeafLayout(true);
+ grid2.filter (output_blob);
+
+ fromPCLPointCloud2 (output_blob, output);
+
+ EXPECT_EQ (int (output.points.size ()), 100);
+ EXPECT_EQ (int (output.width), 100);
+ EXPECT_EQ (int (output.height), 1);
+ EXPECT_EQ (bool (output.is_dense), true);
+
+ // centroids should be identified correctly
+ EXPECT_EQ (grid2.getCentroidIndex (output.points[0].x, output.points[0].y, output.points[0].z), 0);
+ EXPECT_EQ (grid2.getCentroidIndex (output.points[99].x, output.points[99].y, output.points[99].z), 99);
+ EXPECT_EQ (grid2.getCentroidIndexAt (grid2.getGridCoordinates (-1,-1,-1)), -1);
+ //PCL_ERROR ("IGNORE PREVIOUS ERROR: testing it's functionality!\n");
+
+ // input point 195 [0.04872199893, 0.07376000285, 0.01743399911]
+ int centroidIdx2 = grid2.getCentroidIndex (0.048722f, 0.073760f, 0.017434f);
+ EXPECT_NE (centroidIdx2, -1);
+
+ // for arbitrary points, the centroid should be close
+ EXPECT_LE (fabs (output.points[centroidIdx2].x - 0.048722), 0.02);
+ EXPECT_LE (fabs (output.points[centroidIdx2].y - 0.073760), 0.02);
+ EXPECT_LE (fabs (output.points[centroidIdx2].z - 0.017434), 0.02);
+
+ // if getNeighborCentroidIndices works then the other helper functions work as well
+ EXPECT_EQ (grid2.getNeighborCentroidIndices (output.points[0].x, output.points[0].y, output.points[0].z, Eigen::MatrixXi::Zero(3,1))[0], 0);
+ EXPECT_EQ (grid2.getNeighborCentroidIndices (output.points[99].x, output.points[99].y, output.points[99].z, Eigen::MatrixXi::Zero(3,1))[0], 99);
+
+ // neighboring centroid should be in the right position
+ Eigen::MatrixXi directions2 = Eigen::Vector3i (0, 0, 1);
+ vector<int> neighbors2 = grid2.getNeighborCentroidIndices (0.048722f, 0.073760f, 0.017434f, directions2);
+ EXPECT_EQ (neighbors2.size (), size_t (directions2.cols ()));
+ EXPECT_NE (neighbors2.at (0), -1);
+ EXPECT_LE (fabs (output.points[neighbors2.at (0)].x - output.points[centroidIdx2].x), 0.02);
+ EXPECT_LE (fabs (output.points[neighbors2.at (0)].y - output.points[centroidIdx2].y), 0.02);
+ EXPECT_LE (output.points[neighbors2.at (0)].z - output.points[centroidIdx2].z, 0.02 * 2);
+}
+
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST (VoxelGrid_RGB, Filters)
{
}
}
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST (VoxelGrid_RGBA, Filters)
+{
+ PCLPointCloud2 cloud_rgba_blob_;
+ PCLPointCloud2::Ptr cloud_rgba_blob_ptr_;
+ PointCloud<PointXYZRGBA> cloud_rgba_;
+ PointCloud<PointXYZRGBA>::Ptr cloud_rgba_ptr_;
+
+ int col_r[] = {214, 193, 180, 164, 133, 119, 158, 179, 178, 212};
+ int col_g[] = {10, 39, 219, 231, 142, 169, 84, 158, 139, 214};
+ int col_b[] = {101, 26, 46, 189, 211, 154, 246, 16, 139, 153};
+ int col_a[] = {232, 161, 24, 71, 139, 244, 246, 40, 247, 244};
+ float ave_r = 0.0f;
+ float ave_b = 0.0f;
+ float ave_g = 0.0f;
+ float ave_a = 0.0f;
+ for (int i = 0; i < 10; ++i)
+ {
+ ave_r += static_cast<float> (col_r[i]);
+ ave_g += static_cast<float> (col_g[i]);
+ ave_b += static_cast<float> (col_b[i]);
+ ave_a += static_cast<float> (col_a[i]);
+ }
+ ave_r /= 10.0f;
+ ave_g /= 10.0f;
+ ave_b /= 10.0f;
+ ave_a /= 10.0f;
+
+ for (int i = 0; i < 10; ++i)
+ {
+ PointXYZRGBA pt;
+ int rgba = (col_a[i] << 24) | (col_r[i] << 16) | (col_g[i] << 8) | col_b[i];
+ pt.x = 0.0f;
+ pt.y = 0.0f;
+ pt.z = 0.0f;
+ pt.rgba = *reinterpret_cast<uint32_t*> (&rgba);
+ cloud_rgba_.points.push_back (pt);
+ }
+
+ toPCLPointCloud2 (cloud_rgba_, cloud_rgba_blob_);
+ cloud_rgba_blob_ptr_.reset (new PCLPointCloud2 (cloud_rgba_blob_));
+ cloud_rgba_ptr_.reset (new PointCloud<PointXYZRGBA> (cloud_rgba_));
+
+ PointCloud<PointXYZRGBA> output_rgba;
+ VoxelGrid<PointXYZRGBA> grid_rgba;
+
+ grid_rgba.setLeafSize (0.03f, 0.03f, 0.03f);
+ grid_rgba.setInputCloud (cloud_rgba_ptr_);
+ grid_rgba.filter (output_rgba);
+
+ EXPECT_EQ (int (output_rgba.points.size ()), 1);
+ EXPECT_EQ (int (output_rgba.width), 1);
+ EXPECT_EQ (int (output_rgba.height), 1);
+ EXPECT_EQ (bool (output_rgba.is_dense), true);
+ {
+ int rgba;
+ int r,g,b,a;
+ memcpy (&rgba, &(output_rgba.points[0].rgba), sizeof(int));
+ a = (rgba >> 24) & 0xFF; r = (rgba >> 16) & 0xFF; g = (rgba >> 8 ) & 0xFF; b = (rgba >> 0 ) & 0xFF;
+ EXPECT_NEAR (output_rgba.points[0].x, 0.0, 1e-4);
+ EXPECT_NEAR (output_rgba.points[0].y, 0.0, 1e-4);
+ EXPECT_NEAR (output_rgba.points[0].z, 0.0, 1e-4);
+ EXPECT_NEAR (r, ave_r, 1.0);
+ EXPECT_NEAR (g, ave_g, 1.0);
+ EXPECT_NEAR (b, ave_b, 1.0);
+ EXPECT_NEAR (a, ave_a, 1.0);
+ }
+
+ VoxelGrid<PCLPointCloud2> grid2;
+ PCLPointCloud2 output_rgba_blob;
+
+ grid2.setLeafSize (0.03f, 0.03f, 0.03f);
+ grid2.setInputCloud (cloud_rgba_blob_ptr_);
+ grid2.filter (output_rgba_blob);
+
+ fromPCLPointCloud2 (output_rgba_blob, output_rgba);
+
+ EXPECT_EQ (int (output_rgba.points.size ()), 1);
+ EXPECT_EQ (int (output_rgba.width), 1);
+ EXPECT_EQ (int (output_rgba.height), 1);
+ EXPECT_EQ (bool (output_rgba.is_dense), true);
+ {
+ int rgba;
+ int r,g,b,a;
+ memcpy (&rgba, &(output_rgba.points[0].rgba), sizeof(int));
+ a = (rgba >> 24) & 0xFF; r = (rgba >> 16) & 0xFF; g = (rgba >> 8 ) & 0xFF; b = (rgba >> 0 ) & 0xFF;
+ EXPECT_NEAR (output_rgba.points[0].x, 0.0, 1e-4);
+ EXPECT_NEAR (output_rgba.points[0].y, 0.0, 1e-4);
+ EXPECT_NEAR (output_rgba.points[0].z, 0.0, 1e-4);
+ EXPECT_NEAR (r, ave_r, 1.0);
+ EXPECT_NEAR (g, ave_g, 1.0);
+ EXPECT_NEAR (b, ave_b, 1.0);
+ EXPECT_NEAR (a, ave_a, 1.0);
+ }
+}
+
#if 0
////////////////////////////////////////////////////////////////////////////////
float getRandomNumber (float max = 1.0, float min = 0.0)
EdgeIndex ei;
FaceIndex fi;
std::istringstream iss ("1 2 3 4");
- EXPECT_TRUE (iss >> vi >> hei >> ei >> fi);
+ iss >> vi >> hei >> ei >> fi;
EXPECT_EQ (1, vi.get ());
EXPECT_EQ (2, hei.get ());
{
const FaceIndex index = mesh.addFace (ordered_faces [j]);
- if (j < non_manifold [i] || !Mesh::IsManifold::value)
+ if (j < static_cast<unsigned int> (non_manifold [i]) || !Mesh::IsManifold::value)
{
EXPECT_TRUE (index.isValid ());
}
-if(OPENNI_FOUND)
- PCL_ADD_TEST(io_io test_io
- FILES test_io.cpp
- LINK_WITH pcl_gtest pcl_io)
-endif(OPENNI_FOUND)
+PCL_ADD_TEST(io_io test_io
+ FILES test_io.cpp
+ LINK_WITH pcl_gtest pcl_io)
+
PCL_ADD_TEST(io_iterators test_iterators
FILES test_iterators.cpp
LINK_WITH pcl_gtest pcl_io)
PCL_ADD_TEST(point_cloud_image_extractors test_point_cloud_image_extractors
FILES test_point_cloud_image_extractors.cpp
LINK_WITH pcl_gtest pcl_io)
+
+PCL_ADD_TEST(buffers test_buffers
+ FILES test_buffers.cpp
+ LINK_WITH pcl_gtest)
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+#include <pcl/io/buffers.h>
+
+using namespace pcl::io;
+
+template <typename T>
+class BuffersTest : public ::testing::Test
+{
+
+ public:
+
+ BuffersTest ()
+ {
+ if (std::numeric_limits<T>::has_quiet_NaN)
+ invalid_ = std::numeric_limits<T>::quiet_NaN ();
+ else
+ invalid_ = 0;
+ }
+
+ template <typename Buffer> void
+ checkBuffer (Buffer& buffer, const T* data, const T* expected, size_t size)
+ {
+ const T* dptr = data;
+ const T* eptr = expected;
+ for (size_t i = 0; i < size; ++i)
+ {
+ std::vector<T> d (buffer.size ());
+ memcpy (d.data (), dptr, buffer.size () * sizeof (T));
+ buffer.push (d);
+ for (size_t j = 0; j < buffer.size (); ++j)
+ if (isnan (eptr[j]))
+ EXPECT_TRUE (isnan (buffer[j]));
+ else
+ EXPECT_EQ (eptr[j], buffer[j]);
+ dptr += buffer.size ();
+ eptr += buffer.size ();
+ }
+ }
+
+ T invalid_;
+
+};
+
+typedef ::testing::Types<char, int, float> DataTypes;
+TYPED_TEST_CASE (BuffersTest, DataTypes);
+
+TYPED_TEST (BuffersTest, SingleBuffer)
+{
+ SingleBuffer<TypeParam> sb (1);
+ const TypeParam data[] = {5, 4, 3, 2, 1};
+ this->checkBuffer (sb, data, data, sizeof (data) / sizeof (TypeParam));
+}
+
+TYPED_TEST (BuffersTest, MedianBufferWindow1)
+{
+ MedianBuffer<TypeParam> mb (1, 1);
+ const TypeParam data[] = {5, 4, 3, 2, 1};
+ this->checkBuffer (mb, data, data, sizeof (data) / sizeof (TypeParam));
+}
+
+TYPED_TEST (BuffersTest, MedianBufferWindow2)
+{
+ {
+ MedianBuffer<TypeParam> mb (1, 2);
+ const TypeParam data[] = {5, 4, 3, 2, 1};
+ const TypeParam median[] = {5, 5, 4, 3, 2};
+ this->checkBuffer (mb, data, median, sizeof (data) / sizeof (TypeParam));
+ }
+ {
+ MedianBuffer<TypeParam> mb (1, 2);
+ const TypeParam data[] = {3, 4, 1, 3, 4};
+ const TypeParam median[] = {3, 4, 4, 3, 4};
+ this->checkBuffer (mb, data, median, sizeof (data) / sizeof (TypeParam));
+ }
+}
+
+TYPED_TEST (BuffersTest, MedianBufferWindow3)
+{
+ {
+ MedianBuffer<TypeParam> mb (1, 3);
+ const TypeParam data[] = {5, 4, 3, 2, 1, -1, -1};
+ const TypeParam median[] = {5, 5, 4, 3, 2, 1, -1};
+ this->checkBuffer (mb, data, median, sizeof (data) / sizeof (TypeParam));
+ }
+ {
+ MedianBuffer<TypeParam> mb (1, 3);
+ const TypeParam data[] = {3, 4, 1, 3, 4, -1, -1};
+ const TypeParam median[] = {3, 4, 3, 3, 3, 3, -1};
+ this->checkBuffer (mb, data, median, sizeof (data) / sizeof (TypeParam));
+ }
+ {
+ MedianBuffer<TypeParam> mb (1, 3);
+ const TypeParam data[] = {-4, -1, 3, -4, 1, 3, 4, -1};
+ const TypeParam median[] = {-4, -1, -1, -1, 1, 1, 3, 3};
+ this->checkBuffer (mb, data, median, sizeof (data) / sizeof (TypeParam));
+ }
+}
+
+TYPED_TEST (BuffersTest, MedianBufferWindow4)
+{
+ {
+ MedianBuffer<TypeParam> mb (1, 4);
+ const TypeParam data[] = {5, 4, 3, 2, 1, -1, -1};
+ const TypeParam median[] = {5, 5, 4, 4, 3, 2, 1};
+ this->checkBuffer (mb, data, median, sizeof (data) / sizeof (TypeParam));
+ }
+ {
+ MedianBuffer<TypeParam> mb (1, 4);
+ const TypeParam data[] = {-4, -1, 3, -4, 1, 3, 4, -2};
+ const TypeParam median[] = {-4, -1, -1, -1, 1, 3, 3, 3};
+ this->checkBuffer (mb, data, median, sizeof (data) / sizeof (TypeParam));
+ }
+}
+
+TYPED_TEST (BuffersTest, MedianBufferPushInvalid)
+{
+ const TypeParam& invalid = this->invalid_;
+ MedianBuffer<TypeParam> mb (1, 3);
+ const TypeParam data[] = {5, 4, 3, invalid, 1, invalid, invalid, invalid, 9, 3, 1};
+ const TypeParam median[] = {5, 5, 4, 4, 3, 1, 1, invalid, 9, 9, 3};
+ this->checkBuffer (mb, data, median, sizeof (data) / sizeof (TypeParam));
+}
+
+TYPED_TEST (BuffersTest, MedianBufferSize3Window3)
+{
+ {
+ MedianBuffer<TypeParam> mb (3, 3);
+ const TypeParam data[] = {3, 3, 3, 1, 1, 1, -1, -1, -1};
+ const TypeParam median[] = {3, 3, 3, 3, 3, 3, 1, 1, 1};
+ this->checkBuffer (mb, data, median, sizeof (data) / sizeof (TypeParam) / mb.size ());
+ }
+ {
+ MedianBuffer<TypeParam> mb (3, 3);
+ const TypeParam data[] = {3, 2, 1, 1, 1, 1, 3, 2, 1, 1, 2, 3};
+ const TypeParam median[] = {3, 2, 1, 3, 2, 1, 3, 2, 1, 1, 2, 1};
+ this->checkBuffer (mb, data, median, sizeof (data) / sizeof (TypeParam) / mb.size ());
+ }
+}
+
+TYPED_TEST (BuffersTest, AverageBufferWindow1)
+{
+ AverageBuffer<TypeParam> ab (1, 1);
+ const TypeParam data[] = {5, 4, 3, 2, 1};
+ this->checkBuffer (ab, data, data, sizeof (data) / sizeof (TypeParam));
+}
+
+TYPED_TEST (BuffersTest, AverageBufferWindow2)
+{
+ {
+ AverageBuffer<TypeParam> ab (1, 2);
+ const TypeParam data[] = {5, 3, 3, 1, 1};
+ const TypeParam average[] = {5, 4, 3, 2, 1};
+ this->checkBuffer (ab, data, average, sizeof (data) / sizeof (TypeParam));
+ }
+ {
+ AverageBuffer<TypeParam> ab (1, 2);
+ const TypeParam data[] = {3, 5, 1, 13, 3};
+ const TypeParam average[] = {3, 4, 3, 7, 8};
+ this->checkBuffer (ab, data, average, sizeof (data) / sizeof (TypeParam));
+ }
+}
+
+TYPED_TEST (BuffersTest, AverageBufferWindow3)
+{
+ {
+ AverageBuffer<TypeParam> ab (1, 3);
+ const TypeParam data[] = {5, 3, 1, 2, -3, 4, -7};
+ const TypeParam average[] = {5, 4, 3, 2, 0, 1, -2};
+ this->checkBuffer (ab, data, average, sizeof (data) / sizeof (TypeParam));
+ }
+ {
+ AverageBuffer<TypeParam> ab (1, 3);
+ const TypeParam data[] = {3, -5, 2, -3, 4, -1, -3};
+ const TypeParam average[] = {3, -1, 0, -2, 1, 0, 0};
+ this->checkBuffer (ab, data, average, sizeof (data) / sizeof (TypeParam));
+ }
+}
+
+TYPED_TEST (BuffersTest, AverageBufferPushInvalid)
+{
+ const TypeParam& invalid = this->invalid_;
+ AverageBuffer<TypeParam> ab (1, 3);
+ const TypeParam data[] = {5, 3, 7, invalid, 1, invalid, invalid, invalid, 9, 3, -3};
+ const TypeParam median[] = {5, 4, 5, 5, 4, 1, 1, invalid, 9, 6, 3};
+ this->checkBuffer (ab, data, median, sizeof (data) / sizeof (TypeParam));
+}
+
+int main (int argc, char **argv)
+{
+ testing::InitGoogleTest (&argc, argv);
+ return RUN_ALL_TESTS ();
+}
+
}
}
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST (PCL, ASCIIReader)
{
EXPECT_GE(reader.read("test_pcd.txt", rcloud), 0);
EXPECT_EQ(cloud.points.size(), rcloud.points.size() );
- for(int i=0;i < rcloud.points.size(); i++){
+ for(size_t i=0;i < rcloud.points.size(); i++){
EXPECT_FLOAT_EQ(cloud.points[i].x, rcloud.points[i].x);
EXPECT_FLOAT_EQ(cloud.points[i].y,rcloud.points[i].y);
EXPECT_FLOAT_EQ(cloud.points[i].z, rcloud.points[i].z);
}
}
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+class PLYTest : public ::testing::Test
+{
+ protected:
+
+ PLYTest () : mesh_file_ply_("ply_color_mesh.ply")
+ {
+ std::ofstream fs;
+ fs.open (mesh_file_ply_.c_str ());
+ fs << "ply\n"
+ "format ascii 1.0\n"
+ "element vertex 4\n"
+ "property float x\n"
+ "property float y\n"
+ "property float z\n"
+ "property uchar red\n"
+ "property uchar green\n"
+ "property uchar blue\n"
+ "property uchar alpha\n"
+ "element face 2\n"
+ "property list uchar int vertex_indices\n"
+ "end_header\n"
+ "4.23607 0 1.61803 255 0 0 255\n"
+ "2.61803 2.61803 2.61803 0 255 0 0\n"
+ "0 1.61803 4.23607 0 0 255 128\n"
+ "0 -1.61803 4.23607 255 255 255 128\n"
+ "3 0 1 2\n"
+ "3 1 2 3\n";
+ fs.close ();
+
+ // Test colors from ply_benchmark.ply
+ rgba_1_ = static_cast<uint32_t> (255) << 24 | static_cast<uint32_t> (255) << 16 |
+ static_cast<uint32_t> (0) << 8 | static_cast<uint32_t> (0);
+ rgba_2_ = static_cast<uint32_t> (0) << 24 | static_cast<uint32_t> (0) << 16 |
+ static_cast<uint32_t> (255) << 8 | static_cast<uint32_t> (0);
+ rgba_3_ = static_cast<uint32_t> (128) << 24 | static_cast<uint32_t> (0) << 16 |
+ static_cast<uint32_t> (0) << 8 | static_cast<uint32_t> (255);
+ rgba_4_ = static_cast<uint32_t> (128) << 24 | static_cast<uint32_t> (255) << 16 |
+ static_cast<uint32_t> (255) << 8 | static_cast<uint32_t> (255);
+ }
+
+ virtual
+ ~PLYTest () { remove (mesh_file_ply_.c_str ()); }
+
+ std::string mesh_file_ply_;
+ uint32_t rgba_1_;
+ uint32_t rgba_2_;
+ uint32_t rgba_3_;
+ uint32_t rgba_4_;
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST_F (PLYTest, LoadPLYFileColoredASCIIIntoBlob)
+{
+ int res;
+ uint32_t rgba;
+
+ PCLPointCloud2 cloud_blob;
+ uint32_t ps;
+ int32_t offset = -1;
+
+ // check if loading is ok
+ res = loadPLYFile (mesh_file_ply_, cloud_blob);
+ ASSERT_EQ (res, 0);
+
+ // blob has proper structure
+ EXPECT_EQ (cloud_blob.height, 1);
+ EXPECT_EQ (cloud_blob.width, 4);
+ EXPECT_EQ (cloud_blob.fields.size(), 4);
+ EXPECT_FALSE (cloud_blob.is_bigendian);
+ EXPECT_EQ (cloud_blob.point_step, 16);
+ EXPECT_EQ (cloud_blob.row_step, 16 * 4);
+ EXPECT_EQ (cloud_blob.data.size(), 16 * 4);
+ // EXPECT_TRUE (cloud_blob.is_dense); // this is failing and it shouldnt?
+
+ // scope blob data
+ ps = cloud_blob.point_step;
+ for (size_t i = 0; i < cloud_blob.fields.size (); ++i)
+ if (cloud_blob.fields[i].name == std::string("rgba"))
+ offset = static_cast<int32_t> (cloud_blob.fields[i].offset);
+
+ ASSERT_GE (offset, 0);
+
+ // 1st point
+ rgba = *reinterpret_cast<uint32_t *> (&cloud_blob.data[offset]);
+ ASSERT_EQ (rgba, rgba_1_);
+
+ // 2th point
+ rgba = *reinterpret_cast<uint32_t *> (&cloud_blob.data[ps + offset]);
+ ASSERT_EQ (rgba, rgba_2_);
+
+ // 3th point
+ rgba = *reinterpret_cast<uint32_t *> (&cloud_blob.data[2 * ps + offset]);
+ ASSERT_EQ (rgba, rgba_3_);
+
+ // 4th point
+ rgba = *reinterpret_cast<uint32_t *> (&cloud_blob.data[3 * ps + offset]);
+ ASSERT_EQ (rgba, rgba_4_);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST_F (PLYTest, LoadPLYFileColoredASCIIIntoPolygonMesh)
+{
+ int res;
+ uint32_t rgba;
+ PolygonMesh mesh;
+ uint32_t ps;
+ int32_t offset = -1;
+
+ // check if loading is ok
+ res = loadPLYFile (mesh_file_ply_, mesh);
+ ASSERT_EQ (res, 0);
+
+ // blob has proper structure
+ EXPECT_EQ (mesh.cloud.height, 1);
+ EXPECT_EQ (mesh.cloud.width, 4);
+ EXPECT_EQ (mesh.cloud.fields.size(), 4);
+ EXPECT_FALSE (mesh.cloud.is_bigendian);
+ EXPECT_EQ (mesh.cloud.point_step, 16);
+ EXPECT_EQ (mesh.cloud.row_step, 16 * 4);
+ EXPECT_EQ (mesh.cloud.data.size(), 16 * 4);
+ // EXPECT_TRUE (mesh.cloud.is_dense); // this is failing and it shouldnt?
+
+ // scope blob data
+ ps = mesh.cloud.point_step;
+ for (size_t i = 0; i < mesh.cloud.fields.size (); ++i)
+ if (mesh.cloud.fields[i].name == std::string("rgba"))
+ offset = static_cast<int32_t> (mesh.cloud.fields[i].offset);
+
+ ASSERT_GE (offset, 0);
+
+ // 1st point
+ rgba = *reinterpret_cast<uint32_t *> (&mesh.cloud.data[offset]);
+ ASSERT_EQ (rgba, rgba_1_);
+
+ // 2th point
+ rgba = *reinterpret_cast<uint32_t *> (&mesh.cloud.data[ps + offset]);
+ ASSERT_EQ (rgba, rgba_2_);
+
+ // 3th point
+ rgba = *reinterpret_cast<uint32_t *> (&mesh.cloud.data[2 * ps + offset]);
+ ASSERT_EQ (rgba, rgba_3_);
+
+ // 4th point
+ rgba = *reinterpret_cast<uint32_t *> (&mesh.cloud.data[3 * ps + offset]);
+ ASSERT_EQ (rgba, rgba_4_);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename T> class PLYPointCloudTest : public PLYTest { };
+typedef ::testing::Types<BOOST_PP_SEQ_ENUM (PCL_RGB_POINT_TYPES)> RGBPointTypes;
+TYPED_TEST_CASE (PLYPointCloudTest, RGBPointTypes);
+TYPED_TEST (PLYPointCloudTest, LoadPLYFileColoredASCIIIntoPointCloud)
+{
+ int res;
+ PointCloud<TypeParam> cloud_rgb;
+
+ // check if loading is ok
+ res = loadPLYFile (PLYTest::mesh_file_ply_, cloud_rgb);
+ ASSERT_EQ (res, 0);
+
+ // cloud has proper structure
+ EXPECT_EQ (cloud_rgb.height, 1);
+ EXPECT_EQ (cloud_rgb.width, 4);
+ EXPECT_EQ (cloud_rgb.points.size(), 4);
+ // EXPECT_TRUE (cloud_rgb.is_dense); // this is failing and it shouldnt?
+
+ // scope cloud data
+ ASSERT_EQ (cloud_rgb[0].rgba, PLYTest::rgba_1_);
+ ASSERT_EQ (cloud_rgb[1].rgba, PLYTest::rgba_2_);
+ ASSERT_EQ (cloud_rgb[2].rgba, PLYTest::rgba_3_);
+ ASSERT_EQ (cloud_rgb[3].rgba, PLYTest::rgba_4_);
+}
+
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
struct PointXYZFPFH33
std::locale::global (std::locale ("de_DE.UTF-8"));
#endif
}
- catch (std::runtime_error e)
+ catch (const std::runtime_error&)
{
PCL_WARN ("Failed to set locale, skipping test.\n");
}
std::locale::global (std::locale ("en_US.UTF-8"));
#endif
}
- catch (std::runtime_error e)
+ catch (const std::runtime_error&)
{
PCL_WARN ("Failed to set locale, skipping test.\n");
}
ASSERT_FLOAT_EQ (cloud2.points[i].z, cloud.points[i].z);
}
}
- catch(std::exception& e)
+ catch (const std::exception&)
{
}
#endif
pcl::PCLImage image;
PointCloudImageExtractorFromNormalField<PointT> pcie;
- ASSERT_TRUE (pcie.extract(cloud, image));
+ ASSERT_TRUE (pcie.extract (cloud, image));
EXPECT_EQ ("rgb8", image.encoding);
EXPECT_EQ (cloud.width, image.width);
pcl::PCLImage image;
PointCloudImageExtractorFromRGBField<PointT> pcie;
- ASSERT_TRUE (pcie.extract(cloud, image));
+ ASSERT_TRUE (pcie.extract (cloud, image));
EXPECT_EQ ("rgb8", image.encoding);
EXPECT_EQ (cloud.width, image.width);
pcl::PCLImage image;
PointCloudImageExtractorFromRGBField<PointT> pcie;
- ASSERT_TRUE (pcie.extract(cloud, image));
+ ASSERT_TRUE (pcie.extract (cloud, image));
EXPECT_EQ ("rgb8", image.encoding);
EXPECT_EQ (cloud.width, image.width);
PointCloudImageExtractorFromLabelField<PointT> pcie;
pcie.setColorMode (pcie.COLORS_MONO);
- ASSERT_TRUE (pcie.extract(cloud, image));
+ ASSERT_TRUE (pcie.extract (cloud, image));
unsigned short* data = reinterpret_cast<unsigned short*> (&image.data[0]);
EXPECT_EQ ("mono16", image.encoding);
PointCloudImageExtractorFromLabelField<PointT> pcie;
pcie.setColorMode (pcie.COLORS_RGB_RANDOM);
- ASSERT_TRUE (pcie.extract(cloud, image));
+ ASSERT_TRUE (pcie.extract (cloud, image));
EXPECT_EQ ("rgb8", image.encoding);
EXPECT_EQ (cloud.width, image.width);
PointCloudImageExtractorFromLabelField<PointT> pcie;
pcie.setColorMode (pcie.COLORS_RGB_GLASBEY);
- ASSERT_TRUE (pcie.extract(cloud, image));
+ ASSERT_TRUE (pcie.extract (cloud, image));
EXPECT_EQ ("rgb8", image.encoding);
EXPECT_EQ (cloud.width, image.width);
for (size_t i = 0; i < cloud.points.size (); i++)
cloud.points[i].label = i % 2 + 10;
pcl::PCLImage image2;
- ASSERT_TRUE (pcie.extract(cloud, image2));
+ ASSERT_TRUE (pcie.extract (cloud, image2));
- // The first color should be pure blue
- EXPECT_EQ (0, image.data[0]);
- EXPECT_EQ (0, image.data[1]);
- EXPECT_EQ (255, image.data[2]);
+ // The first label should get the first Glasbey color
+ EXPECT_EQ (GlasbeyLUT::data ()[0], image.data[0]);
+ EXPECT_EQ (GlasbeyLUT::data ()[1], image.data[1]);
+ EXPECT_EQ (GlasbeyLUT::data ()[2], image.data[2]);
// Make sure the colors are the same
for (size_t i = 0; i < 2 * 2 * 3; ++i)
EXPECT_EQ (image2.data[i], image.data[i]);
pcl::PCLImage image;
PointCloudImageExtractorFromZField<PointT> pcie;
- ASSERT_TRUE (pcie.extract(cloud, image));
+ ASSERT_TRUE (pcie.extract (cloud, image));
unsigned short* data = reinterpret_cast<unsigned short*> (&image.data[0]);
EXPECT_EQ ("mono16", image.encoding);
pcl::PCLImage image;
PointCloudImageExtractorFromCurvatureField<PointT> pcie;
- ASSERT_TRUE (pcie.extract(cloud, image));
+ ASSERT_TRUE (pcie.extract (cloud, image));
unsigned short* data = reinterpret_cast<unsigned short*> (&image.data[0]);
EXPECT_EQ ("mono16", image.encoding);
// by default Curvature field extractor scales to full range of unsigned short
EXPECT_EQ (0, data[0]);
- EXPECT_EQ (std::numeric_limits<unsigned short>::max() , data[1]);
+ EXPECT_EQ (std::numeric_limits<unsigned short>::max () , data[1]);
EXPECT_EQ (0, data[2]);
- EXPECT_EQ (std::numeric_limits<unsigned short>::max() , data[3]);
+ EXPECT_EQ (std::numeric_limits<unsigned short>::max () , data[3]);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
pcl::PCLImage image;
PointCloudImageExtractorFromIntensityField<PointT> pcie;
- ASSERT_TRUE (pcie.extract(cloud, image));
+ ASSERT_TRUE (pcie.extract (cloud, image));
unsigned short* data = reinterpret_cast<unsigned short*> (&image.data[0]);
EXPECT_EQ ("mono16", image.encoding);
// by default Intensity field extractor does not apply scaling
for (size_t i = 0; i < cloud.points.size (); i++)
- EXPECT_EQ (static_cast<unsigned short>(cloud.points[i].intensity), data[i]);
+ EXPECT_EQ (static_cast<unsigned short> (cloud.points[i].intensity), data[i]);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
PointCloudImageExtractorFromCurvatureField<PointT> pcie;
- ASSERT_TRUE (pcie.extract(cloud, image));
+ ASSERT_TRUE (pcie.extract (cloud, image));
{
unsigned short* data = reinterpret_cast<unsigned short*> (&image.data[0]);
- EXPECT_EQ (std::numeric_limits<unsigned short>::max(), data[3]);
+ EXPECT_EQ (std::numeric_limits<unsigned short>::max (), data[3]);
}
pcie.setPaintNaNsWithBlack (true);
- ASSERT_TRUE (pcie.extract(cloud, image));
+ ASSERT_TRUE (pcie.extract (cloud, image));
{
unsigned short* data = reinterpret_cast<unsigned short*> (&image.data[0]);
ASSERT_EQ (keypoints.width, keypoints.points.size ());
ASSERT_EQ (keypoints.height, 1);
EXPECT_EQ (keypoints.points.size (), static_cast<size_t> (169));
+ EXPECT_EQ (keypoints.header, cloud_xyzi->header);
+ EXPECT_EQ (keypoints.sensor_origin_ (0), cloud_xyzi->sensor_origin_ (0));
+ EXPECT_EQ (keypoints.sensor_origin_ (1), cloud_xyzi->sensor_origin_ (1));
+ EXPECT_EQ (keypoints.sensor_origin_ (2), cloud_xyzi->sensor_origin_ (2));
+ EXPECT_EQ (keypoints.sensor_origin_ (3), cloud_xyzi->sensor_origin_ (3));
+ EXPECT_EQ (keypoints.sensor_orientation_.w (), cloud_xyzi->sensor_orientation_.w ());
+ EXPECT_EQ (keypoints.sensor_orientation_.x (), cloud_xyzi->sensor_orientation_.x ());
+ EXPECT_EQ (keypoints.sensor_orientation_.y (), cloud_xyzi->sensor_orientation_.y ());
+ EXPECT_EQ (keypoints.sensor_orientation_.z (), cloud_xyzi->sensor_orientation_.z ());
// Change the values and re-compute
sift_detector.setScales (0.05f, 5, 3);
LINK_WITH pcl_gtest pcl_io pcl_registration pcl_features
ARGUMENTS "${PCL_SOURCE_DIR}/test/bunny.pcd")
+PCL_ADD_TEST(fpcs_ia test_fpcs_ia
+ FILES test_fpcs_ia.cpp test_fpcs_ia_data.h
+ LINK_WITH pcl_gtest pcl_io pcl_registration pcl_features pcl_search pcl_kdtree
+ ARGUMENTS "${PCL_SOURCE_DIR}/test/bun0.pcd" "${PCL_SOURCE_DIR}/test/bun4.pcd")
+
+PCL_ADD_TEST(kfpcs_ia test_kfpcs_ia
+ FILES test_kfpcs_ia.cpp test_kfpcs_ia_data.h
+ LINK_WITH pcl_gtest pcl_io pcl_registration pcl_features pcl_search pcl_kdtree
+ ARGUMENTS "${PCL_SOURCE_DIR}/test/office1_keypoints.pcd" "${PCL_SOURCE_DIR}/test/office2_keypoints.pcd")
\ No newline at end of file
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Point Cloud Library (PCL) - www.pointclouds.org
+* Copyright (c) 2014-, Open Perception, Inc.
+*
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the copyright holder(s) nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+*/
+
+#include <gtest/gtest.h>
+
+#include <pcl/point_types.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/registration/ia_fpcs.h>
+
+#include "test_fpcs_ia_data.h"
+
+using namespace pcl;
+using namespace pcl::io;
+using namespace pcl::registration;
+using namespace std;
+
+PointCloud<PointXYZ> cloud_source, cloud_target;
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST (PCL, FPCSInitialAlignment)
+{
+ // transform the source cloud by a large amount
+ Eigen::Vector3f initial_offset (1.f, 0.f, 0.f);
+ float angle = static_cast<float> (M_PI) / 2.f;
+ Eigen::Quaternionf initial_rotation (cos (angle / 2.f), 0, 0, sin (angle / 2.f));
+ PointCloud<PointXYZ> cloud_source_transformed;
+ transformPointCloud (cloud_source, cloud_source_transformed, initial_offset, initial_rotation);
+
+ // create shared pointers
+ PointCloud<PointXYZ>::Ptr cloud_source_ptr, cloud_target_ptr;
+ cloud_source_ptr = cloud_source_transformed.makeShared ();
+ cloud_target_ptr = cloud_target.makeShared ();
+
+ // initialize fpcs
+ PointCloud <PointXYZ> source_aligned;
+ FPCSInitialAlignment <PointXYZ, PointXYZ> fpcs_ia;
+ fpcs_ia.setInputSource (cloud_source_ptr);
+ fpcs_ia.setInputTarget (cloud_target_ptr);
+
+ fpcs_ia.setNumberOfThreads (nr_threads);
+ fpcs_ia.setApproxOverlap (approx_overlap);
+ fpcs_ia.setDelta (delta, true);
+ fpcs_ia.setNumberOfSamples (nr_samples);
+
+ // align
+ fpcs_ia.align (source_aligned);
+ EXPECT_EQ (static_cast <int> (source_aligned.points.size ()), static_cast <int> (cloud_source.points.size ()));
+
+ // check for correct coarse transformation marix
+ //Eigen::Matrix4f transform_res_from_fpcs = fpcs_ia.getFinalTransformation ();
+ //for (int i = 0; i < 4; ++i)
+ // for (int j = 0; j < 4; ++j)
+ // EXPECT_NEAR (transform_res_from_fpcs (i,j), transform_from_fpcs[i][j], 0.5);
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+int
+main (int argc, char** argv)
+{
+ if (argc < 3)
+ {
+ std::cerr << "No test files given. Please download `bun0.pcd` and `bun4.pcd` pass their path to the test." << std::endl;
+ return (-1);
+ }
+
+ // Input
+ if (loadPCDFile (argv[1], cloud_source) < 0)
+ {
+ std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl;
+ return (-1);
+ }
+ if (loadPCDFile (argv[2], cloud_target) < 0)
+ {
+ std::cerr << "Failed to read test file. Please download `bun4.pcd` and pass its path to the test." << std::endl;
+ return (-1);
+ }
+
+ testing::InitGoogleTest (&argc, argv);
+ return (RUN_ALL_TESTS ());
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
--- /dev/null
+#ifndef TEST_FPCS_DATA_H_
+#define TEST_FPCS_DATA_H_
+
+const int nr_threads = 1;
+const float approx_overlap = 0.9f;
+const float delta = 1.f;
+const int nr_samples = 100;
+
+const float transform_from_fpcs [4][4] = {
+ { -0.0019f, 0.8266f, -0.5628f, -0.0378f },
+ { -0.9999f, -0.0094f, -0.0104f, 0.9997f },
+ { -0.0139f, 0.5627f, 0.8265f, 0.0521f },
+ { 0.f, 0.f, 0.f, 1.f }
+};
+
+#endif // TEST_FPCS_DATA_H_
--- /dev/null
+/*
+* Software License Agreement (BSD License)
+*
+* Point Cloud Library (PCL) - www.pointclouds.org
+* Copyright (c) 2014-, Open Perception, Inc.
+*
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the copyright holder(s) nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+*/
+
+#include <gtest/gtest.h>
+
+#include <pcl/point_types.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/registration/ia_kfpcs.h>
+
+#include "test_kfpcs_ia_data.h"
+
+using namespace pcl;
+using namespace pcl::io;
+using namespace pcl::registration;
+using namespace std;
+
+PointCloud<PointXYZI> cloud_source, cloud_target;
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST (PCL, KFPCSInitialAlignment)
+{
+ // create shared pointers
+ PointCloud<PointXYZI>::Ptr cloud_source_ptr, cloud_target_ptr;
+ cloud_source_ptr = cloud_source.makeShared ();
+ cloud_target_ptr = cloud_target.makeShared ();
+
+ // initialize k-fpcs
+ PointCloud <PointXYZI> cloud_source_aligned;
+ KFPCSInitialAlignment <PointXYZI, PointXYZI> kfpcs_ia;
+ kfpcs_ia.setInputSource (cloud_source_ptr);
+ kfpcs_ia.setInputTarget (cloud_target_ptr);
+
+ //kfpcs_ia.setNumberOfThreads (nr_threads);
+ kfpcs_ia.setApproxOverlap (approx_overlap);
+ kfpcs_ia.setDelta (voxel_size, false);
+ kfpcs_ia.setScoreThreshold (abort_score);
+
+ // repeat alignment 2 times to increase probability to ~99.99%
+ const float max_angle3d = 0.1745f, max_translation3d = 1.f;
+ float angle3d = FLT_MAX, translation3d = FLT_MAX;
+ for (int i = 0; i < 2; i++)
+ {
+ kfpcs_ia.align (cloud_source_aligned);
+
+ // copy initial matrix
+ Eigen::Matrix4f transform_groundtruth;
+ for (int i = 0; i < 4; i++)
+ for (int j = 0; j < 4; j++)
+ transform_groundtruth (i, j) = transformation_office1_office2[i][j];
+
+ // check for correct transformation
+ Eigen::Matrix4f transform_rest = kfpcs_ia.getFinalTransformation ().colPivHouseholderQr ().solve (transform_groundtruth);
+ angle3d = std::min (angle3d, Eigen::AngleAxisf (transform_rest.block <3, 3> (0, 0)).angle ());
+ translation3d = std::min (translation3d, transform_rest.block <3, 1> (0, 3).norm ());
+
+ if (angle3d < max_angle3d && translation3d < max_translation3d)
+ break;
+ }
+
+ EXPECT_EQ (static_cast <int> (cloud_source_aligned.points.size ()), static_cast <int> (cloud_source.points.size ()));
+ EXPECT_NEAR (angle3d, 0.f, max_angle3d);
+ EXPECT_NEAR (translation3d, 0.f, max_translation3d);
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+int
+main (int argc, char** argv)
+{
+ if (argc < 3)
+ {
+ std::cerr << "No test files given. Please download `bun0.pcd` and `bun4.pcd` pass their path to the test." << std::endl;
+ return (-1);
+ }
+
+ // Input
+ if (loadPCDFile (argv[1], cloud_source) < 0)
+ {
+ std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl;
+ return (-1);
+ }
+ if (loadPCDFile (argv[2], cloud_target) < 0)
+ {
+ std::cerr << "Failed to read test file. Please download `bun4.pcd` and pass its path to the test." << std::endl;
+ return (-1);
+ }
+
+ testing::InitGoogleTest (&argc, argv);
+ return (RUN_ALL_TESTS ());
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
--- /dev/null
+#ifndef TEST_KFPCS_DATA_H_
+#define TEST_KFPCS_DATA_H_
+
+const int nr_threads = 1;
+const float voxel_size = 0.1f;
+const float approx_overlap = 0.9f;
+const float abort_score = 0.0f;
+
+const float transformation_office1_office2 [4][4] = {
+ { -0.6946f, -0.7194f, -0.0051f, -3.6352f },
+ { 0.7194f, -0.6945f, -0.0100f, -2.3865f },
+ { 0.0037f, -0.0106f, 0.9999f, 0.7778f },
+ { 0.0000f, 0.0000f, 0.0000f, 1.0000f }
+};
+
+#endif // TEST_KFPCS_DATA_H_
void
sampleRandomTransform (Eigen::Affine3f &trans, float max_angle, float max_trans)
{
+ srand(0);
// Sample random transform
Eigen::Vector3f axis((float)rand() / RAND_MAX, (float)rand() / RAND_MAX, (float)rand() / RAND_MAX);
axis.normalize();
FILES test_sample_consensus_quadric_models.cpp
LINK_WITH pcl_gtest pcl_sample_consensus)
-PCL_ADD_TEST(sample_consensus_line_model test_sample_consensus_line_model
- FILES test_sample_consensus_line_model.cpp
+PCL_ADD_TEST(sample_consensus_line_models test_sample_consensus_line_models
+ FILES test_sample_consensus_line_models.cpp
LINK_WITH pcl_gtest pcl_sample_consensus)
+++ /dev/null
-/*
- * Software License Agreement (BSD License)
- *
- * Point Cloud Library (PCL) - www.pointclouds.org
- * Copyright (c) 2010-2012, Willow Garage, Inc.
- * Copyright (c) 2014-, Open Perception, Inc.
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the copyright holder(s) nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- */
-
-#include <gtest/gtest.h>
-
-#include <pcl/pcl_tests.h>
-
-#include <pcl/sample_consensus/ransac.h>
-#include <pcl/sample_consensus/sac_model_line.h>
-
-using namespace pcl;
-
-typedef SampleConsensusModelLine<PointXYZ>::Ptr SampleConsensusModelLinePtr;
-
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-TEST (SampleConsensusModelLine, RANSAC)
-{
- srand (0);
-
- // Use a custom point cloud for these tests until we need something better
- PointCloud<PointXYZ> cloud;
- cloud.points.resize (10);
-
- cloud.points[0].getVector3fMap () << 1.0f, 2.00f, 3.00f;
- cloud.points[1].getVector3fMap () << 4.0f, 5.00f, 6.00f;
- cloud.points[2].getVector3fMap () << 7.0f, 8.00f, 9.00f;
- cloud.points[3].getVector3fMap () << 10.0f, 11.00f, 12.00f;
- cloud.points[4].getVector3fMap () << 13.0f, 14.00f, 15.00f;
- cloud.points[5].getVector3fMap () << 16.0f, 17.00f, 18.00f;
- cloud.points[6].getVector3fMap () << 19.0f, 20.00f, 21.00f;
- cloud.points[7].getVector3fMap () << 22.0f, 23.00f, 24.00f;
- cloud.points[8].getVector3fMap () << -5.0f, 1.57f, 0.75f;
- cloud.points[9].getVector3fMap () << 4.0f, 2.00f, 3.00f;
-
- // Create a shared line model pointer directly
- SampleConsensusModelLinePtr model (new SampleConsensusModelLine<PointXYZ> (cloud.makeShared ()));
-
- // Create the RANSAC object
- RandomSampleConsensus<PointXYZ> sac (model, 0.001);
-
- // Algorithm tests
- bool result = sac.computeModel ();
- ASSERT_TRUE (result);
-
- std::vector<int> sample;
- sac.getModel (sample);
- EXPECT_EQ (2, sample.size ());
-
- std::vector<int> inliers;
- sac.getInliers (inliers);
- EXPECT_EQ (8, inliers.size ());
-
- Eigen::VectorXf coeff;
- sac.getModelCoefficients (coeff);
- EXPECT_EQ (6, coeff.size ());
- EXPECT_NEAR (1, coeff[4] / coeff[3], 1e-4);
- EXPECT_NEAR (1, coeff[5] / coeff[3], 1e-4);
-
- Eigen::VectorXf coeff_refined;
- model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
- EXPECT_EQ (6, coeff_refined.size ());
- EXPECT_NEAR (1, coeff[4] / coeff[3], 1e-4);
- EXPECT_NEAR (1, coeff[5] / coeff[3], 1e-4);
-
- // Projection tests
- PointCloud<PointXYZ> proj_points;
- model->projectPoints (inliers, coeff_refined, proj_points);
-
- EXPECT_XYZ_NEAR (PointXYZ ( 7.0, 8.0, 9.0), proj_points.points[2], 1e-4);
- EXPECT_XYZ_NEAR (PointXYZ (10.0, 11.0, 12.0), proj_points.points[3], 1e-4);
- EXPECT_XYZ_NEAR (PointXYZ (16.0, 17.0, 18.0), proj_points.points[5], 1e-4);
-}
-
-int
-main (int argc, char** argv)
-{
- testing::InitGoogleTest (&argc, argv);
- return (RUN_ALL_TESTS ());
-}
-
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <gtest/gtest.h>
+
+#include <pcl/pcl_tests.h>
+
+#include <pcl/sample_consensus/ransac.h>
+#include <pcl/sample_consensus/sac_model_line.h>
+#include <pcl/sample_consensus/sac_model_parallel_line.h>
+
+using namespace pcl;
+
+typedef SampleConsensusModelLine<PointXYZ>::Ptr SampleConsensusModelLinePtr;
+typedef SampleConsensusModelParallelLine<PointXYZ>::Ptr SampleConsensusModelParallelLinePtr;
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST (SampleConsensusModelLine, RANSAC)
+{
+ // Use a custom point cloud for these tests until we need something better
+ PointCloud<PointXYZ> cloud;
+ cloud.points.resize (10);
+
+ cloud.points[0].getVector3fMap () << 1.0f, 2.00f, 3.00f;
+ cloud.points[1].getVector3fMap () << 4.0f, 5.00f, 6.00f;
+ cloud.points[2].getVector3fMap () << 7.0f, 8.00f, 9.00f;
+ cloud.points[3].getVector3fMap () << 10.0f, 11.00f, 12.00f;
+ cloud.points[4].getVector3fMap () << 13.0f, 14.00f, 15.00f;
+ cloud.points[5].getVector3fMap () << 16.0f, 17.00f, 18.00f;
+ cloud.points[6].getVector3fMap () << 19.0f, 20.00f, 21.00f;
+ cloud.points[7].getVector3fMap () << 22.0f, 23.00f, 24.00f;
+ cloud.points[8].getVector3fMap () << -5.0f, 1.57f, 0.75f;
+ cloud.points[9].getVector3fMap () << 4.0f, 2.00f, 3.00f;
+
+ // Create a shared line model pointer directly
+ SampleConsensusModelLinePtr model (new SampleConsensusModelLine<PointXYZ> (cloud.makeShared ()));
+
+ // Create the RANSAC object
+ RandomSampleConsensus<PointXYZ> sac (model, 0.001);
+
+ // Algorithm tests
+ bool result = sac.computeModel ();
+ ASSERT_TRUE (result);
+
+ std::vector<int> sample;
+ sac.getModel (sample);
+ EXPECT_EQ (2, sample.size ());
+
+ std::vector<int> inliers;
+ sac.getInliers (inliers);
+ EXPECT_EQ (8, inliers.size ());
+
+ Eigen::VectorXf coeff;
+ sac.getModelCoefficients (coeff);
+ EXPECT_EQ (6, coeff.size ());
+ EXPECT_NEAR (1, coeff[4] / coeff[3], 1e-4);
+ EXPECT_NEAR (1, coeff[5] / coeff[3], 1e-4);
+
+ Eigen::VectorXf coeff_refined;
+ model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
+ EXPECT_EQ (6, coeff_refined.size ());
+ EXPECT_NEAR (1, coeff[4] / coeff[3], 1e-4);
+ EXPECT_NEAR (1, coeff[5] / coeff[3], 1e-4);
+
+ // Projection tests
+ PointCloud<PointXYZ> proj_points;
+ model->projectPoints (inliers, coeff_refined, proj_points);
+
+ EXPECT_XYZ_NEAR (PointXYZ ( 7.0, 8.0, 9.0), proj_points.points[2], 1e-4);
+ EXPECT_XYZ_NEAR (PointXYZ (10.0, 11.0, 12.0), proj_points.points[3], 1e-4);
+ EXPECT_XYZ_NEAR (PointXYZ (16.0, 17.0, 18.0), proj_points.points[5], 1e-4);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST (SampleConsensusModelParallelLine, RANSAC)
+{
+ PointCloud<PointXYZ> cloud (16, 1);
+
+ // Line 1
+ cloud.points[0].getVector3fMap () << 1.0f, 2.00f, 3.00f;
+ cloud.points[1].getVector3fMap () << 4.0f, 5.00f, 6.00f;
+ cloud.points[2].getVector3fMap () << 7.0f, 8.00f, 9.00f;
+ cloud.points[3].getVector3fMap () << 10.0f, 11.00f, 12.00f;
+ cloud.points[4].getVector3fMap () << 13.0f, 14.00f, 15.00f;
+ cloud.points[5].getVector3fMap () << 16.0f, 17.00f, 18.00f;
+ cloud.points[6].getVector3fMap () << 19.0f, 20.00f, 21.00f;
+ cloud.points[7].getVector3fMap () << 22.0f, 23.00f, 24.00f;
+ // Random points
+ cloud.points[8].getVector3fMap () << -5.0f, 1.57f, 0.75f;
+ cloud.points[9].getVector3fMap () << 4.0f, 2.00f, 3.00f;
+ // Line 2 (parallel to the Z axis)
+ cloud.points[10].getVector3fMap () << -1.00f, 5.00f, 0.0f;
+ cloud.points[11].getVector3fMap () << -1.05f, 5.01f, 1.0f;
+ cloud.points[12].getVector3fMap () << -1.01f, 5.05f, 2.0f;
+ cloud.points[13].getVector3fMap () << -1.05f, 5.01f, 3.0f;
+ cloud.points[14].getVector3fMap () << -1.01f, 5.05f, 4.0f;
+ cloud.points[15].getVector3fMap () << -1.05f, 5.01f, 5.0f;
+
+ // Create a shared line model pointer directly
+ SampleConsensusModelParallelLinePtr model (new SampleConsensusModelParallelLine<PointXYZ> (cloud.makeShared ()));
+ model->setAxis (Eigen::Vector3f (0, 0, 1));
+ model->setEpsAngle (0.1);
+
+ // Create the RANSAC object
+ RandomSampleConsensus<PointXYZ> sac (model, 0.1);
+
+ // Algorithm tests
+ bool result = sac.computeModel ();
+ ASSERT_TRUE (result);
+
+ std::vector<int> sample;
+ sac.getModel (sample);
+ EXPECT_EQ (2, sample.size ());
+
+ std::vector<int> inliers;
+ sac.getInliers (inliers);
+ EXPECT_EQ (6, inliers.size ());
+
+ Eigen::VectorXf coeff;
+ sac.getModelCoefficients (coeff);
+ EXPECT_EQ (6, coeff.size ());
+ EXPECT_NEAR (0, coeff[3], 1e-4);
+ EXPECT_NEAR (0, coeff[4], 1e-4);
+ EXPECT_NEAR (1, coeff[5], 1e-4);
+
+ // Projection tests
+ PointCloud<PointXYZ> proj_points;
+ model->projectPoints (inliers, coeff, proj_points);
+
+ EXPECT_XYZ_NEAR (PointXYZ (-1.05, 5.05, 3.0), proj_points.points[13], 0.1);
+ EXPECT_XYZ_NEAR (PointXYZ (-1.05, 5.05, 4.0), proj_points.points[14], 0.1);
+}
+
+int
+main (int argc, char** argv)
+{
+ testing::InitGoogleTest (&argc, argv);
+ return (RUN_ALL_TESTS ());
+}
+
--- /dev/null
+# Random walker requires Eigen::Sparse module that is available since 3.1.0
+if(NOT ("${EIGEN_VERSION}" VERSION_LESS 3.1.0))
+ PCL_ADD_TEST(random_walker test_random_walker
+ FILES test_random_walker.cpp
+ LINK_WITH pcl_gtest
+ ARGUMENTS "${PCL_SOURCE_DIR}/test/segmentation/data")
+endif()
--- /dev/null
+; Graph 0
+; -------
+;
+; No seeds
+;
+; 0 - 1
+; | / |
+; 3 - 2
+
+Size 4
+
+Topology
+{
+ Edge "0 1 1.0"
+ Edge "0 3 1.0"
+ Edge "1 2 1.0"
+ Edge "1 3 1.0"
+ Edge "2 3 1.0"
+}
+
+Seeds
+{
+}
+
+Segmentation "0 0 0 0"
+
+Dimensions "4 0"
+
+Potentials
+{
+}
--- /dev/null
+; Graph 1
+; -------
+;
+; All edge weights are 1.0
+;
+; 1 - 0
+; | / |
+; [2]- 3 - 4 -[5]
+; | / |
+; 7 - 6
+
+Size 8
+
+Topology
+{
+ Edge "0 1 1.0"
+ Edge "0 2 1.0"
+ Edge "0 3 1.0"
+ Edge "1 2 1.0"
+ Edge "2 3 1.0"
+ Edge "3 4 1.0"
+ Edge "4 5 1.0"
+ Edge "4 7 1.0"
+ Edge "5 6 1.0"
+ Edge "5 7 1.0"
+ Edge "6 7 1.0"
+}
+
+Seeds
+{
+ Seed "2 2"
+ Seed "5 5"
+}
+
+Segmentation "2 2 2 2 5 5 5 5"
+
+Dimensions "6 2"
+
+Potentials
+{
+ 2 "0.88 0.94 1.00 0.72 0.27 0.00 0.05 0.11"
+ 5 "0.11 0.05 0.00 0.27 0.72 1.00 0.94 0.88"
+}
--- /dev/null
+; Graph 2
+; -------
+;
+; Exactly the same as Graph 1, but with 3 - 4 edge removed
+; All edge weights are 1.0
+;
+; 1 - 0
+; | / |
+; [2]- 3 4 -[5]
+; | / |
+; 7 - 6
+
+Size 8
+
+Topology
+{
+ Edge "0 1 1.0"
+ Edge "0 2 1.0"
+ Edge "0 3 1.0"
+ Edge "1 2 1.0"
+ Edge "2 3 1.0"
+ Edge "4 5 1.0"
+ Edge "4 7 1.0"
+ Edge "5 6 1.0"
+ Edge "5 7 1.0"
+ Edge "6 7 1.0"
+}
+
+Seeds
+{
+ Seed "2 2"
+ Seed "5 5"
+}
+
+Segmentation "2 2 2 2 5 5 5 5"
+
+Dimensions "6 2"
+
+Potentials
+{
+ 2 "1 1 1 1 0 0 0 0"
+ 5 "0 0 0 0 1 1 1 1"
+}
--- /dev/null
+; Graph 3
+; -------
+;
+; All edge weights are 1.0
+; One- and two- vertex components
+;
+; 0 [2] [4]
+; | |
+; [1] 3
+
+Size 5
+
+Topology
+{
+ Edge "0 1 1.0"
+ Edge "2 3 1.0"
+}
+
+Seeds
+{
+ Seed "1 1"
+ Seed "2 2"
+ Seed "4 4"
+}
+
+Segmentation "1 1 2 2 4"
+
+Dimensions "2 2"
+
+Potentials
+{
+ 1 "1 1 0 0 0"
+ 2 "0 0 1 1 0"
+ 4 "0 0 0 0 1"
+}
--- /dev/null
+; Graph 4
+; -------
+;
+; All edge weights are 1.0
+; Only one color
+;
+; 0 - 1
+; | |
+; 3 -[2]
+
+Size 4
+
+Topology
+{
+ Edge "0 1 1.0"
+ Edge "0 3 1.0"
+ Edge "1 2 1.0"
+ Edge "2 3 1.0"
+}
+
+Seeds
+{
+ Seed "2 2"
+}
+
+Segmentation "2 2 2 2"
+
+Dimensions "3 1"
+
+Potentials
+{
+ 2 "1 1 1 1"
+}
--- /dev/null
+; Graph 5
+; -------
+;
+; No edges
+;
+; [0] [1] [2] [3]
+
+Size 4
+
+Topology
+{
+}
+
+Seeds
+{
+ Seed "0 1"
+ Seed "1 2"
+ Seed "2 3"
+ Seed "3 4"
+}
+
+Segmentation "1 2 3 4"
+
+Dimensions "0 0"
+
+Potentials
+{
+ 1 "1 0 0 0"
+ 2 "0 1 0 0"
+ 3 "0 0 1 0"
+ 4 "0 0 0 1"
+}
--- /dev/null
+; Graph 6
+; -------
+;
+; All edge weights are 1.0
+; Each vertex has its own color
+;
+; [0]-[1]
+; | X |
+; [3]-[2]
+
+
+Size 4
+
+Topology
+{
+ Edge "0 1 1.0"
+ Edge "0 2 1.0"
+ Edge "0 3 1.0"
+ Edge "1 2 1.0"
+ Edge "1 3 1.0"
+ Edge "2 3 1.0"
+}
+
+Seeds
+{
+ Seed "0 1"
+ Seed "1 2"
+ Seed "2 3"
+ Seed "3 4"
+}
+
+Segmentation "1 2 3 4"
+
+Dimensions "0 0"
+
+Potentials
+{
+ 1 "1 0 0 0"
+ 2 "0 1 0 0"
+ 3 "0 0 1 0"
+ 4 "0 0 0 1"
+}
--- /dev/null
+; Graph 7
+; -------
+;
+; 2 1 - 6 0
+; | / | |
+; [3]-[4]- 7 - 8
+; | | X |
+; 11 - 5 10 -[9]
+
+Size 12
+
+Topology
+{
+ Edge "0 8 0.01"
+ Edge "1 3 1.0"
+ Edge "1 6 1.0"
+ Edge "2 3 0.5"
+ Edge "3 4 1.0"
+ Edge "4 5 0.01"
+ Edge "4 7 0.01"
+ Edge "5 11 0.01"
+ Edge "6 7 1.0"
+ Edge "7 8 0.1"
+ Edge "7 9 0.1"
+ Edge "7 10 0.1"
+ Edge "8 9 0.1"
+ Edge "8 10 0.1"
+ Edge "9 10 0.1"
+}
+
+Seeds
+{
+ Seed "3 3"
+ Seed "4 4"
+ Seed "9 9"
+}
+
+Segmentation "9 3 3 3 4 4 3 3 9 9 9 4"
+
+Dimensions "9 3"
+
+Potentials
+{
+ 3 "0.30 0.87 1.00 1.00 0.00 0.00 0.74 0.61 0.30 0.00 0.30 0.00"
+ 4 "0.00 0.00 0.00 0.00 1.00 1.00 0.01 0.01 0.00 0.00 0.00 1.00"
+ 9 "0.68 0.12 0.00 0.00 0.00 0.00 0.24 0.36 0.68 1.00 0.68 0.00"
+}
--- /dev/null
+; Graph 8
+; -------
+;
+; Reproduces DC circuit example in "Discrete Calculus" book (p. 107)
+;
+; 0 -[1]- 2 - 3
+; | | | |
+; 7 - 6 - 5 - 4
+; | | | |
+; [8]- 9 - 10- 11
+; | | | |
+; 15- 14- 13-[12]
+
+Size 16
+
+Topology
+{
+ Edge "0 1 1.0"
+ Edge "0 7 1.0"
+ Edge "1 2 1.0"
+ Edge "1 6 1.0"
+ Edge "2 3 1.0"
+ Edge "2 5 1.0"
+ Edge "3 4 1.0"
+ Edge "4 5 1.0"
+ Edge "4 11 1.0"
+ Edge "5 6 1.0"
+ Edge "5 10 1.0"
+ Edge "6 7 1.0"
+ Edge "6 9 1.0"
+ Edge "7 8 1.0"
+ Edge "8 9 1.0"
+ Edge "8 15 1.0"
+ Edge "9 10 1.0"
+ Edge "9 14 1.0"
+ Edge "10 11 1.0"
+ Edge "10 13 1.0"
+ Edge "11 12 1.0"
+ Edge "12 13 1.0"
+ Edge "13 14 1.0"
+ Edge "14 15 1.0"
+}
+
+Seeds
+{
+ Seed "1 2"
+ Seed "8 1"
+ Seed "12 2"
+}
+
+Segmentation "2 2 2 2 2 2 2 1 1 1 2 2 2 2 1 1"
+
+Dimensions "13 2"
+
+Potentials
+{
+ 1 "0.26 0.00 0.13 0.15 0.18 0.23 0.32 0.53 1.00 0.53 0.30 0.16 0.00 0.27 0.52 0.76"
+ 2 "0.73 1.00 0.86 0.84 0.81 0.76 0.67 0.46 0.00 0.46 0.69 0.83 1.00 0.72 0.47 0.23"
+}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2012-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <iostream>
+#include <fstream>
+#include <sstream>
+
+#include <boost/format.hpp>
+#include <boost/lexical_cast.hpp>
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/info_parser.hpp>
+#include <boost/foreach.hpp>
+
+#include <gtest/gtest.h>
+
+#include <pcl/segmentation/random_walker.h>
+
+std::string TEST_DATA_DIR;
+
+typedef float Weight;
+typedef uint32_t Color;
+typedef boost::adjacency_list
+ <boost::vecS,
+ boost::vecS,
+ boost::undirectedS,
+ boost::property<boost::vertex_color_t, Color,
+ boost::property<boost::vertex_degree_t, Weight> >,
+ boost::property<boost::edge_weight_t, Weight> > Graph;
+typedef boost::graph_traits<Graph> GraphTraits;
+typedef GraphTraits::edge_descriptor EdgeDescriptor;
+typedef GraphTraits::vertex_descriptor VertexDescriptor;
+typedef GraphTraits::edge_iterator EdgeIterator;
+typedef GraphTraits::vertex_iterator VertexIterator;
+typedef boost::property_map<Graph, boost::edge_weight_t>::type EdgeWeightMap;
+typedef boost::property_map<Graph, boost::vertex_color_t>::type VertexColorMap;
+typedef Eigen::SparseMatrix<Weight> SparseMatrix;
+typedef Eigen::Matrix<Weight, Eigen::Dynamic, Eigen::Dynamic> Matrix;
+typedef Eigen::Matrix<Weight, Eigen::Dynamic, 1> Vector;
+typedef boost::bimap<size_t, VertexDescriptor> VertexDescriptorBimap;
+typedef boost::bimap<size_t, Color> ColorBimap;
+typedef pcl::segmentation::detail::RandomWalker<Graph, EdgeWeightMap, VertexColorMap> RandomWalker;
+
+
+struct GraphInfo
+{
+
+ GraphInfo (const std::string& filename)
+ {
+ using boost::property_tree::ptree;
+ ptree pt;
+ read_info (filename, pt);
+
+ size = pt.get<size_t> ("Size");
+ graph = Graph (size);
+ segmentation.resize (size);
+ color_map = boost::get (boost::vertex_color, graph);
+
+ // Read graph topology
+ BOOST_FOREACH (ptree::value_type& v, pt.get_child ("Topology"))
+ {
+ uint32_t source, target;
+ float weight;
+ std::stringstream (v.second.data ()) >> source >> target >> weight;
+ boost::add_edge (source, target, weight, graph);
+ }
+
+ // Initialize color property map with zeros
+ VertexIterator vi, v_end;
+ for (boost::tie (vi, v_end) = boost::vertices (graph); vi != v_end; ++vi)
+ color_map[*vi] = 0;
+
+ // Read seeds
+ BOOST_FOREACH (ptree::value_type& v, pt.get_child ("Seeds"))
+ {
+ uint32_t id, color;
+ std::stringstream (v.second.data ()) >> id >> color;
+ color_map[id] = color;
+ colors.insert (color);
+ }
+
+ // Read expected cluster assignment
+ std::stringstream ss (pt.get<std::string> ("Segmentation"));
+ for (size_t i = 0; i < size; ++i)
+ ss >> segmentation[i];
+
+ // Read expected dimensions of matrices L and B
+ std::stringstream (pt.get<std::string> ("Dimensions")) >> rows >> cols;
+
+ // Read expected potentials
+ BOOST_FOREACH (ptree::value_type& v, pt.get_child ("Potentials"))
+ {
+ Color color = boost::lexical_cast<uint32_t> (v.first);
+ potentials[color] = Vector::Zero (size);
+ std::stringstream ss (v.second.data ());
+ for (size_t i = 0; i < size; ++i)
+ ss >> potentials[color] (i);
+ }
+ }
+
+ Graph graph;
+ std::vector<Color> segmentation;
+ VertexColorMap color_map;
+ std::map<Color, Vector> potentials;
+ std::set<Color> colors;
+ size_t size; // number of vertices
+ size_t rows; // expected number of rows in matrices L and B
+ size_t cols; // expected number of cols in matrix B
+
+};
+
+class RandomWalkerTest : public ::testing::TestWithParam<const char*>
+{
+
+ public:
+
+ RandomWalkerTest ()
+ : g (TEST_DATA_DIR + "/" + GetParam ())
+ {
+ }
+
+ GraphInfo g;
+
+};
+
+TEST_P (RandomWalkerTest, BuildLinearSystem)
+{
+ RandomWalker rw (g.graph,
+ boost::get (boost::edge_weight, g.graph),
+ boost::get (boost::vertex_color, g.graph));
+
+ rw.computeVertexDegrees ();
+ rw.buildLinearSystem ();
+
+ ASSERT_EQ (g.rows, rw.L.rows ());
+ ASSERT_EQ (g.rows, rw.L.cols ());
+ ASSERT_EQ (g.rows, rw.B.rows ());
+ ASSERT_EQ (g.cols, rw.B.cols ());
+
+ std::vector<Weight> degrees (g.rows, 0.0);
+ std::vector<Weight> L_sums (g.rows, 0.0);
+ std::vector<Weight> B_sums (g.rows, 0.0);
+ for (int k = 0; k < rw.L.outerSize (); ++k)
+ {
+ for (SparseMatrix::InnerIterator it (rw.L, k); it; ++it)
+ {
+ EXPECT_GE (it.row (), it.col ()); // the matrix should be lower triangular
+ if (it.row () == it.col ())
+ {
+ degrees[it.row ()] = it.value ();
+ }
+ else
+ {
+ L_sums[it.row ()] -= it.value ();
+ L_sums[it.col ()] -= it.value ();
+ }
+ }
+ }
+ for (int k = 0; k < rw.B.outerSize (); ++k)
+ {
+ for (SparseMatrix::InnerIterator it (rw.B, k); it; ++it)
+ {
+ B_sums[it.row ()] += it.value ();
+ }
+ }
+ for (size_t i = 0; i < g.rows; ++i)
+ {
+ float sum = L_sums[i] + B_sums[i];
+ EXPECT_FLOAT_EQ (degrees[i], sum);
+ }
+}
+
+TEST_P (RandomWalkerTest, Segment)
+{
+ bool result = pcl::segmentation::randomWalker (g.graph);
+ ASSERT_TRUE (result);
+ VertexIterator vi, v_end;
+ for (boost::tie (vi, v_end) = boost::vertices (g.graph); vi != v_end; ++vi)
+ EXPECT_EQ (g.segmentation[*vi], g.color_map[*vi]);
+}
+
+TEST_P (RandomWalkerTest, GetPotentials)
+{
+ Matrix p;
+ std::map<Color, size_t> map;
+
+ pcl::segmentation::randomWalker (g.graph,
+ boost::get (boost::edge_weight, g.graph),
+ boost::get (boost::vertex_color, g.graph),
+ p,
+ map);
+
+ ASSERT_EQ (g.size, p.rows ());
+ ASSERT_EQ (g.colors.size (), p.cols ());
+ ASSERT_EQ (g.colors.size (), map.size ());
+
+ for (std::set<Color>::iterator it = g.colors.begin (); it != g.colors.end (); ++it)
+ for (size_t i = 0; i < g.size; ++i)
+ if (g.potentials.count (*it))
+ EXPECT_NEAR (g.potentials[*it] (i), p (i, map[*it]), 0.01);
+}
+
+INSTANTIATE_TEST_CASE_P (VariousGraphs,
+ RandomWalkerTest,
+ ::testing::Values ("graph0.info",
+ "graph1.info",
+ "graph2.info",
+ "graph3.info",
+ "graph4.info",
+ "graph5.info",
+ "graph6.info",
+ "graph7.info",
+ "graph8.info"));
+
+int
+main (int argc, char** argv)
+{
+ if (argc < 2)
+ {
+ std::cerr << "Please provide a path to the directory with test graph descriptions." << std::endl;
+ return (-1);
+ }
+
+ TEST_DATA_DIR = std::string (argv[1]);
+
+ try
+ {
+ ::testing::InitGoogleTest (&argc, argv);
+ return RUN_ALL_TESTS ();
+ }
+ catch (std::exception& e)
+ {
+ std::cerr << "Unhandled exception: " << e.what () << "\n";
+ }
+
+ return 1;
+}
+
+++ /dev/null
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2013, Intelligent Robotics Lab, DLUT.
- * Author: Qinghua Li, Yan Zhuang, Fei Yan
- *
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Intelligent Robotics Lab, DLUT. nor the names
- * of its contributors may be used to endorse or promote products
- * derived from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/**
- * \file test_bearing_angle_image.cpp
- * \created on: July 07, 2013
- * \author: Qinghua Li (qinghua__li@163.com)
- */
-
-#include <gtest/gtest.h>
-#include <iostream>
-#include <pcl/range_image/bearing_angle_image.h>
-
-pcl::BearingAngleImage bearing_angle_image;
-pcl::PointCloud<pcl::PointXYZ> point_cloud (3, 2);
-
-/////////////////////////////////////////////////////////////////////
-TEST (BearingAngleImageTest, GetAngle)
-{
- pcl::PointXYZ point1 (1.0, 2.0, 3.0);
- pcl::PointXYZ point2 (2.0, 1.0, 1.0);
-
- double angle = bearing_angle_image.getAngle (point1, point2);
- EXPECT_NEAR (40.203, angle, 1e-3);
-}
-
-/////////////////////////////////////////////////////////////////////
-TEST (BearingAngleImageTest, GenerateBAImage)
-{
- point_cloud.points[0] = pcl::PointXYZ (3.0, 1.5, -2.0);
- point_cloud.points[1] = pcl::PointXYZ (1.0, 3.0, 2.0);
- point_cloud.points[2] = pcl::PointXYZ (2.0, 3.0, 2.0);
-
- point_cloud.points[3] = pcl::PointXYZ (2.0, 3.0, 1.0);
- point_cloud.points[4] = pcl::PointXYZ (4.0, 2.0, 2.0);
- point_cloud.points[5] = pcl::PointXYZ (-1.5, 3.0, 1.0);
-
- bearing_angle_image.generateBAImage (point_cloud);
-
- uint8_t grays[6];
- for (int i = 0; i < 3 * 2; ++i)
- {
- grays[i] = (bearing_angle_image.points[i].rgba >> 8) & 0xff;
- }
-
- EXPECT_EQ (0, grays[0]);
- EXPECT_EQ (0, grays[1]);
- EXPECT_EQ (0, grays[2]);
- EXPECT_EQ (112, grays[3]);
- EXPECT_EQ (80, grays[4]);
- EXPECT_EQ (0, grays[5]);
-}
-
-
-/* ---[ */
-int
-main (int argc, char** argv)
-{
- testing::InitGoogleTest (&argc, argv);
- return (RUN_ALL_TESTS ());
-}
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/shot_omp.h>
#include <pcl/features/board.h>
-#include <pcl/keypoints/uniform_sampling.h>
+#include <pcl/filters/uniform_sampling.h>
#include <pcl/recognition/cg/hough_3d.h>
#include <pcl/recognition/cg/geometric_consistency.h>
#include <pcl/kdtree/kdtree_flann.h>
//Assertions
EXPECT_EQ (rototranslations.size (), 1);
- EXPECT_LT (computeRmsE (model_, scene_, rototranslations[0]), 1E-4);
+ EXPECT_LT (computeRmsE (model_, scene_, rototranslations[0]), 1E-2);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
norm_est.compute (*scene_normals_);
//Downsampling
- PointCloud<int> sampled_indices;
UniformSampling<PointType> uniform_sampling;
uniform_sampling.setInputCloud (model_);
uniform_sampling.setRadiusSearch (0.005);
- uniform_sampling.compute (sampled_indices);
- copyPointCloud (*model_, sampled_indices.points, *model_downsampled_);
+ uniform_sampling.filter (*model_downsampled_);
uniform_sampling.setInputCloud (scene_);
uniform_sampling.setRadiusSearch (0.02);
- uniform_sampling.compute (sampled_indices);
- copyPointCloud (*scene_, sampled_indices.points, *scene_downsampled_);
+ uniform_sampling.filter (*scene_downsampled_);
//Descriptor
SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est;
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
+#include <pcl/pcl_tests.h>
+
using namespace pcl;
using namespace pcl::io;
using namespace std;
EXPECT_EQ (1, points2[3].z);
}
+//////////////////////////////////////////////////////////////////////////////////////////////
+TEST (PCL, TransformCopyFields)
+{
+ Eigen::Affine3f transform;
+ transform = Eigen::Translation3f (100, 0, 0);
+
+ PointXYZRGBNormal empty_point;
+ std::vector<int> indices (1);
+
+ PointCloud<PointXYZRGBNormal> cloud (2, 1);
+ cloud.points[0].rgba = 0xFF0000;
+ cloud.points[1].rgba = 0x00FF00;
+
+ // Preserve data in all fields
+ {
+ PointCloud<PointXYZRGBNormal> cloud_out;
+ transformPointCloud (cloud, cloud_out, transform, true);
+ ASSERT_EQ (cloud.size (), cloud_out.size ());
+ EXPECT_RGBA_EQ (cloud.points[0], cloud_out.points[0]);
+ EXPECT_RGBA_EQ (cloud.points[1], cloud_out.points[1]);
+ }
+ // Preserve data in all fields (with indices)
+ {
+ PointCloud<PointXYZRGBNormal> cloud_out;
+ transformPointCloud (cloud, indices, cloud_out, transform, true);
+ ASSERT_EQ (indices.size (), cloud_out.size ());
+ EXPECT_RGBA_EQ (cloud.points[0], cloud_out.points[0]);
+ }
+ // Do not preserve data in all fields
+ {
+ PointCloud<PointXYZRGBNormal> cloud_out;
+ transformPointCloud (cloud, cloud_out, transform, false);
+ ASSERT_EQ (cloud.size (), cloud_out.size ());
+ EXPECT_RGBA_EQ (empty_point, cloud_out.points[0]);
+ EXPECT_RGBA_EQ (empty_point, cloud_out.points[1]);
+ }
+ // Do not preserve data in all fields (with indices)
+ {
+ PointCloud<PointXYZRGBNormal> cloud_out;
+ transformPointCloud (cloud, indices, cloud_out, transform, false);
+ ASSERT_EQ (indices.size (), cloud_out.size ());
+ EXPECT_RGBA_EQ (empty_point, cloud_out.points[0]);
+ }
+ // Preserve data in all fields (with normals version)
+ {
+ PointCloud<PointXYZRGBNormal> cloud_out;
+ transformPointCloudWithNormals (cloud, cloud_out, transform, true);
+ ASSERT_EQ (cloud.size (), cloud_out.size ());
+ EXPECT_RGBA_EQ (cloud.points[0], cloud_out.points[0]);
+ EXPECT_RGBA_EQ (cloud.points[1], cloud_out.points[1]);
+ }
+ // Preserve data in all fields (with normals and indices version)
+ {
+ PointCloud<PointXYZRGBNormal> cloud_out;
+ transformPointCloudWithNormals (cloud, indices, cloud_out, transform, true);
+ ASSERT_EQ (indices.size (), cloud_out.size ());
+ EXPECT_RGBA_EQ (cloud.points[0], cloud_out.points[0]);
+ }
+ // Do not preserve data in all fields (with normals version)
+ {
+ PointCloud<PointXYZRGBNormal> cloud_out;
+ transformPointCloudWithNormals (cloud, cloud_out, transform, false);
+ ASSERT_EQ (cloud.size (), cloud_out.size ());
+ EXPECT_RGBA_EQ (empty_point, cloud_out.points[0]);
+ EXPECT_RGBA_EQ (empty_point, cloud_out.points[1]);
+ }
+ // Do not preserve data in all fields (with normals and indices version)
+ {
+ PointCloud<PointXYZRGBNormal> cloud_out;
+ transformPointCloudWithNormals (cloud, indices, cloud_out, transform, false);
+ ASSERT_EQ (indices.size (), cloud_out.size ());
+ EXPECT_RGBA_EQ (empty_point, cloud_out.points[0]);
+ }
+}
+
//////////////////////////////////////////////////////////////////////////////////////////////
TEST (PCL, Matrix4Affine3Transform)
{
EXPECT_NEAR (pt.y, v3t.y (), 1e-4); EXPECT_NEAR (pt.y, v4t.y (), 1e-4);
EXPECT_NEAR (pt.z, v3t.z (), 1e-4); EXPECT_NEAR (pt.z, v4t.z (), 1e-4);
+ PointNormal pn;
+ pn.getVector3fMap () = p.getVector3fMap ();
+ pn.getNormalVector3fMap () = Eigen::Vector3f (0.60f, 0.48f, 0.64f);
+ Eigen::Vector3f n3 = pn.getNormalVector3fMap ();
+ Eigen::Vector4f n4 = pn.getNormalVector4fMap ();
+
+ Eigen::Vector3f n3t (affine.rotation() * n3);
+ Eigen::Vector4f n4t (transformation * n4);
+
+ PointNormal pnt = pcl::transformPointWithNormal (pn, affine);
+
+ EXPECT_NEAR (pnt.x, v3t.x (), 1e-4); EXPECT_NEAR (pnt.x, v4t.x (), 1e-4);
+ EXPECT_NEAR (pnt.y, v3t.y (), 1e-4); EXPECT_NEAR (pnt.y, v4t.y (), 1e-4);
+ EXPECT_NEAR (pnt.z, v3t.z (), 1e-4); EXPECT_NEAR (pnt.z, v4t.z (), 1e-4);
+ EXPECT_NEAR (pnt.normal_x, n3t.x (), 1e-4); EXPECT_NEAR (pnt.normal_x, n4t.x (), 1e-4);
+ EXPECT_NEAR (pnt.normal_y, n3t.y (), 1e-4); EXPECT_NEAR (pnt.normal_y, n4t.y (), 1e-4);
+ EXPECT_NEAR (pnt.normal_z, n3t.z (), 1e-4); EXPECT_NEAR (pnt.normal_z, n4t.z (), 1e-4);
+
PointCloud<PointXYZ> c, ct;
c.push_back (p);
pcl::transformPointCloud (c, ct, affine);
- EXPECT_FLOAT_EQ (pt.x, ct[0].x);
- EXPECT_FLOAT_EQ (pt.y, ct[0].y);
- EXPECT_FLOAT_EQ (pt.z, ct[0].z);
+ EXPECT_NEAR (pt.x, ct[0].x, 1e-4);
+ EXPECT_NEAR (pt.y, ct[0].y, 1e-4);
+ EXPECT_NEAR (pt.z, ct[0].z, 1e-4);
pcl::transformPointCloud (c, ct, transformation);
- EXPECT_FLOAT_EQ (pt.x, ct[0].x);
- EXPECT_FLOAT_EQ (pt.y, ct[0].y);
- EXPECT_FLOAT_EQ (pt.z, ct[0].z);
+ EXPECT_NEAR (pt.x, ct[0].x, 1e-4);
+ EXPECT_NEAR (pt.y, ct[0].y, 1e-4);
+ EXPECT_NEAR (pt.z, ct[0].z, 1e-4);
affine = transformation;
std::vector<int> indices (1); indices[0] = 0;
pcl::transformPointCloud (c, indices, ct, affine);
- EXPECT_FLOAT_EQ (pt.x, ct[0].x);
- EXPECT_FLOAT_EQ (pt.y, ct[0].y);
- EXPECT_FLOAT_EQ (pt.z, ct[0].z);
+ EXPECT_NEAR (pt.x, ct[0].x, 1e-4);
+ EXPECT_NEAR (pt.y, ct[0].y, 1e-4);
+ EXPECT_NEAR (pt.z, ct[0].z, 1e-4);
pcl::transformPointCloud (c, indices, ct, transformation);
- EXPECT_FLOAT_EQ (pt.x, ct[0].x);
- EXPECT_FLOAT_EQ (pt.y, ct[0].y);
- EXPECT_FLOAT_EQ (pt.z, ct[0].z);
+ EXPECT_NEAR (pt.x, ct[0].x, 1e-4);
+ EXPECT_NEAR (pt.y, ct[0].y, 1e-4);
+ EXPECT_NEAR (pt.z, ct[0].z, 1e-4);
}
//////////////////////////////////////////////////////////////////////////////////////////////
set (SUBSYS_NAME tools)
set (SUBSYS_DESC "Useful PCL-based command line tools")
-set (SUBSYS_DEPS common io filters sample_consensus segmentation search kdtree features surface octree registration recognition geometry keypoints)
+set (SUBSYS_DEPS common io filters sample_consensus segmentation search kdtree features surface octree registration recognition geometry keypoints ml)
set (DEFAULT ON)
set (REASON "")
PCL_ADD_EXECUTABLE (pcl_compute_cloud_error "${SUBSYS_NAME}" compute_cloud_error.cpp)
target_link_libraries (pcl_compute_cloud_error pcl_common pcl_io pcl_kdtree pcl_search)
+ PCL_ADD_EXECUTABLE (pcl_train_unary_classifier "${SUBSYS_NAME}" train_unary_classifier.cpp)
+ target_link_libraries (pcl_train_unary_classifier pcl_common pcl_io pcl_segmentation)
+
+ PCL_ADD_EXECUTABLE (pcl_unary_classifier_segment "${SUBSYS_NAME}" unary_classifier_segment.cpp)
+ target_link_libraries (pcl_unary_classifier_segment pcl_common pcl_io pcl_segmentation)
+
+ PCL_ADD_EXECUTABLE (pcl_crf_segmentation "${SUBSYS_NAME}" crf_segmentation.cpp)
+ target_link_libraries (pcl_crf_segmentation pcl_common pcl_io pcl_segmentation)
+
# NOTE: boost/uuid/uuid.hpp only exists for versions > 1.41
if(Boost_MAJOR_VERSION GREATER 1 OR Boost_MINOR_VERSION GREATER 41)
PCL_ADD_EXECUTABLE (pcl_add_gaussian_noise "${SUBSYS_NAME}" add_gaussian_noise.cpp)
PCL_ADD_EXECUTABLE(pcl_grid_min "${SUBSYS_NAME}" grid_min.cpp)
target_link_libraries(pcl_grid_min pcl_common pcl_io pcl_filters)
- if(BUILD_OPENNI AND OPENNI_FOUND)
+ if(WITH_OPENNI)
PCL_ADD_EXECUTABLE(pcl_oni2pcd "${SUBSYS_NAME}" oni2pcd.cpp)
target_link_libraries(pcl_oni2pcd pcl_common pcl_io)
- endif(BUILD_OPENNI AND OPENNI_FOUND)
+ endif()
if (QHULL_FOUND)
PCL_ADD_EXECUTABLE(pcl_crop_to_hull "${SUBSYS_NAME}" crop_to_hull.cpp)
PCL_ADD_EXECUTABLE(pcl_obj2pcd "${SUBSYS_NAME}" obj2pcd.cpp)
target_link_libraries(pcl_obj2pcd pcl_common pcl_io)
+ PCL_ADD_EXECUTABLE(pcl_obj2ply "${SUBSYS_NAME}" obj2ply.cpp)
+ target_link_libraries(pcl_obj2ply pcl_common pcl_io)
+
PCL_ADD_EXECUTABLE(pcl_vtk2pcd "${SUBSYS_NAME}" vtk2pcd.cpp)
target_link_libraries(pcl_vtk2pcd pcl_common pcl_io)
PCL_ADD_EXECUTABLE (pcl_voxel_grid_occlusion_estimation "${SUBSYS_NAME}" voxel_grid_occlusion_estimation.cpp)
target_link_libraries (pcl_voxel_grid_occlusion_estimation pcl_common pcl_io pcl_filters pcl_visualization)
- if(BUILD_OPENNI AND OPENNI_FOUND)
+ if(WITH_OPENNI)
PCL_ADD_EXECUTABLE(pcl_openni_save_image "${SUBSYS_NAME}" openni_save_image.cpp)
target_link_libraries(pcl_openni_save_image pcl_common pcl_io pcl_visualization)
- endif(BUILD_OPENNI AND OPENNI_FOUND)
+ endif()
endif(BUILD_visualization)
endif(NOT VTK_FOUND)
#pragma GCC system_header
#endif
+#ifndef Q_MOC_RUN
// Marking all Boost headers as system headers to remove warnings
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/thread/thread.hpp>
//#include <boost/uuid/uuid.hpp>
//#include <boost/uuid/uuid_generators.hpp>
+#endif
#endif // PCL_TOOLS_BOOST_H_
vtk_file_indices = parse_file_extension_argument (argc, argv, ".vtk");
if (vtk_file_indices.size () != 1)
{
- print_error ("Need one ouput VTK file to continue.\n");
+ print_error ("Need one output VTK file to continue.\n");
return (-1);
}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011-2012, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author : Christian Potthast
+ * Email : potthast@usc.edu
+ *
+ */
+
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/console/print.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/time.h>
+
+#include <pcl/segmentation/crf_segmentation.h>
+#include <pcl/features/normal_3d.h>
+
+using namespace pcl;
+using namespace pcl::io;
+using namespace pcl::console;
+
+float default_leaf_size = 0.005f;
+double default_feature_threshold = 5.0;
+double default_normal_radius_search = 0.03;
+
+typedef PointXYZRGBA PointT;
+typedef PointCloud<PointT> CloudT;
+typedef PointCloud<PointXYZRGBL> CloudLT;
+
+void
+printHelp (int, char **argv)
+{
+ print_error ("Syntax is: %s input.pcd output.pcd <options>\n", argv[0]);
+ print_info (" where options are:\n");
+ print_info (" -leaf x,y,z = the VoxelGrid leaf size (default: ");
+ print_value ("%f, %f, %f", default_leaf_size, default_leaf_size, default_leaf_size); print_info (")\n");
+ print_info (" -normal-search X = Normal radius search (default: ");
+ print_value ("%f", default_normal_radius_search); print_info (")\n");
+}
+
+bool
+loadCloud (const std::string &filename, CloudT::Ptr &cloud)
+{
+ TicToc tt;
+ print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
+
+ tt.tic ();
+ if (loadPCDFile (filename, *cloud) < 0)
+ return (false);
+ print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud->width * cloud->height); print_info (" points]\n");
+
+ return (true);
+}
+
+bool
+loadCloud (const std::string &filename, CloudLT::Ptr &cloud)
+{
+ TicToc tt;
+ print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
+
+ tt.tic ();
+ if (loadPCDFile (filename, *cloud) < 0)
+ return (false);
+ print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud->width * cloud->height); print_info (" points]\n");
+
+ return (true);
+}
+
+void
+compute (const CloudT::Ptr &cloud,
+ const CloudLT::Ptr &anno,
+ float normal_radius_search,
+ float leaf_x, float leaf_y, float leaf_z,
+ CloudLT::Ptr &out)
+{
+ TicToc tt;
+ tt.tic ();
+
+ print_highlight ("Computing ");
+
+ pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointNormal>);
+ cloud_normals->width = cloud->width;
+ cloud_normals->height = cloud->height;
+ cloud_normals->points.resize (cloud->points.size ());
+ for (size_t i = 0; i < cloud->points.size (); i++)
+ {
+ cloud_normals->points[i].x = cloud->points[i].x;
+ cloud_normals->points[i].y = cloud->points[i].y;
+ cloud_normals->points[i].z = cloud->points[i].z;
+ }
+
+ // estimate surface normals
+ pcl::NormalEstimation<PointT, pcl::PointNormal> ne;
+ pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
+ ne.setSearchMethod (tree);
+ ne.setInputCloud (cloud);
+ ne.setRadiusSearch (normal_radius_search);
+ ne.compute (*cloud_normals);
+
+ pcl::CrfSegmentation<PointT> crf;
+ crf.setInputCloud (cloud);
+ crf.setNormalCloud (cloud_normals);
+ crf.setAnnotatedCloud (anno);
+ crf.setVoxelGridLeafSize (leaf_x, leaf_y, leaf_z);
+ crf.setSmoothnessKernelParameters (3, 3, 3, 1.0);
+ crf.setAppearanceKernelParameters (30, 30, 30, 20, 20, 20, 3.5);
+ crf.setSurfaceKernelParameters (20, 20, 20, 0.3f, 0.3f, 0.3f, 8.5);
+ //crf.setSurfaceKernelParameters (20, 20, 20, 0.3, 0.3, 0.3, 0.0);
+ crf.setNumberOfIterations (10);
+ crf.segmentPoints (*out);
+
+ print_info ("[done, ");
+ print_value ("%g", tt.toc ());
+ print_info (" ms : "); print_value ("%d", out->width * out->height);
+ print_info (" points]\n");
+}
+
+void
+saveCloud (const std::string &filename, CloudLT::Ptr &output)
+{
+ TicToc tt;
+ tt.tic ();
+
+ print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
+
+ PCDWriter w;
+ w.write (filename, *output);
+
+ print_info ("[done, ");
+ print_value ("%g", tt.toc ()); print_info (" ms : ");
+ print_value ("%d", output->width * output->height); print_info (" points]\n");
+}
+
+/* ---[ */
+int
+main (int argc, char** argv)
+{
+ print_info ("Train unary classifier using FPFH. For more information, use: %s -h\n", argv[0]);
+
+ if (argc < 4)
+ {
+ printHelp (argc, argv);
+ return (-1);
+ }
+
+ // Parse the command line arguments for .pcd files
+ std::vector<int> p_file_indices;
+ p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
+ if (p_file_indices.size () != 3)
+ {
+ print_error ("Need one input PCD file, pre-segmented PCD file and one output PCD file to continue.\n");
+ return (-1);
+ }
+
+ // Load the input file
+ CloudT::Ptr cloud (new CloudT);
+ if (!loadCloud (argv[p_file_indices[0]], cloud))
+ return (-1);
+
+ // Load the input file
+ CloudLT::Ptr cloud_anno (new CloudLT);
+ if (!loadCloud (argv[p_file_indices[1]], cloud_anno))
+ return (-1);
+
+
+ // TODO:: make this as an optional argument ??
+ std::vector<int> tmp_indices;
+ pcl::removeNaNFromPointCloud (*cloud, *cloud, tmp_indices);
+
+ // parse optional input arguments from the command line
+ float normal_radius_search = static_cast<float> (default_normal_radius_search);
+
+ // Command line parsing
+ float leaf_x = default_leaf_size,
+ leaf_y = default_leaf_size,
+ leaf_z = default_leaf_size;
+
+ std::vector<double> values;
+ parse_x_arguments (argc, argv, "-leaf", values);
+ if (values.size () == 1)
+ {
+ leaf_x = static_cast<float> (values[0]);
+ leaf_y = static_cast<float> (values[0]);
+ leaf_z = static_cast<float> (values[0]);
+ }
+ else if (values.size () == 3)
+ {
+ leaf_x = static_cast<float> (values[0]);
+ leaf_y = static_cast<float> (values[1]);
+ leaf_z = static_cast<float> (values[2]);
+ }
+ else
+ {
+ print_error ("Leaf size must be specified with either 1 or 3 numbers (%lu given). ", values.size ());
+ }
+ print_info ("Using a leaf size of: "); print_value ("%f, %f, %f\n", leaf_x, leaf_y, leaf_z);
+
+ parse_argument (argc, argv, "-normal-radius-search", normal_radius_search);
+
+ // segment the pre-segmented cloud
+ CloudLT::Ptr out (new CloudLT);
+ compute (cloud, cloud_anno, normal_radius_search, leaf_x, leaf_y, leaf_z, out);
+
+ // save cloud
+ saveCloud (argv[p_file_indices[2]], out);
+}
+
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
+#include <pcl/registration/incremental_registration.h>
#include <string>
#include <iostream>
std::vector<int> pcd_indices;
pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
- CloudPtr model (new Cloud);
- if (pcl::io::loadPCDFile (argv[pcd_indices[0]], *model) == -1)
+ pcl::IterativeClosestPoint<PointType, PointType>::Ptr icp;
+ if (nonLinear)
{
- std::cout << "Could not read file" << std::endl;
- return -1;
+ std::cout << "Using IterativeClosestPointNonLinear" << std::endl;
+ icp.reset (new pcl::IterativeClosestPointNonLinear<PointType, PointType> ());
}
- std::cout << argv[pcd_indices[0]] << " width: " << model->width << " height: " << model->height << std::endl;
-
- std::string result_filename (argv[pcd_indices[0]]);
- result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
- pcl::io::savePCDFile (result_filename.c_str (), *model);
- std::cout << "saving first model to " << result_filename << std::endl;
+ else
+ {
+ std::cout << "Using IterativeClosestPoint" << std::endl;
+ icp.reset (new pcl::IterativeClosestPoint<PointType, PointType> ());
+ }
+ icp->setMaximumIterations (iter);
+ icp->setMaxCorrespondenceDistance (dist);
+ icp->setRANSACOutlierRejectionThreshold (rans);
- Eigen::Matrix4f t (Eigen::Matrix4f::Identity ());
+ pcl::registration::IncrementalRegistration<PointType> iicp;
+ iicp.setRegistration (icp);
- for (size_t i = 1; i < pcd_indices.size (); i++)
+ for (size_t i = 0; i < pcd_indices.size (); i++)
{
CloudPtr data (new Cloud);
if (pcl::io::loadPCDFile (argv[pcd_indices[i]], *data) == -1)
std::cout << "Could not read file" << std::endl;
return -1;
}
- std::cout << argv[pcd_indices[i]] << " width: " << data->width << " height: " << data->height << std::endl;
-
- pcl::IterativeClosestPoint<PointType, PointType> *icp;
- if (nonLinear)
+ if (!iicp.registerCloud (data))
{
- std::cout << "Using IterativeClosestPointNonLinear" << std::endl;
- icp = new pcl::IterativeClosestPointNonLinear<PointType, PointType>();
- }
- else
- {
- std::cout << "Using IterativeClosestPoint" << std::endl;
- icp = new pcl::IterativeClosestPoint<PointType, PointType>();
- }
-
- icp->setMaximumIterations (iter);
- icp->setMaxCorrespondenceDistance (dist);
- icp->setRANSACOutlierRejectionThreshold (rans);
-
- icp->setInputTarget (model);
-
- icp->setInputSource (data);
+ std::cout << "Registration failed. Resetting transform" << std::endl;
+ iicp.reset ();
+ iicp.registerCloud (data);
+ };
CloudPtr tmp (new Cloud);
- icp->align (*tmp);
-
- t = t * icp->getFinalTransformation ();
-
- pcl::transformPointCloud (*data, *tmp, t);
-
- std::cout << icp->getFinalTransformation () << std::endl;
+ pcl::transformPointCloud (*data, *tmp, iicp.getAbsoluteTransform ());
- *model = *data;
+ std::cout << iicp.getAbsoluteTransform () << std::endl;
std::string result_filename (argv[pcd_indices[i]]);
result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
int lumIter = 1;
pcl::console::parse_argument (argc, argv, "-l", lumIter);
+ double loopDist = 5.0;
+ pcl::console::parse_argument (argc, argv, "-D", loopDist);
+
+ int loopCount = 20;
+ pcl::console::parse_argument (argc, argv, "-c", loopCount);
+
pcl::registration::LUM<PointType> lum;
lum.setMaxIterations (lumIter);
lum.setConvergenceThreshold (0.001f);
//std::cout << i << " " << j << " " << diff.norm () << std::endl;
- if(diff.norm () < 5.0 && (i - j == 1 || i - j > 20))
+ if(diff.norm () < loopDist && (i - j == 1 || i - j > loopCount))
{
- if(i - j > 20)
+ if(i - j > loopCount)
std::cout << "add connection between " << i << " (" << clouds[i].first << ") and " << j << " (" << clouds[j].first << ")" << std::endl;
pcl::registration::CorrespondenceEstimation<PointType, PointType> ce;
ce.setInputTarget (clouds[i].second);
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/pcd_io.h>
+#include <pcl/io/vtk_lib_io.h>
#include <pcl/common/transforms.h>
#include <vtkVersion.h>
#include <vtkPLYReader.h>
return (-1);
}
- vtkSmartPointer<vtkPolyData> polydata1;
+ vtkSmartPointer<vtkPolyData> polydata1 = vtkSmartPointer<vtkPolyData>::New ();;
if (ply_file_indices.size () == 1)
{
- vtkSmartPointer<vtkPLYReader> readerQuery = vtkSmartPointer<vtkPLYReader>::New ();
- readerQuery->SetFileName (argv[ply_file_indices[0]]);
+ pcl::PolygonMesh mesh;
+ pcl::io::loadPolygonFilePLY (argv[ply_file_indices[0]], mesh);
+ pcl::io::mesh2vtk (mesh, polydata1);
}
else if (obj_file_indices.size () == 1)
{
VoxelGrid<PointXYZ> grid_;
grid_.setInputCloud (cloud_1);
grid_.setLeafSize (leaf_size, leaf_size, leaf_size);
- grid_.filter (*cloud_1);
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr res(new pcl::PointCloud<pcl::PointXYZ>);
+ grid_.filter (*res);
if (VIS)
{
visualization::PCLVisualizer vis3 ("VOXELIZED SAMPLES CLOUD");
- vis3.addPointCloud (cloud_1);
+ vis3.addPointCloud (res);
vis3.spin ();
}
- savePCDFileASCII (argv[pcd_file_indices[0]], *cloud_1);
+ savePCDFileASCII (argv[pcd_file_indices[0]], *res);
}
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011-2012, Willow Garage, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <pcl/io/obj_io.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/console/print.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/time.h>
+
+void
+printHelp (int,
+ char **argv)
+{
+ PCL_ERROR ("Syntax is: %s [-format 0|1] [-use_camera 0|1] input.obj output.ply\n", argv[0]);
+ PCL_ERROR ("format = 0 writes a binary file (default behavior), format = 1 writes an ASCII file.\n");
+}
+
+int
+main (int argc,
+ char** argv)
+{
+ PCL_INFO ("Convert a OBJ file to PLY format. For more information, use: %s -h\n", argv[0]);
+
+ if (argc < 3)
+ {
+ printHelp (argc, argv);
+ return (-1);
+ }
+
+ // Parse the command line arguments for files
+ std::vector<int> obj_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".obj");
+ std::vector<int> ply_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".ply");
+ if (obj_file_indices.size () != 1 || ply_file_indices.size () != 1)
+ {
+ PCL_ERROR ("Need one input OBJ file and one output PLY file.\n");
+ return (-1);
+ }
+
+ // Command line parsing
+ bool ascii = false;
+ pcl::console::parse_argument (argc, argv, "-format", ascii);
+ PCL_INFO ("PLY output format: %s\n", ascii ? "ascii" : "binary");
+
+ // Load the first file
+ pcl::PolygonMesh mesh;
+ if (pcl::io::loadOBJFile (argv[obj_file_indices[0]], mesh) != 0)
+ {
+ PCL_ERROR ("Error loading OBJ file\n");
+ return (-1);
+ }
+
+ if (ascii)
+ {
+ if (pcl::io::savePLYFile (argv[ply_file_indices[0]], mesh) != 0)
+ {
+ PCL_ERROR ("Error writing PLY file\n");
+ return (-1);
+ }
+ }
+ else if (pcl::io::savePLYFileBinary (argv[ply_file_indices[0]], mesh) != 0)
+ {
+ PCL_ERROR ("Error writing PLY file\n");
+ return (-1);
+ }
+
+ return (0);
+}
std::cout << " the field type) will be used." << std::endl;
std::cout << " --colors : Choose color mapping mode for labels (only for label field)." << std::endl;
std::cout << " Supported options:" << std::endl;
- std::cout << " * mono : Use shades of gray (default)" << std::endl;
- std::cout << " - rgb : Use randomly generated RGB colors" << std::endl;
- std::cout << " - glasbey : Use fixed colors from the Glasbey lookup table¹" << std::endl;
+ std::cout << " - mono : Shades of gray" << std::endl;
+ std::cout << " - rgb : Randomly generated RGB colors" << std::endl;
+ std::cout << " * glasbey : Fixed colors from the Glasbey table¹ (default)" << std::endl;
std::cout << std::endl;
std::cout << "Notes:" << std::endl;
std::cout << std::endl;
std::cout << "¹) The Glasbey lookup table is a color table structured in a maximally" << std::endl;
std::cout << " discontinuous manner. Adjacent color bins are chosen to be as distinct" << std::endl;
- std::cout << " from one another as possible (see http://fiji.sc/Glasbey)." << std::endl;
+ std::cout << " from one another as possible (see https://github.com/taketwo/glasbey)." << std::endl;
std::cout << " The label with the smallest id will be assigned the first color from the" << std::endl;
std::cout << " table, the second smallest will have the second color, and so on. Thus," << std::endl;
std::cout << " if you have several clouds with the same labels, you will get repetitive" << std::endl;
pcie.setScalingMethod(pcie.SCALING_FIXED_FACTOR);
pcie.setScalingFactor(factor);
}
- catch (boost::bad_lexical_cast)
+ catch (const boost::bad_lexical_cast&)
{
print_error ("The value of --scale option should be \"no\", \"auto\", or a floating point number.\n");
return false;
template<typename T> bool
parseColorsOption (int argc, char** argv, T& pcie)
{
- std::string colors = "mono";
+ std::string colors = "glasbey";
pcl::console::parse_argument (argc, argv, "--colors", colors);
print_info ("Colors: "); print_value ("%s\n", colors.c_str());
if (colors == "mono")
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011-2012, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author : Christian Potthast
+ * Email : potthast@usc.edu
+ *
+ */
+
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/console/print.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/time.h>
+
+#include <pcl/segmentation/unary_classifier.h>
+
+using namespace pcl;
+using namespace pcl::io;
+using namespace pcl::console;
+
+unsigned int default_cluster_size = 10;
+double default_normal_radius_search = 0.01;
+double default_fpfh_radius_search = 0.05;
+
+typedef PointXYZ PointT;
+typedef PointCloud<PointT> CloudT;
+typedef PointCloud<PointXYZRGBL> CloudLT;
+typedef PointCloud<FPFHSignature33> FeatureT;
+
+void
+printHelp (int, char **argv)
+{
+ print_error ("Syntax is: %s input.pcd output.pcd <options>\n", argv[0]);
+ print_info (" where options are:\n");
+ print_info (" -label = point cloud with labeled objects \n");
+ print_info (" -k X = k-means cluster size (default: ");
+ print_value ("%d", static_cast<int> (default_cluster_size)); print_info (")\n");
+ print_info (" -normal-search X = Normal radius search (default: ");
+ print_value ("%f", default_normal_radius_search); print_info (")\n");
+ print_info (" -fpfh-search X = FPFH radius search (default: ");
+ print_value ("%f", default_fpfh_radius_search); print_info (")\n");
+}
+
+bool
+loadCloud (const std::string &filename, CloudT::Ptr &cloud)
+{
+ TicToc tt;
+ print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
+
+ tt.tic ();
+ if (loadPCDFile (filename, *cloud) < 0)
+ return (false);
+ print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud->width * cloud->height); print_info (" points]\n");
+
+ return (true);
+}
+
+bool
+loadCloud (const std::string &filename, CloudLT::Ptr &cloud)
+{
+ TicToc tt;
+ print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
+
+ tt.tic ();
+ if (loadPCDFile (filename, *cloud) < 0)
+ return (false);
+ print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud->width * cloud->height); print_info (" points]\n");
+
+ return (true);
+}
+
+void
+compute (const CloudT::Ptr &input, std::vector<FeatureT, Eigen::aligned_allocator<FeatureT> > &output,
+ unsigned int k,
+ float normal_radius_search,
+ float fpfh_radius_search,
+ bool label)
+{
+ TicToc tt;
+ tt.tic ();
+
+ print_highlight ("Computing ");
+
+ UnaryClassifier<PointT> classifier;
+ classifier.setInputCloud (input);
+ classifier.setClusterSize (k);
+ classifier.setNormalRadiusSearch (normal_radius_search);
+ classifier.setFPFHRadiusSearch (fpfh_radius_search);
+ classifier.setLabelField (label);
+
+ FeatureT::Ptr feature (new FeatureT);
+ classifier.train (feature);
+ output.push_back (*feature);
+
+ print_info ("[done, ");
+ print_value ("%g", tt.toc ());
+ print_info (" ms : "); print_value ("%d", feature->width * feature->height);
+ print_info (" features]\n");
+}
+
+void
+compute (const CloudLT::Ptr &input, std::vector<FeatureT, Eigen::aligned_allocator<FeatureT> > &output,
+ unsigned int k,
+ float normal_radius_search,
+ float fpfh_radius_search,
+ bool label)
+{
+ TicToc tt;
+ tt.tic ();
+
+ UnaryClassifier<PointXYZRGBL> classifier;
+ classifier.setInputCloud (input);
+ classifier.setClusterSize (k);
+ classifier.setNormalRadiusSearch (normal_radius_search);
+ classifier.setFPFHRadiusSearch (fpfh_radius_search);
+ classifier.setLabelField (label);
+
+ classifier.trainWithLabel (output);
+
+ print_highlight ("Computing ");
+ print_info ("[done, ");
+ print_value ("%g", tt.toc ());
+ print_info (" ms , ");
+ print_value ("%d", output.size ());
+ print_info (" objects : ");
+ print_value ("%d", output[0].width * output[0].height);
+ print_info (" features]\n");
+}
+
+void
+saveCloud (const std::string &filename, std::vector<FeatureT, Eigen::aligned_allocator<FeatureT> > &output)
+{
+ TicToc tt;
+ tt.tic ();
+
+ if (output.size () == 1)
+ {
+ print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
+ PCDWriter w;
+ w.write (filename, output[0]);
+
+ print_info ("[done, ");
+ print_value ("%g", tt.toc ());
+ print_info (" ms : "); print_value ("%d", output[0].width * output[0].height);
+ print_info (" features]\n");
+ }
+ else
+ {
+ for (size_t i = 0; i < output.size (); i++)
+ {
+ std::string fname (filename);
+ std::string s = boost::lexical_cast<std::string>( static_cast<int> (i) );
+ fname = fname + "_" + s + ".pcd";
+
+ print_highlight ("Saving "); print_value ("%s ", fname.c_str ());
+
+ PCDWriter w;
+ w.write (fname, output[i]);
+
+ print_info ("[done, ");
+ print_value ("%g", tt.toc ());
+ print_info (" ms , ");
+ print_value ("%d", i);
+ print_info (" objects : ");
+ print_value ("%d", output[i].width * output[i].height);
+ print_info (" features]\n");
+ }
+ }
+}
+
+/* ---[ */
+int
+main (int argc, char** argv)
+{
+ print_info ("Train unary classifier using FPFH. For more information, use: %s -h\n", argv[0]);
+
+ if (argc < 3)
+ {
+ printHelp (argc, argv);
+ return (-1);
+ }
+
+ bool label = (find_argument (argc, argv, "-label") != -1);
+
+ // Parse the command line arguments for .pcd files
+ std::vector<int> p_file_indices;
+ p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
+ if (!label)
+ {
+ if (p_file_indices.size () != 2)
+ {
+ print_error ("Need one input PCD file and one output PCD file to continue.\n");
+ return (-1);
+ }
+ }
+ else
+ {
+ if (p_file_indices.size () != 1)
+ {
+ print_error ("Need one input PCD file and one output file name to continue.\n");
+ return (-1);
+ }
+ }
+
+ // parse optional input arguments from the command line
+ unsigned int k = default_cluster_size;
+ float normal_radius_search = static_cast<float> (default_normal_radius_search);
+ float fpfh_radius_search = static_cast<float> (default_fpfh_radius_search);
+
+ parse_argument (argc, argv, "-k", k);
+ parse_argument (argc, argv, "-normal-radius-search", normal_radius_search);
+ parse_argument (argc, argv, "-fpfh-radius-search", fpfh_radius_search);
+
+ print_info ("\nlabel: %d \n", label);
+ print_info ("k-means cluster size: %d \n", k);
+ print_info ("normal-radius-search: %f \n", normal_radius_search);
+ print_info ("fpfh-radius-search: %f \n\n", fpfh_radius_search);
+
+ std::vector<FeatureT, Eigen::aligned_allocator<FeatureT> > features;
+ //FeatureT::Ptr feature (new FeatureT);
+ if (!label)
+ {
+ // Load the input file
+ CloudT::Ptr cloud (new CloudT);
+ if (!loadCloud (argv[p_file_indices[0]], cloud))
+ return (-1);
+
+ // compute the features
+ compute (cloud, features, k, normal_radius_search, fpfh_radius_search, label);
+ }
+ else
+ {
+ // Load the input file
+ CloudLT::Ptr cloudL (new CloudLT);
+ if (!loadCloud (argv[p_file_indices[0]], cloudL))
+ return (-1);
+
+ // compute the features
+ compute (cloudL, features, k, normal_radius_search, fpfh_radius_search, label);
+ }
+
+ // Save features to file
+ saveCloud (argv[2], features);
+}
+
print_info (" where options are:\n");
print_info (" -trans dx,dy,dz = the translation (default: ");
print_value ("%0.1f, %0.1f, %0.1f", 0, 0, 0); print_info (")\n");
- print_info (" -quat w,x,y,z = rotation as quaternion\n");
+ print_info (" -quat x,y,z,w = rotation as quaternion\n");
print_info (" -axisangle ax,ay,az,theta = rotation in axis-angle form\n");
print_info (" -scale x,y,z = scale each dimension with these values\n");
print_info (" -matrix v1,v2,...,v8,v9 = a 3x3 affine transform\n");
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2011-2012, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author : Christian Potthast
+ * Email : potthast@usc.edu
+ *
+ */
+
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/console/print.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/time.h>
+
+#include <pcl/segmentation/unary_classifier.h>
+
+using namespace pcl;
+using namespace pcl::io;
+using namespace pcl::console;
+
+double default_feature_threshold = 5.0;
+double default_normal_radius_search = 0.01;
+double default_fpfh_radius_search = 0.05;
+
+typedef PointXYZRGBA PointT;
+typedef PointCloud<PointT> CloudT;
+typedef PointCloud<PointXYZRGBL> CloudLT;
+typedef PointCloud<FPFHSignature33> FeatureT;
+
+void
+printHelp (int, char **argv)
+{
+ print_error ("Syntax is: %s input.pcd output.pcd <options>\n", argv[0]);
+ print_info (" where options are:\n");
+ print_info (" -d = trained features directory \n");
+ print_info (" -threshold X = feature threshold (default: ");
+ print_value ("%f", default_feature_threshold); print_info (")\n");
+ print_info (" -normal-search X = Normal radius search (default: ");
+ print_value ("%f", default_normal_radius_search); print_info (")\n");
+ print_info (" -fpfh-search X = FPFH radius search (default: ");
+ print_value ("%f", default_fpfh_radius_search); print_info (")\n");
+}
+
+bool
+loadTrainedFeatures (std::vector<FeatureT::Ptr> &out,
+ const boost::filesystem::path &base_dir)
+{
+ if (!boost::filesystem::exists (base_dir) && !boost::filesystem::is_directory (base_dir))
+ return false;
+
+ for (boost::filesystem::directory_iterator it (base_dir); it != boost::filesystem::directory_iterator (); ++it)
+ {
+ if (!boost::filesystem::is_directory (it->status ()) &&
+ boost::filesystem::extension (it->path ()) == ".pcd")
+ {
+ std::stringstream ss;
+ ss << it->path ().string ();
+
+ print_highlight ("Loading %s \n", ss.str ().c_str ());
+
+ FeatureT::Ptr features (new FeatureT);
+ if (loadPCDFile (ss.str ().c_str (), *features) < 0)
+ return false;
+
+ out.push_back (features);
+ }
+ }
+ return true;
+}
+
+bool
+loadCloud (const std::string &filename, CloudT::Ptr &cloud)
+{
+ TicToc tt;
+ print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
+
+ tt.tic ();
+ if (loadPCDFile (filename, *cloud) < 0)
+ return (false);
+ print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud->width * cloud->height); print_info (" points]\n");
+
+ return (true);
+}
+
+void
+compute (const CloudT::Ptr &input, std::vector<FeatureT::Ptr> &trained_features,
+ CloudLT::Ptr &out,
+ float normal_radius_search,
+ float fpfh_radius_search,
+ float feature_threshold)
+{
+ TicToc tt;
+ tt.tic ();
+
+ print_highlight ("Computing ");
+
+ UnaryClassifier<PointT> classifier;
+ classifier.setInputCloud (input);
+ classifier.setTrainedFeatures (trained_features);
+ classifier.setNormalRadiusSearch (normal_radius_search);
+ classifier.setFPFHRadiusSearch (fpfh_radius_search);
+ classifier.setFeatureThreshold (feature_threshold);
+
+ classifier.segment (out);
+
+ print_info ("[done, ");
+ print_value ("%g", tt.toc ());
+ print_info (" ms : "); print_value ("%d", out->width * out->height);
+ print_info (" points]\n");
+
+}
+
+void
+saveCloud (const std::string &filename, CloudLT::Ptr &output)
+{
+ TicToc tt;
+ tt.tic ();
+
+ print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
+
+ PCDWriter w;
+ w.write (filename, *output);
+
+ print_info ("[done, ");
+ print_value ("%g", tt.toc ()); print_info (" ms : ");
+ print_value ("%d", output->width * output->height); print_info (" points]\n");
+}
+
+/* ---[ */
+int
+main (int argc, char** argv)
+{
+ print_info ("Train unary classifier using FPFH. For more information, use: %s -h\n", argv[0]);
+
+ if (argc < 4)
+ {
+ printHelp (argc, argv);
+ return (-1);
+ }
+
+ // Parse the command line arguments for .pcd files
+ std::vector<int> p_file_indices;
+ p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
+ if (p_file_indices.size () != 2)
+ {
+ print_error ("Need one input PCD file and one output PCD file to continue.\n");
+ return (-1);
+ }
+
+ // Load the input file
+ CloudT::Ptr cloud (new CloudT);
+ if (!loadCloud (argv[p_file_indices[0]], cloud))
+ return (-1);
+
+ // TODO:: make this as an optional argument ??
+ std::vector<int> tmp_indices;
+ pcl::removeNaNFromPointCloud (*cloud, *cloud, tmp_indices);
+
+ // parse optional input arguments from the command line
+ float normal_radius_search = static_cast<float> (default_normal_radius_search);
+ float fpfh_radius_search = static_cast<float> (default_fpfh_radius_search);
+ float feature_threshold = static_cast<float> (default_feature_threshold);
+ std::string dir_name;
+
+ parse_argument (argc, argv, "-d", dir_name);
+ parse_argument (argc, argv, "-threshold", feature_threshold);
+ parse_argument (argc, argv, "-normal-radius-search", normal_radius_search);
+ parse_argument (argc, argv, "-fpfh-radius-search", fpfh_radius_search);
+
+
+ print_info ("trained feature directory: %s \n", dir_name.c_str ());
+
+ // load the trained features
+ std::vector<FeatureT::Ptr> trained_features;
+ if (!loadTrainedFeatures (trained_features, dir_name.c_str ()))
+ return (-1);
+
+ print_info ("feature_threshold: %f \n", feature_threshold);
+ print_info ("normal-radius-search: %f \n", normal_radius_search);
+ print_info ("fpfh-radius-search: %f \n\n", fpfh_radius_search);
+
+ CloudLT::Ptr out (new CloudLT);
+ compute (cloud, trained_features, out, normal_radius_search, fpfh_radius_search, feature_threshold);
+
+ saveCloud (argv[p_file_indices[1]], out);
+}
+
#include <pcl/PCLPointCloud2.h>
#include <pcl/io/pcd_io.h>
-#include <pcl/keypoints/uniform_sampling.h>
+#include <pcl/filters/uniform_sampling.h>
#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>
UniformSampling<PointXYZ> us;
us.setInputCloud (xyz);
us.setRadiusSearch (radius);
- PointCloud<int> subsampled_indices;
- us.compute (subsampled_indices);
- std::sort (subsampled_indices.points.begin (), subsampled_indices.points.end ());
+ PointCloud<PointXYZ> output_;
+ us.filter (output_);
- print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", subsampled_indices.width * subsampled_indices.height); print_info (" points]\n");
+ print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output_.size()); print_info (" points]\n");
// Convert data back
- copyPointCloud (*input, subsampled_indices.points, output);
+ toPCLPointCloud2 (output_, output);
}
void
tt.tic ();
// estimate the occluded space
- std::vector <Eigen::Vector3i> occluded_voxels;
+ std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > occluded_voxels;
vg.occlusionEstimationAll (occluded_voxels);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", (int)occluded_voxels.size ()); print_info (" occluded voxels]\n");
++k;
++n;
}
- while (k < 2 || (n < maximum_particle_number_ && n < calcKLBound (k)));
+ while (n < maximum_particle_number_ && (k < 2 || n < calcKLBound (k)));
particles_ = S; // swap
particle_num_ = static_cast<int> (particles_->points.size ());
int width = (smoothed.width +1) / 2;
int height = (smoothed.height +1) / 2;
-
- int *ii = new int[width];
- int *ii_ptr = ii;
+ std::vector<int> ii (width);
for (int i = 0; i < width; ++i)
- *ii_ptr++ = 2*i;
+ ii[i] = 2 * i;
FloatImagePtr down (new FloatImage (width, height));
#ifdef _OPENMP
-#pragma omp parallel for shared (output) private (ii) num_threads (threads_)
+#pragma omp parallel for shared (output) firstprivate (ii) num_threads (threads_)
#endif
for (int j = 0; j < height; ++j)
{
std::vector<int>& status,
Eigen::Affine3f& motion) const
{
- std::vector<Eigen::Array2f> next_pts (prev_keypoints->size ());
+ std::vector<Eigen::Array2f, Eigen::aligned_allocator<Eigen::Array2f> > next_pts (prev_keypoints->size ());
Eigen::Array2f half_win ((track_width_-1)*0.5f, (track_height_-1)*0.5f);
pcl::TransformationFromCorrespondences transformation_computer;
const int nb_points = prev_keypoints->size ();
endif(NOT VTK_FOUND)
PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}")
-PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS vtk OPT_DEPS openni openni2)
+PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS vtk OPT_DEPS openni openni2 ensenso davidSDK dssdk rssdk)
if (ANDROID)
set (build FALSE)
src/common/float_image_utils.cpp
src/vtk/pcl_image_canvas_source_2d.cpp
src/vtk/pcl_context_item.cpp
- src/vtk/vtkVertexBufferObject.cxx
- src/vtk/vtkVertexBufferObjectMapper.cxx
src/vtk/vtkRenderWindowInteractorFix.cpp
)
if("${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}" VERSION_LESS "5.6")
)
endif("${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}" VERSION_GREATER "5.6")
+ if(VTK_RENDERING_BACKEND_OPENGL_VERSION VERSION_LESS 2)
+ list(APPEND srcs
+ "src/vtk/vtkVertexBufferObject.cxx"
+ "src/vtk/vtkVertexBufferObjectMapper.cxx"
+ )
+ endif()
+
set(incs
"include/pcl/${SUBSYS_NAME}/eigen.h"
"include/pcl/${SUBSYS_NAME}/boost.h"
set(vtk_incs
"include/pcl/${SUBSYS_NAME}/vtk/pcl_image_canvas_source_2d.h"
"include/pcl/${SUBSYS_NAME}/vtk/pcl_context_item.h"
- "include/pcl/${SUBSYS_NAME}/vtk/vtkVertexBufferObject.h"
"include/pcl/${SUBSYS_NAME}/vtk/vtkRenderWindowInteractorFix.h"
- "include/pcl/${SUBSYS_NAME}/vtk/vtkVertexBufferObjectMapper.h"
)
+ if(VTK_RENDERING_BACKEND_OPENGL_VERSION VERSION_LESS 2)
+ list(APPEND vtk_incs
+ "include/pcl/${SUBSYS_NAME}/vtk/vtkVertexBufferObject.h"
+ "include/pcl/${SUBSYS_NAME}/vtk/vtkVertexBufferObjectMapper.h"
+ )
+ endif()
+
# on apple, a workaround is used for the cocoa render window interactor
if(APPLE)
list(APPEND srcs
endif()
set(EXT_DEPS "")
- if(OPENNI_FOUND)
- list(APPEND EXT_DEPS openni-dev)
- endif(OPENNI_FOUND)
- if(OPENNI2_FOUND)
- list(APPEND EXT_DEPS openni2-dev)
- endif(OPENNI2_FOUND)
+ if(WITH_OPENNI)
+ list(APPEND EXT_DEPS libopenni)
+ endif()
+ if(WITH_OPENNI2)
+ list(APPEND EXT_DEPS libopenni2)
+ endif()
+ if(WITH_ENSENSO)
+ list(APPEND EXT_DEPS ensenso)
+ endif()
+ if(WITH_DAVIDSDK)
+ list(APPEND EXT_DEPS davidSDK)
+ endif()
+ if(WITH_DSSDK)
+ list(APPEND EXT_DEPS dssdk)
+ endif()
+ if(WITH_RSSDK)
+ list(APPEND EXT_DEPS rssdk)
+ endif()
PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}"
"${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "")
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
#include <boost/foreach.hpp>
+#ifndef Q_MOC_RUN
#include <boost/date_time/posix_time/posix_time.hpp>
+#endif
#include <boost/filesystem.hpp>
#endif
#include <pcl/pcl_macros.h>
#include <pcl/visualization/eigen.h>
#include <vtkMatrix4x4.h>
+#include <vtkSmartPointer.h>
+#include <vtkLookupTable.h>
namespace pcl
{
PCL_EXPORTS float
viewScreenArea (const Eigen::Vector3d &eye, const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Matrix4d &view_projection_matrix, int width, int height);
+ /** \brief Set of rendering properties. */
enum RenderingProperties
{
- PCL_VISUALIZER_POINT_SIZE,
- PCL_VISUALIZER_OPACITY,
- PCL_VISUALIZER_LINE_WIDTH,
+ PCL_VISUALIZER_POINT_SIZE, /**< integer starting from 1 */
+ PCL_VISUALIZER_OPACITY, /**< Float going from 0.0 (transparent) to 1.0 (opaque) */
+ PCL_VISUALIZER_LINE_WIDTH, /**< Integer starting from 1 */
PCL_VISUALIZER_FONT_SIZE,
- PCL_VISUALIZER_COLOR,
+ PCL_VISUALIZER_COLOR, /**< 3 floats (R, G, B) going from 0.0 (dark) to 1.0 (light) */
PCL_VISUALIZER_REPRESENTATION,
PCL_VISUALIZER_IMMEDIATE_RENDERING,
- PCL_VISUALIZER_SHADING
+ PCL_VISUALIZER_SHADING,
+ PCL_VISUALIZER_LUT, /**< colormap type \ref pcl::visualization::LookUpTableRepresentationProperties */
+ PCL_VISUALIZER_LUT_RANGE /**< two doubles (min and max) or \ref pcl::visualization::LookUpTableRepresentationProperties::PCL_VISUALIZER_LUT_RANGE_AUTO */
};
enum RenderingRepresentationProperties
PCL_VISUALIZER_SHADING_PHONG
};
+ /*! Colormap properties. See [mathworks colormap page](http://www.mathworks.com/help/matlab/ref/colormap.html#input_argument_name) for image representations of the colormaps. */
+ enum LookUpTableRepresentationProperties
+ {
+ PCL_VISUALIZER_LUT_JET, /**< Jet colormap */
+ PCL_VISUALIZER_LUT_JET_INVERSE, /**< Inverse jet colormap */
+ PCL_VISUALIZER_LUT_HSV, /**< HSV colormap */
+ PCL_VISUALIZER_LUT_HSV_INVERSE, /**< Inverse HSV colormap */
+ PCL_VISUALIZER_LUT_GREY, /**< Grey colormap (black to white) */
+ PCL_VISUALIZER_LUT_BLUE2RED, /**< Blue to red colormap (blue to white to red) */
+ PCL_VISUALIZER_LUT_RANGE_AUTO /**< Set LUT range to min and max values of the data */
+ };
+
+ /** \brief Generate a lookup table for a colormap.
+ * \param[in] colormap_type
+ * \param[out] table a vtk lookup table
+ * \note The list of available colormaps can be found in \ref pcl::visualization::LookUpTableRepresentationProperties.
+ */
+ PCL_EXPORTS bool
+ getColormapLUT (LookUpTableRepresentationProperties colormap_type, vtkSmartPointer<vtkLookupTable> &table);
+
//////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Camera class holds a set of camera parameters together with the window pos/size. */
class PCL_EXPORTS Camera
if (am_it == layer_map_.end ())
{
PCL_DEBUG ("[pcl::visualization::ImageViewer::addMask] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
- am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, true);
+ am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, false);
}
// Construct a search object to get the camera parameters
search.setInputCloud (image);
std::vector<float> xy;
xy.reserve (mask.size () * 2);
- const float image_height_f = static_cast<float> (image->height);
for (size_t i = 0; i < mask.size (); ++i)
{
pcl::PointXY p_projected;
search.projectPoint (mask[i], p_projected);
xy.push_back (p_projected.x);
- #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 10))
- xy.push_back (image_height_f - p_projected.y);
+ #if ((VTK_MAJOR_VERSION >= 6) || ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 7)))
+ xy.push_back (static_cast<float> (image->height) - p_projected.y);
#else
xy.push_back (p_projected.y);
#endif
static_cast<unsigned char> (g*255.0),
static_cast<unsigned char> (b*255.0));
points->setOpacity (opacity);
+ points->set (xy);
am_it->actor->GetScene ()->AddItem (points);
return (true);
}
if (am_it == layer_map_.end ())
{
PCL_DEBUG ("[pcl::visualization::ImageViewer::addPlanarPolygon] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
- am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, true);
+ am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, false);
}
// Construct a search object to get the camera parameters and fill points
pcl::search::OrganizedNeighbor<T> search;
search.setInputCloud (image);
- const float image_height_f = static_cast<float> (image->height);
std::vector<float> xy;
xy.reserve ((polygon.getContour ().size () + 1) * 2);
for (size_t i = 0; i < polygon.getContour ().size (); ++i)
pcl::PointXY p;
search.projectPoint (polygon.getContour ()[i], p);
xy.push_back (p.x);
- #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 10))
- xy.push_back (image_height_f - p.y);
+ #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 7))
+ xy.push_back (static_cast<float> (image->height) - p.y);
#else
xy.push_back (p.y);
#endif
poly->setColors (static_cast<unsigned char> (r * 255.0),
static_cast<unsigned char> (g * 255.0),
static_cast<unsigned char> (b * 255.0));
+ poly->setOpacity (opacity);
poly->set (xy);
am_it->actor->GetScene ()->AddItem (poly);
if (am_it == layer_map_.end ())
{
PCL_DEBUG ("[pcl::visualization::ImageViewer::addRectangle] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
- am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, true);
+ am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, false);
}
// Construct a search object to get the camera parameters
if (pp_2d[i].x > max_pt_2d.x) max_pt_2d.x = pp_2d[i].x;
if (pp_2d[i].y > max_pt_2d.y) max_pt_2d.y = pp_2d[i].y;
}
-#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 7))
+#if ((VTK_MAJOR_VERSION >= 6) || ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 7)))
min_pt_2d.y = float (image->height) - min_pt_2d.y;
max_pt_2d.y = float (image->height) - max_pt_2d.y;
#endif
if (am_it == layer_map_.end ())
{
PCL_DEBUG ("[pcl::visualization::ImageViewer::addRectangle] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
- am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, true);
+ am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, false);
}
// Construct a search object to get the camera parameters
if (pp_2d[i].x > max_pt_2d.x) max_pt_2d.x = pp_2d[i].x;
if (pp_2d[i].y > max_pt_2d.y) max_pt_2d.y = pp_2d[i].y;
}
-#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 7))
+#if ((VTK_MAJOR_VERSION >= 6) ||((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 7)))
min_pt_2d.y = float (image->height) - min_pt_2d.y;
max_pt_2d.y = float (image->height) - max_pt_2d.y;
#endif
#include <vtkVectorText.h>
#include <vtkAlgorithmOutput.h>
#include <vtkFollower.h>
+#include <vtkMath.h>
#include <vtkSphereSource.h>
#include <vtkProperty2D.h>
#include <vtkDataSetSurfaceFilter.h>
#include <vtkAppendPolyData.h>
#include <vtkTextProperty.h>
#include <vtkLODActor.h>
+#include <vtkLineSource.h>
#include <pcl/visualization/common/shapes.h>
const PointCloudGeometryHandler<PointT> &geometry_handler,
const std::string &id, int viewport)
{
- // Check to see if this ID entry already exists (has it been already added to the visualizer?)
- CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
-
- if (am_it != cloud_actor_map_->end ())
+ if (contains (id))
{
- PCL_WARN ("[addPointCloud] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
+ PCL_WARN ("[addPointCloud] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
return (false);
}
- //PointCloudColorHandlerRandom<PointT> color_handler (cloud);
+ if (pcl::traits::has_color<PointT>())
+ {
+ PointCloudColorHandlerRGBField<PointT> color_handler_rgb_field (cloud);
+ return (fromHandlersToScreen (geometry_handler, color_handler_rgb_field, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
+ }
PointCloudColorHandlerCustom<PointT> color_handler (cloud, 255, 255, 255);
return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
}
const GeometryHandlerConstPtr &geometry_handler,
const std::string &id, int viewport)
{
- // Check to see if this ID entry already exists (has it been already added to the visualizer?)
- CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
-
- if (am_it != cloud_actor_map_->end ())
+ if (contains (id))
{
// Here we're just pushing the handlers onto the queue. If needed, something fancier could
// be done such as checking if a specific handler already exists, etc.
+ CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
am_it->second.geometry_handlers.push_back (geometry_handler);
return (true);
}
const PointCloudColorHandler<PointT> &color_handler,
const std::string &id, int viewport)
{
- // Check to see if this ID entry already exists (has it been already added to the visualizer?)
- CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
-
- if (am_it != cloud_actor_map_->end ())
+ if (contains (id))
{
- PCL_WARN ("[addPointCloud] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
+ PCL_WARN ("[addPointCloud] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
// Here we're just pushing the handlers onto the queue. If needed, something fancier could
// be done such as checking if a specific handler already exists, etc.
const PointCloudGeometryHandler<PointT> &geometry_handler,
const std::string &id, int viewport)
{
- // Check to see if this ID entry already exists (has it been already added to the visualizer?)
- CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
-
- if (am_it != cloud_actor_map_->end ())
+ if (contains (id))
{
- PCL_WARN ("[addPointCloud] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
+ PCL_WARN ("[addPointCloud] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
// Here we're just pushing the handlers onto the queue. If needed, something fancier could
// be done such as checking if a specific handler already exists, etc.
//cloud_actor_map_[id].geometry_handlers.push_back (geometry_handler);
template <typename P1, typename P2> bool
pcl::visualization::PCLVisualizer::addLine (const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id, int viewport)
{
- // Check to see if this ID entry already exists (has it been already added to the visualizer?)
- ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
- if (am_it != shape_actor_map_->end ())
+ if (contains (id))
{
- PCL_WARN ("[addLine] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
+ PCL_WARN ("[addLine] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
return (false);
}
template <typename P1, typename P2> bool
pcl::visualization::PCLVisualizer::addArrow (const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id, int viewport)
{
- // Check to see if this ID entry already exists (has it been already added to the visualizer?)
- ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
- if (am_it != shape_actor_map_->end ())
+ if (contains (id))
{
- PCL_WARN ("[addArrow] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
+ PCL_WARN ("[addArrow] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
return (false);
}
template <typename P1, typename P2> bool
pcl::visualization::PCLVisualizer::addArrow (const P1 &pt1, const P2 &pt2, double r, double g, double b, bool display_length, const std::string &id, int viewport)
{
- // Check to see if this ID entry already exists (has it been already added to the visualizer?)
- ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
- if (am_it != shape_actor_map_->end ())
+ if (contains (id))
{
- PCL_WARN ("[addArrow] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
+ PCL_WARN ("[addArrow] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
return (false);
}
////////////////////////////////////////////////////////////////////////////////////////////
template <typename P1, typename P2> bool
pcl::visualization::PCLVisualizer::addArrow (const P1 &pt1, const P2 &pt2,
- double r_line, double g_line, double b_line,
- double r_text, double g_text, double b_text,
- const std::string &id, int viewport)
+ double r_line, double g_line, double b_line,
+ double r_text, double g_text, double b_text,
+ const std::string &id, int viewport)
{
- // Check to see if this ID entry already exists (has it been already added to the visualizer?)
- ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
- if (am_it != shape_actor_map_->end ())
+ if (contains (id))
{
- PCL_WARN ("[addArrow] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
+ PCL_WARN ("[addArrow] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
return (false);
}
template <typename PointT> bool
pcl::visualization::PCLVisualizer::addSphere (const PointT ¢er, double radius, double r, double g, double b, const std::string &id, int viewport)
{
- // Check to see if this ID entry already exists (has it been already added to the visualizer?)
- ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
- if (am_it != shape_actor_map_->end ())
+ if (contains (id))
{
- PCL_WARN ("[addSphere] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
+ PCL_WARN ("[addSphere] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
return (false);
}
template<typename PointT> bool
pcl::visualization::PCLVisualizer::updateSphere (const PointT ¢er, double radius, double r, double g, double b, const std::string &id)
{
- // Check to see if this ID entry already exists (has it been already added to the visualizer?)
- ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
- if (am_it == shape_actor_map_->end ())
+ if (!contains (id))
+ {
return (false);
+ }
//////////////////////////////////////////////////////////////////////////
// Get the actor pointer
+ ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
+ if (!actor)
+ return (false);
#if VTK_MAJOR_VERSION < 6
vtkAlgorithm *algo = actor->GetMapper ()->GetInput ()->GetProducerPort ()->GetProducer ();
#else
vtkAlgorithm *algo = actor->GetMapper ()->GetInputAlgorithm ();
#endif
vtkSphereSource *src = vtkSphereSource::SafeDownCast (algo);
+ if (!src)
+ return (false);
src->SetCenter (double (center.x), double (center.y), double (center.z));
src->SetRadius (radius);
else
tid = id;
- // Check to see if this ID entry already exists (has it been already added to the visualizer?)
- ShapeActorMap::iterator am_it = shape_actor_map_->find (tid);
- if (am_it != shape_actor_map_->end ())
+ if (contains (tid))
{
- pcl::console::print_warn (stderr, "[addText3d] A text with id <%s> already exists! Please choose a different id and retry.\n", tid.c_str ());
+ PCL_WARN ("[addText3D] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
return (false);
}
PCL_ERROR ("[addPointCloudNormals] The number of points differs from the number of normals!\n");
return (false);
}
- // Check to see if this ID entry already exists (has it been already added to the visualizer?)
- CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
-
- if (am_it != cloud_actor_map_->end ())
+ if (contains (id))
{
- PCL_WARN ("[addPointCloudNormals] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
+ PCL_WARN ("[addPointCloudNormals] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
return (false);
}
}
}
- data->SetArray (&pts[0], 2 * nr_normals * 3, 0);
+ data->SetArray (&pts[0], 2 * nr_normals * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
points->SetData (data);
vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
return (true);
}
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointNT> bool
+pcl::visualization::PCLVisualizer::addPointCloudPrincipalCurvatures (
+ const typename pcl::PointCloud<PointNT>::ConstPtr &cloud,
+ const pcl::PointCloud<pcl::PrincipalCurvatures>::ConstPtr &pcs,
+ int level, float scale,
+ const std::string &id, int viewport)
+{
+ return (addPointCloudPrincipalCurvatures<PointNT, PointNT> (cloud, cloud, pcs, level, scale, id, viewport));
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT, typename PointNT> bool
+pcl::visualization::PCLVisualizer::addPointCloudPrincipalCurvatures (
+ const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
+ const typename pcl::PointCloud<PointNT>::ConstPtr &normals,
+ const pcl::PointCloud<pcl::PrincipalCurvatures>::ConstPtr &pcs,
+ int level, float scale,
+ const std::string &id, int viewport)
+{
+ if (pcs->points.size () != cloud->points.size () || normals->points.size () != cloud->points.size ())
+ {
+ pcl::console::print_error ("[addPointCloudPrincipalCurvatures] The number of points differs from the number of principal curvatures/normals!\n");
+ return (false);
+ }
+
+ if (contains (id))
+ {
+ PCL_WARN ("[addPointCloudPrincipalCurvatures] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
+ return (false);
+ }
+
+ vtkSmartPointer<vtkAppendPolyData> polydata_1 = vtkSmartPointer<vtkAppendPolyData>::New ();
+ vtkSmartPointer<vtkAppendPolyData> polydata_2 = vtkSmartPointer<vtkAppendPolyData>::New ();
+
+ // Setup two colors - one for each line
+ unsigned char green[3] = {0, 255, 0};
+ unsigned char blue[3] = {0, 0, 255};
+
+ // Setup the colors array
+ vtkSmartPointer<vtkUnsignedCharArray> line_1_colors =vtkSmartPointer<vtkUnsignedCharArray>::New ();
+ line_1_colors->SetNumberOfComponents (3);
+ line_1_colors->SetName ("Colors");
+ vtkSmartPointer<vtkUnsignedCharArray> line_2_colors =vtkSmartPointer<vtkUnsignedCharArray>::New ();
+ line_2_colors->SetNumberOfComponents (3);
+ line_2_colors->SetName ("Colors");
+
+ // Create the first sets of lines
+ for (size_t i = 0; i < cloud->points.size (); i+=level)
+ {
+ PointT p = cloud->points[i];
+ p.x += (pcs->points[i].pc1 * pcs->points[i].principal_curvature[0]) * scale;
+ p.y += (pcs->points[i].pc1 * pcs->points[i].principal_curvature[1]) * scale;
+ p.z += (pcs->points[i].pc1 * pcs->points[i].principal_curvature[2]) * scale;
+
+ vtkSmartPointer<vtkLineSource> line_1 = vtkSmartPointer<vtkLineSource>::New ();
+ line_1->SetPoint1 (cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
+ line_1->SetPoint2 (p.x, p.y, p.z);
+ line_1->Update ();
+#if VTK_MAJOR_VERSION < 6
+ polydata_1->AddInput (line_1->GetOutput ());
+#else
+ polydata_1->AddInputData (line_1->GetOutput ());
+#endif
+ line_1_colors->InsertNextTupleValue (green);
+ }
+ polydata_1->Update ();
+ vtkSmartPointer<vtkPolyData> line_1_data = polydata_1->GetOutput ();
+ line_1_data->GetCellData ()->SetScalars (line_1_colors);
+
+ // Create the second sets of lines
+ for (size_t i = 0; i < cloud->points.size (); i += level)
+ {
+ Eigen::Vector3f pc (pcs->points[i].principal_curvature[0],
+ pcs->points[i].principal_curvature[1],
+ pcs->points[i].principal_curvature[2]);
+ Eigen::Vector3f normal (normals->points[i].normal[0],
+ normals->points[i].normal[1],
+ normals->points[i].normal[2]);
+ Eigen::Vector3f pc_c = pc.cross (normal);
+
+ PointT p = cloud->points[i];
+ p.x += (pcs->points[i].pc2 * pc_c[0]) * scale;
+ p.y += (pcs->points[i].pc2 * pc_c[1]) * scale;
+ p.z += (pcs->points[i].pc2 * pc_c[2]) * scale;
+
+ vtkSmartPointer<vtkLineSource> line_2 = vtkSmartPointer<vtkLineSource>::New ();
+ line_2->SetPoint1 (cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
+ line_2->SetPoint2 (p.x, p.y, p.z);
+ line_2->Update ();
+#if VTK_MAJOR_VERSION < 6
+ polydata_2->AddInput (line_2->GetOutput ());
+#else
+ polydata_2->AddInputData (line_2->GetOutput ());
+#endif
+
+ line_2_colors->InsertNextTupleValue (blue);
+ }
+ polydata_2->Update ();
+ vtkSmartPointer<vtkPolyData> line_2_data = polydata_2->GetOutput ();
+ line_2_data->GetCellData ()->SetScalars (line_2_colors);
+
+ // Assemble the two sets of lines
+ vtkSmartPointer<vtkAppendPolyData> alldata = vtkSmartPointer<vtkAppendPolyData>::New ();
+#if VTK_MAJOR_VERSION < 6
+ alldata->AddInput (line_1_data);
+ alldata->AddInput (line_2_data);
+#else
+ alldata->AddInputData (line_1_data);
+ alldata->AddInputData (line_2_data);
+#endif
+
+ // Create an Actor
+ vtkSmartPointer<vtkLODActor> actor;
+ createActorFromVTKDataSet (alldata->GetOutput (), actor);
+ actor->GetMapper ()->SetScalarModeToUseCellData ();
+
+ // Add it to all renderers
+ addActorToRenderer (actor, viewport);
+
+ // Save the pointer/ID pair to the global actor map
+ CloudActor act;
+ act.actor = actor;
+ (*cloud_actor_map_)[id] = act;
+ return (true);
+}
+
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename GradientT> bool
pcl::visualization::PCLVisualizer::addPointCloudIntensityGradients (
PCL_ERROR ("[addPointCloudGradients] The number of points differs from the number of gradients!\n");
return (false);
}
- // Check to see if this ID entry already exists (has it been already added to the visualizer?)
- CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
-
- if (am_it != cloud_actor_map_->end ())
+ if (contains (id))
{
- PCL_WARN ("[addPointCloudGradients] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
+ PCL_WARN ("[addPointCloudGradients] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
return (false);
}
lines->InsertCellPoint(2*j+1);
}
- data->SetArray (&pts[0], 2 * nr_gradients * 3, 0);
+ data->SetArray (&pts[0], 2 * nr_gradients * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
points->SetData (data);
vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
const std::string &id,
int viewport)
{
- // Check to see if this ID entry already exists (has it been already added to the visualizer?)
- ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
- if (am_it != shape_actor_map_->end ())
- {
- PCL_WARN ("[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
- return (false);
- }
-
- int n_corr = int (correspondences.size ());
- vtkSmartPointer<vtkPolyData> line_data = vtkSmartPointer<vtkPolyData>::New ();
-
- // Prepare colors
- vtkSmartPointer<vtkUnsignedCharArray> line_colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
- line_colors->SetNumberOfComponents (3);
- line_colors->SetName ("Colors");
- line_colors->SetNumberOfTuples (n_corr);
- unsigned char* colors = line_colors->GetPointer (0);
- memset (colors, 0, line_colors->GetNumberOfTuples () * line_colors->GetNumberOfComponents ());
- pcl::RGB rgb;
- // Will use random colors or RED by default
- rgb.r = 255; rgb.g = rgb.b = 0;
+ pcl::Correspondences corrs;
+ corrs.resize (correspondences.size ());
- // Prepare coordinates
- vtkSmartPointer<vtkPoints> line_points = vtkSmartPointer<vtkPoints>::New ();
- line_points->SetNumberOfPoints (2 * n_corr);
-
- vtkSmartPointer<vtkIdTypeArray> line_cells_id = vtkSmartPointer<vtkIdTypeArray>::New ();
- line_cells_id->SetNumberOfComponents (3);
- line_cells_id->SetNumberOfTuples (n_corr);
- vtkIdType *line_cell_id = line_cells_id->GetPointer (0);
- vtkSmartPointer<vtkCellArray> line_cells = vtkSmartPointer<vtkCellArray>::New ();
-
- vtkSmartPointer<vtkFloatArray> line_tcoords = vtkSmartPointer<vtkFloatArray>::New ();
- line_tcoords->SetNumberOfComponents (1);
- line_tcoords->SetNumberOfTuples (n_corr * 2);
- line_tcoords->SetName ("Texture Coordinates");
-
- double tc[3] = {0.0, 0.0, 0.0};
-
- int j = 0, idx = 0;
- // Draw lines between the best corresponding points
- for (int i = 0; i < n_corr; ++i)
+ size_t index = 0;
+ for (pcl::Correspondences::iterator corrs_it (corrs.begin ()); corrs_it != corrs.end (); ++corrs_it)
{
- const PointT &p_src = source_points->points[i];
- const PointT &p_tgt = target_points->points[correspondences[i]];
-
- int id1 = j * 2 + 0, id2 = j * 2 + 1;
- // Set the points
- line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z);
- line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z);
- // Set the cell ID
- *line_cell_id++ = 2;
- *line_cell_id++ = id1;
- *line_cell_id++ = id2;
- // Set the texture coords
- tc[0] = 0.; line_tcoords->SetTuple (id1, tc);
- tc[0] = 1.; line_tcoords->SetTuple (id2, tc);
-
- getRandomColors (rgb);
- colors[idx+0] = rgb.r;
- colors[idx+1] = rgb.g;
- colors[idx+2] = rgb.b;
+ corrs_it->index_query = index;
+ corrs_it->index_match = correspondences[index];
+ index++;
}
- line_colors->SetNumberOfTuples (j);
- line_cells_id->SetNumberOfTuples (j);
- line_cells->SetCells (n_corr, line_cells_id);
- line_points->SetNumberOfPoints (j*2);
- line_tcoords->SetNumberOfTuples (j*2);
-
- // Fill in the lines
- line_data->SetPoints (line_points);
- line_data->SetLines (line_cells);
- line_data->GetPointData ()->SetTCoords (line_tcoords);
- line_data->GetCellData ()->SetScalars (line_colors);
-
- // Create an Actor
- vtkSmartPointer<vtkLODActor> actor;
- createActorFromVTKDataSet (line_data, actor);
- actor->GetProperty ()->SetRepresentationToWireframe ();
- actor->GetProperty ()->SetOpacity (0.5);
- addActorToRenderer (actor, viewport);
-
- // Save the pointer/ID pair to the global actor map
- (*shape_actor_map_)[id] = actor;
- return (true);
+ return (addCorrespondences<PointT> (source_points, target_points, corrs, id, viewport));
}
//////////////////////////////////////////////////////////////////////////////////////////////
const pcl::Correspondences &correspondences,
int nth,
const std::string &id,
- int viewport)
+ int viewport,
+ bool overwrite)
{
if (correspondences.empty ())
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
- if (am_it != shape_actor_map_->end ())
+ if (am_it != shape_actor_map_->end () && overwrite == false)
{
PCL_WARN ("[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
return (false);
+ } else if (am_it == shape_actor_map_->end () && overwrite == true)
+ {
+ overwrite = false; // Correspondences doesn't exist, add them instead of updating them
}
- int n_corr = int (correspondences.size () / nth + 1);
+ int n_corr = int (correspondences.size () / nth);
vtkSmartPointer<vtkPolyData> line_data = vtkSmartPointer<vtkPolyData>::New ();
// Prepare colors
line_colors->SetNumberOfComponents (3);
line_colors->SetName ("Colors");
line_colors->SetNumberOfTuples (n_corr);
- unsigned char* colors = line_colors->GetPointer (0);
- memset (colors, 0, line_colors->GetNumberOfTuples () * line_colors->GetNumberOfComponents ());
- pcl::RGB rgb;
- // Will use random colors or RED by default
- rgb.r = 255; rgb.g = rgb.b = 0;
// Prepare coordinates
vtkSmartPointer<vtkPoints> line_points = vtkSmartPointer<vtkPoints>::New ();
double tc[3] = {0.0, 0.0, 0.0};
- int j = 0, idx = 0;
+ Eigen::Affine3f source_transformation;
+ source_transformation.linear () = source_points->sensor_orientation_.matrix ();
+ source_transformation.translation () = source_points->sensor_origin_.head (3);
+ Eigen::Affine3f target_transformation;
+ target_transformation.linear () = target_points->sensor_orientation_.matrix ();
+ target_transformation.translation () = target_points->sensor_origin_.head (3);
+
+ int j = 0;
// Draw lines between the best corresponding points
- for (size_t i = 0; i < correspondences.size (); i += nth, idx = j * 3, ++j)
+ for (size_t i = 0; i < correspondences.size (); i += nth, ++j)
{
- const PointT &p_src = source_points->points[correspondences[i].index_query];
- const PointT &p_tgt = target_points->points[correspondences[i].index_match];
+ if (correspondences[i].index_match == -1)
+ {
+ PCL_WARN ("[addCorrespondences] No valid index_match for correspondence %d\n", i);
+ continue;
+ }
+
+ PointT p_src (source_points->points[correspondences[i].index_query]);
+ PointT p_tgt (target_points->points[correspondences[i].index_match]);
+
+ p_src.getVector3fMap () = source_transformation * p_src.getVector3fMap ();
+ p_tgt.getVector3fMap () = target_transformation * p_tgt.getVector3fMap ();
int id1 = j * 2 + 0, id2 = j * 2 + 1;
// Set the points
tc[0] = 0.; line_tcoords->SetTuple (id1, tc);
tc[0] = 1.; line_tcoords->SetTuple (id2, tc);
- getRandomColors (rgb);
- colors[idx+0] = rgb.r;
- colors[idx+1] = rgb.g;
- colors[idx+2] = rgb.b;
+ float rgb[3];
+ rgb[0] = vtkMath::Random (32, 255); // min / max
+ rgb[1] = vtkMath::Random (32, 255);
+ rgb[2] = vtkMath::Random (32, 255);
+ line_colors->InsertTuple (i, rgb);
}
line_colors->SetNumberOfTuples (j);
line_cells_id->SetNumberOfTuples (j);
line_data->GetCellData ()->SetScalars (line_colors);
// Create an Actor
- vtkSmartPointer<vtkLODActor> actor;
- createActorFromVTKDataSet (line_data, actor);
- actor->GetProperty ()->SetRepresentationToWireframe ();
- actor->GetProperty ()->SetOpacity (0.5);
- addActorToRenderer (actor, viewport);
+ if (!overwrite)
+ {
+ vtkSmartPointer<vtkLODActor> actor;
+ createActorFromVTKDataSet (line_data, actor);
+ actor->GetProperty ()->SetRepresentationToWireframe ();
+ actor->GetProperty ()->SetOpacity (0.5);
+ addActorToRenderer (actor, viewport);
- // Save the pointer/ID pair to the global actor map
- (*shape_actor_map_)[id] = actor;
+ // Save the pointer/ID pair to the global actor map
+ (*shape_actor_map_)[id] = actor;
+ }
+ else
+ {
+ vtkSmartPointer<vtkLODActor> actor = vtkLODActor::SafeDownCast (am_it->second);
+ if (!actor)
+ return (false);
+ // Update the mapper
+#if VTK_MAJOR_VERSION < 6
+ reinterpret_cast<vtkPolyDataMapper*>(actor->GetMapper ())->SetInput (line_data);
+#else
+ reinterpret_cast<vtkPolyDataMapper*> (actor->GetMapper ())->SetInputData (line_data);
+#endif
+ }
return (true);
}
const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
const pcl::Correspondences &correspondences,
int nth,
- const std::string &id)
+ const std::string &id,
+ int viewport)
{
- if (correspondences.empty ())
- {
- PCL_DEBUG ("[updateCorrespondences] An empty set of correspondences given! Nothing to display.\n");
- return (false);
- }
-
- // Check to see if this ID entry already exists (has it been already added to the visualizer?)
- ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
- if (am_it == shape_actor_map_->end ())
- return (false);
-
- vtkSmartPointer<vtkLODActor> actor = vtkLODActor::SafeDownCast (am_it->second);
- vtkSmartPointer<vtkPolyData> line_data = reinterpret_cast<vtkPolyDataMapper*>(actor->GetMapper ())->GetInput ();
-
- int n_corr = int (correspondences.size () / nth + 1);
-
- // Prepare colors
- vtkSmartPointer<vtkUnsignedCharArray> line_colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
- line_colors->SetNumberOfComponents (3);
- line_colors->SetName ("Colors");
- line_colors->SetNumberOfTuples (n_corr);
- unsigned char* colors = line_colors->GetPointer (0);
- memset (colors, 0, line_colors->GetNumberOfTuples () * line_colors->GetNumberOfComponents ());
- pcl::RGB rgb;
- // Will use random colors or RED by default
- rgb.r = 255.0; rgb.g = rgb.b = 0.0;
-
- // Prepare coordinates
- vtkSmartPointer<vtkPoints> line_points = vtkSmartPointer<vtkPoints>::New ();
- line_points->SetNumberOfPoints (2 * n_corr);
-
- vtkSmartPointer<vtkIdTypeArray> line_cells_id = vtkSmartPointer<vtkIdTypeArray>::New ();
- line_cells_id->SetNumberOfComponents (3);
- line_cells_id->SetNumberOfTuples (n_corr);
- vtkIdType *line_cell_id = line_cells_id->GetPointer (0);
- vtkSmartPointer<vtkCellArray> line_cells = line_data->GetLines ();
-
- vtkSmartPointer<vtkFloatArray> line_tcoords = vtkSmartPointer<vtkFloatArray>::New ();
- line_tcoords->SetNumberOfComponents (1);
- line_tcoords->SetNumberOfTuples (n_corr * 2);
- line_tcoords->SetName ("Texture Coordinates");
-
- double tc[3] = {0.0, 0.0, 0.0};
-
- int j = 0, idx = 0;
- // Draw lines between the best corresponding points
- for (size_t i = 0; i < correspondences.size (); i += nth, idx = j * 3, ++j)
- {
- const PointT &p_src = source_points->points[correspondences[i].index_query];
- const PointT &p_tgt = target_points->points[correspondences[i].index_match];
-
- int id1 = j * 2 + 0, id2 = j * 2 + 1;
- // Set the points
- line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z);
- line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z);
- // Set the cell ID
- *line_cell_id++ = 2;
- *line_cell_id++ = id1;
- *line_cell_id++ = id2;
- // Set the texture coords
- tc[0] = 0.; line_tcoords->SetTuple (id1, tc);
- tc[0] = 1.; line_tcoords->SetTuple (id2, tc);
-
- getRandomColors (rgb);
- colors[idx+0] = rgb.r;
- colors[idx+1] = rgb.g;
- colors[idx+2] = rgb.b;
- }
- line_colors->SetNumberOfTuples (j);
- line_cells_id->SetNumberOfTuples (j);
- line_cells->SetCells (n_corr, line_cells_id);
- line_points->SetNumberOfPoints (j*2);
- line_tcoords->SetNumberOfTuples (j*2);
-
- // Fill in the lines
- line_data->SetPoints (line_points);
- line_data->SetLines (line_cells);
- line_data->GetPointData ()->SetTCoords (line_tcoords);
- line_data->GetCellData ()->SetScalars (line_colors);
-
- // Update the mapper
-#if VTK_MAJOR_VERSION < 6
- reinterpret_cast<vtkPolyDataMapper*>(actor->GetMapper ())->SetInput (line_data);
-#else
- reinterpret_cast<vtkPolyDataMapper*> (actor->GetMapper ())->SetInputData (line_data);
-#endif
-
- return (true);
+ return (addCorrespondences<PointT> (source_points, target_points, correspondences, nth, id, viewport, true));
}
//////////////////////////////////////////////////////////////////////////////////////////////
if (vertices.empty () || cloud->points.empty ())
return (false);
- CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
- if (am_it != cloud_actor_map_->end ())
+ if (contains (id))
{
- pcl::console::print_warn (stderr,
- "[addPolygonMesh] A shape with id <%s> already exists! Please choose a different id and retry.\n",
- id.c_str ());
+ PCL_WARN ("[addPolygonMesh] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
return (false);
}
vtkSmartPointer<vtkPolyData> polydata = static_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
if (!polydata)
return (false);
- vtkSmartPointer<vtkCellArray> cells = polydata->GetStrips ();
+ vtkSmartPointer<vtkCellArray> cells = polydata->GetPolys ();
if (!cells)
return (false);
vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
// Update colors
vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
+ if (!colors)
+ return (false);
int rgb_idx = -1;
std::vector<pcl::PCLPointField> fields;
rgb_idx = pcl::getFieldIndex (*cloud, "rgb", fields);
#ifndef PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_
#define PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_
+#include <set>
+#include <map>
+
#include <pcl/pcl_macros.h>
+#include <pcl/common/colors.h>
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
colors[cp * 3 + 1] = static_cast<unsigned char> (g_);
colors[cp * 3 + 2] = static_cast<unsigned char> (b_);
}
- reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0);
+ reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
return (true);
}
colors[cp * 3 + 1] = static_cast<unsigned char> (g_);
colors[cp * 3 + 2] = static_cast<unsigned char> (b_);
}
- reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0);
+ reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
return (true);
}
{
if (!capable_ || !cloud_)
return (false);
+
+ // Get the RGB field index
+ std::vector<pcl::PCLPointField> fields;
+ int rgba_index = -1;
+ rgba_index = pcl::getFieldIndex (*cloud_, "rgb", fields);
+ if (rgba_index == -1)
+ rgba_index = pcl::getFieldIndex (*cloud_, "rgba", fields);
+
+ int rgba_offset = fields[rgba_index].offset;
if (!scalars)
scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
if (fields_[d].name == "x")
x_idx = static_cast<int> (d);
+ pcl::RGB rgb;
if (x_idx != -1)
{
// Color every point
!pcl_isfinite (cloud_->points[cp].z))
continue;
- colors[j ] = cloud_->points[cp].r;
- colors[j + 1] = cloud_->points[cp].g;
- colors[j + 2] = cloud_->points[cp].b;
+ memcpy (&rgb, (reinterpret_cast<const char *> (&cloud_->points[cp])) + rgba_offset, sizeof (pcl::RGB));
+ colors[j ] = rgb.r;
+ colors[j + 1] = rgb.g;
+ colors[j + 2] = rgb.b;
j += 3;
}
}
for (vtkIdType cp = 0; cp < nr_points; ++cp)
{
int idx = static_cast<int> (cp) * 3;
- colors[idx ] = cloud_->points[cp].r;
- colors[idx + 1] = cloud_->points[cp].g;
- colors[idx + 2] = cloud_->points[cp].b;
+ memcpy (&rgb, (reinterpret_cast<const char *> (&cloud_->points[cp])) + rgba_offset, sizeof (pcl::RGB));
+ colors[idx ] = rgb.r;
+ colors[idx + 1] = rgb.g;
+ colors[idx + 2] = rgb.b;
}
}
return (true);
j++;
}
}
- reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetArray (colors, j, 0);
+ reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetArray (colors, j, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
return (true);
}
return (true);
}
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::visualization::PointCloudColorHandlerLabelField<PointT>::setInputCloud (const PointCloudConstPtr &cloud)
+{
+ PointCloudColorHandler<PointT>::setInputCloud (cloud);
+ field_idx_ = pcl::getFieldIndex (*cloud, "label", fields_);
+ if (field_idx_ != -1)
+ {
+ capable_ = true;
+ return;
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> bool
+pcl::visualization::PointCloudColorHandlerLabelField<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
+{
+ if (!capable_ || !cloud_)
+ return (false);
+
+ if (!scalars)
+ scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+ scalars->SetNumberOfComponents (3);
+
+ vtkIdType nr_points = cloud_->points.size ();
+ reinterpret_cast<vtkUnsignedCharArray*> (&(*scalars))->SetNumberOfTuples (nr_points);
+ unsigned char* colors = reinterpret_cast<vtkUnsignedCharArray*> (&(*scalars))->GetPointer (0);
+
+
+ std::map<uint32_t, pcl::RGB> colormap;
+ if (!static_mapping_)
+ {
+ std::set<uint32_t> labels;
+ // First pass: find unique labels
+ for (vtkIdType i = 0; i < nr_points; ++i)
+ labels.insert (cloud_->points[i].label);
+
+ // Assign Glasbey colors in ascending order of labels
+ size_t color = 0;
+ for (std::set<uint32_t>::iterator iter = labels.begin (); iter != labels.end (); ++iter, ++color)
+ colormap[*iter] = GlasbeyLUT::at (color % GlasbeyLUT::size ());
+ }
+
+ int j = 0;
+ for (vtkIdType cp = 0; cp < nr_points; ++cp)
+ {
+ if (pcl::isFinite (cloud_->points[cp]))
+ {
+ const pcl::RGB& color = static_mapping_ ? GlasbeyLUT::at (cloud_->points[cp].label % GlasbeyLUT::size ()) : colormap[cloud_->points[cp].label];
+ colors[j ] = color.r;
+ colors[j + 1] = color.g;
+ colors[j + 2] = color.b;
+ j += 3;
+ }
+ }
+
+ return (true);
+}
+
#endif // PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_
#include <pcl/visualization/mouse_event.h>
#include <pcl/visualization/point_picking_event.h>
#include <pcl/visualization/area_picking_event.h>
+#ifndef Q_MOC_RUN
#include <boost/signals2/signal.hpp>
-
+#endif
#include <vtkInteractorStyleRubberBandPick.h>
class vtkRendererCollection;
/** \brief Empty constructor. */
PCLVisualizerInteractorStyle () :
- init_ (), rens_ (), actors_ (), win_height_ (), win_width_ (), win_pos_x_ (), win_pos_y_ (),
- max_win_height_ (), max_win_width_ (), grid_enabled_ (), grid_actor_ (), lut_enabled_ (),
+ init_ (), rens_ (), cloud_actors_ (), shape_actors_ (), win_height_ (), win_width_ (), win_pos_x_ (), win_pos_y_ (),
+ max_win_height_ (), max_win_width_ (), use_vbos_ (false), grid_enabled_ (), grid_actor_ (), lut_enabled_ (),
lut_actor_ (), snapshot_writer_ (), wif_ (), mouse_signal_ (), keyboard_signal_ (),
point_picking_signal_ (), area_picking_signal_ (), stereo_anaglyph_mask_default_ (),
- mouse_callback_ (), modifier_ (), camera_file_ (), camera_ (), camera_saved_ (), win_ ()
+ mouse_callback_ (), modifier_ (), camera_file_ (), camera_ (), camera_saved_ (), win_ (), lut_actor_id_ ("")
{}
/** \brief Empty destructor */
virtual void
Initialize ();
- /** \brief Pass a pointer to the actor map
+ /** \brief Pass a pointer to the cloud actor map
+ * \param[in] actors the actor map that will be used with this style
+ */
+ inline void
+ setCloudActorMap (const CloudActorMapPtr &actors) { cloud_actors_ = actors; }
+
+ /** \brief Pass a pointer to the shape actor map
* \param[in] actors the actor map that will be used with this style
*/
inline void
- setCloudActorMap (const CloudActorMapPtr &actors) { actors_ = actors; }
+ setShapeActorMap (const ShapeActorMapPtr &actors) { shape_actors_ = actors; }
/** \brief Get the cloud actor map pointer. */
inline CloudActorMapPtr
- getCloudActorMap () { return (actors_); }
+ getCloudActorMap () { return (cloud_actors_); }
+
+ /** \brief Get the cloud actor map pointer. */
+ inline ShapeActorMapPtr
+ getShapeActorMap () { return (shape_actors_); }
/** \brief Pass a set of renderers to the interactor style.
* \param[in] rens the vtkRendererCollection to use
void
setRendererCollection (vtkSmartPointer<vtkRendererCollection> &rens) { rens_ = rens; }
- /** \brief Pass a pointer to the actor map
- * \param[in] use_vbos
+ /** \brief Use Vertex Buffer Objects renderers.
+ * This is an optimization for the obsolete OpenGL backend. Modern OpenGL2 backend (VTK ≥ 6.3) uses vertex
+ * buffer objects by default, transparently for the user.
+ * \param[in] use_vbos set to true to use VBOs
*/
inline void
setUseVbos (const bool use_vbos) { use_vbos_ = use_vbos; }
/** \brief Collection of vtkRenderers stored internally. */
vtkSmartPointer<vtkRendererCollection> rens_;
- /** \brief Actor map stored internally. */
- CloudActorMapPtr actors_;
+ /** \brief Cloud actor map stored internally. */
+ CloudActorMapPtr cloud_actors_;
+
+ /** \brief Shape map stored internally. */
+ ShapeActorMapPtr shape_actors_;
/** \brief The current window width/height. */
int win_height_, win_width_;
/** \brief The maximum resizeable window width/height. */
int max_win_height_, max_win_width_;
- /** \brief The maximum resizeable window width/height. */
+ /** \brief Boolean that holds whether or not to use the vtkVertexBufferObjectMapper*/
bool use_vbos_;
/** \brief Set to true if the grid actor is enabled. */
friend class PointPickingCallback;
friend class PCLVisualizer;
+
+ private:
+ /** \brief ID used to fetch/display the look up table on the visualizer
+ * It should be set by PCLVisualizer \ref setLookUpTableID
+ * @note If empty, a random actor added to the interactor will be used */
+ std::string lut_actor_id_;
+
+ /** \brief Add/remove the look up table displayed when 'u' is pressed, can also be used to update the current LUT displayed
+ * \ref lut_actor_id_ is used (if not empty) to chose which cloud/shape actor LUT will be updated (depending on what is available)
+ * If \ref lut_actor_id_ is empty the first actor with LUT support found will be used. */
+ void
+ updateLookUpTableDisplay (bool add_lut = false);
};
/** \brief PCL histogram visualizer interactory style class.
* \param[in] data data who's frequency distribution is to be found
* \param[in] nbins number of bins for the histogram
* \param[out] histogram vector of pairs containing the histogram. The first field of the pair represent the middle value of the corresponding bin. The second field denotes the frequency of data in that bin.
+ * \note NaN values will be ignored!
*/
void
computeHistogram (std::vector<double> const & data, int const nbins, std::vector<std::pair<double, double> > &histogram);
bool
removeAllShapes (int viewport = 0);
+ /** \brief Removes all existing 3D axes (coordinate systems)
+ * \param[in] viewport view port where the 3D axes should be removed from (default: all)
+ */
+ bool
+ removeAllCoordinateSystems (int viewport = 0);
+
/** \brief Set the viewport's background color.
* \param[in] r the red component of the RGB color
* \param[in] g the green component of the RGB color
double r = 1.0, double g = 1.0, double b = 1.0,
const std::string &id = "", int viewport = 0);
+ /** \brief Check if the cloud, shape, or coordinate with the given id was already added to this vizualizer.
+ * \param[in] id the id of the cloud, shape, or coordinate to check
+ * \return true if a cloud, shape, or coordinate with the specified id was found
+ */
+ inline bool
+ contains(const std::string &id) const
+ {
+ return (cloud_actor_map_->find (id) != cloud_actor_map_->end () ||
+ shape_actor_map_->find (id) != shape_actor_map_->end () ||
+ coordinate_actor_map_->find (id) != coordinate_actor_map_-> end());
+ }
+
/** \brief Add the estimated surface normals of a Point Cloud to screen.
* \param[in] cloud the input point cloud dataset containing XYZ data and normals
* \param[in] level display only every level'th point (default: 100)
int level = 100, float scale = 0.02f,
const std::string &id = "cloud", int viewport = 0);
+ /** \brief Add the estimated principal curvatures of a Point Cloud to screen.
+ * \param[in] cloud the input point cloud dataset containing the XYZ data and normals
+ * \param[in] pcs the input point cloud dataset containing the principal curvatures data
+ * \param[in] level display only every level'th point. Default: 100
+ * \param[in] scale the normal arrow scale. Default: 1.0
+ * \param[in] id the point cloud object id. Default: "cloud"
+ * \param[in] viewport the view port where the Point Cloud should be added (default: all)
+ */
+ template <typename PointNT> bool
+ addPointCloudPrincipalCurvatures (
+ const typename pcl::PointCloud<PointNT>::ConstPtr &cloud,
+ const typename pcl::PointCloud<pcl::PrincipalCurvatures>::ConstPtr &pcs,
+ int level = 100, float scale = 1.0f,
+ const std::string &id = "cloud", int viewport = 0);
+
/** \brief Add the estimated principal curvatures of a Point Cloud to screen.
* \param[in] cloud the input point cloud dataset containing the XYZ data
* \param[in] normals the input point cloud dataset containing the normal data
* \param[in] id the point cloud object id. Default: "cloud"
* \param[in] viewport the view port where the Point Cloud should be added (default: all)
*/
- bool
+ template <typename PointT, typename PointNT> bool
addPointCloudPrincipalCurvatures (
- const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud,
- const pcl::PointCloud<pcl::Normal>::ConstPtr &normals,
+ const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
+ const typename pcl::PointCloud<PointNT>::ConstPtr &normals,
const pcl::PointCloud<pcl::PrincipalCurvatures>::ConstPtr &pcs,
int level = 100, float scale = 1.0f,
const std::string &id = "cloud", int viewport = 0);
addPointCloud (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud,
const std::string &id = "cloud", int viewport = 0)
{
- pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> color_handler (cloud);
+ pcl::visualization::PointCloudColorHandlerRGB
AField<pcl::PointXYZRGBA> color_handler (cloud);
return (addPointCloud<pcl::PointXYZRGBA> (cloud, color_handler, id, viewport));
}
+ /** \brief Add a PointXYZL Point Cloud to screen.
+ * \param[in] cloud the input point cloud dataset
+ * \param[in] id the point cloud object id (default: cloud)
+ * \param[in] viewport the view port where the Point Cloud should be added (default: all)
+ */
+ inline bool
+ addPointCloud (const pcl::PointCloud<pcl::PointXYZL>::ConstPtr &cloud,
+ const std::string &id = "cloud", int viewport = 0)
+ {
+ pcl::visualization::PointCloudColorHandlerLabelField<pcl::PointXYZL> color_handler (cloud);
+ return (addPointCloud<pcl::PointXYZL> (cloud, color_handler, id, viewport));
+ }
+
/** \brief Updates the XYZ data for an existing cloud object id on screen.
* \param[in] cloud the input point cloud dataset
* \param[in] id the point cloud object id to update (default: cloud)
updatePointCloud (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud,
const std::string &id = "cloud")
{
- pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> color_handler (cloud);
+ pcl::visualization::PointCloudColorHandlerRGB
AField<pcl::PointXYZRGBA> color_handler (cloud);
return (updatePointCloud<pcl::PointXYZRGBA> (cloud, color_handler, id));
}
+ /** \brief Updates the XYZL data for an existing cloud object id on screen.
+ * \param[in] cloud the input point cloud dataset
+ * \param[in] id the point cloud object id to update (default: cloud)
+ * \return false if no cloud with the specified ID was found
+ */
+ inline bool
+ updatePointCloud (const pcl::PointCloud<pcl::PointXYZL>::ConstPtr &cloud,
+ const std::string &id = "cloud")
+ {
+ pcl::visualization::PointCloudColorHandlerLabelField<pcl::PointXYZL> color_handler (cloud);
+ return (updatePointCloud<pcl::PointXYZL> (cloud, color_handler, id));
+ }
+
/** \brief Add a PolygonMesh object to screen
* \param[in] polymesh the polygonal mesh
* \param[in] id the polygon object id (default: "polygon")
* \param[in] nth display only the Nth correspondence (e.g., skip the rest)
* \param[in] id the polygon object id (default: "correspondences")
* \param[in] viewport the view port where the correspondences should be added (default: all)
+ * \param[in] overwrite allow to overwrite already existing correspondences
*/
template <typename PointT> bool
addCorrespondences (const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
const pcl::Correspondences &correspondences,
int nth,
const std::string &id = "correspondences",
- int viewport = 0);
+ int viewport = 0,
+ bool overwrite = false);
/** \brief Add the specified correspondences to the display.
* \param[in] source_points The source points
* \param[in] correspondences The mapping from source points to target points. Each element must be an index into target_points
* \param[in] nth display only the Nth correspondence (e.g., skip the rest)
* \param[in] id the polygon object id (default: "correspondences")
+ * \param[in] viewport the view port where the correspondences should be updated (default: all)
*/
template <typename PointT> bool
updateCorrespondences (
const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
const pcl::Correspondences &correspondences,
int nth,
- const std::string &id = "correspondences");
+ const std::string &id = "correspondences",
+ int viewport = 0);
+
+ /** \brief Update the specified correspondences to the display.
+ * \param[in] source_points The source points
+ * \param[in] target_points The target points
+ * \param[in] correspondences The mapping from source points to target points. Each element must be an index into target_points
+ * \param[in] id the polygon object id (default: "correspondences")
+ * \param[in] viewport the view port where the correspondences should be updated (default: all)
+ */
+ template <typename PointT> bool
+ updateCorrespondences (
+ const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
+ const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
+ const pcl::Correspondences &correspondences,
+ const std::string &id = "correspondences",
+ int viewport = 0)
+ {
+ // If Nth not given, display all correspondences
+ return (updateCorrespondences<PointT> (source_points, target_points,
+ correspondences, 1, id, viewport));
+ }
/** \brief Remove the specified correspondences from the display.
* \param[in] id the polygon correspondences object id (i.e., given on \ref addCorrespondences)
* \param[in] val3 the third value to be set
* \param[in] id the point cloud object id (default: cloud)
* \param[in] viewport the view port where the Point Cloud's rendering properties should be modified (default: all)
+ * \note The list of properties can be found in \ref pcl::visualization::LookUpTableRepresentationProperties.
*/
bool
setPointCloudRenderingProperties (int property, double val1, double val2, double val3,
const std::string &id = "cloud", int viewport = 0);
+ /** \brief Set the rendering properties of a PointCloud (2x values - e.g., LUT minmax values)
+ * \param[in] property the property type
+ * \param[in] val1 the first value to be set
+ * \param[in] val2 the second value to be set
+ * \param[in] id the point cloud object id (default: cloud)
+ * \param[in] viewport the view port where the Point Cloud's rendering properties should be modified (default: all)
+ * \note The list of properties can be found in \ref pcl::visualization::LookUpTableRepresentationProperties.
+ */
+ bool
+ setPointCloudRenderingProperties (int property, double val1, double val2,
+ const std::string &id = "cloud", int viewport = 0);
+
/** \brief Set the rendering properties of a PointCloud
* \param[in] property the property type
* \param[in] value the value to be set
* \param[in] id the point cloud object id (default: cloud)
* \param[in] viewport the view port where the Point Cloud's rendering properties should be modified (default: all)
+ * \note The list of properties can be found in \ref pcl::visualization::LookUpTableRepresentationProperties.
*/
bool
setPointCloudRenderingProperties (int property, double value,
* \param[in] property the property type
* \param[in] value the resultant property value
* \param[in] id the point cloud object id (default: cloud)
+ * \note The list of properties can be found in \ref pcl::visualization::LookUpTableRepresentationProperties.
*/
bool
getPointCloudRenderingProperties (int property, double &value,
* \param[in] value the value to be set
* \param[in] id the shape object id
* \param[in] viewport the view port where the shape's properties should be modified (default: all)
+ * \note When using \ref addPolygonMesh you you should use \ref setPointCloudRenderingProperties
+ * \note The list of properties can be found in \ref pcl::visualization::LookUpTableRepresentationProperties.
*/
bool
setShapeRenderingProperties (int property, double value,
* \param[in] val3 the third value to be set
* \param[in] id the shape object id
* \param[in] viewport the view port where the shape's properties should be modified (default: all)
+ * \note When using \ref addPolygonMesh you you should use \ref setPointCloudRenderingProperties
*/
bool
setShapeRenderingProperties (int property, double val1, double val2, double val3,
* \param[in] id the line id/name (default: "arrow")
* \param[in] viewport (optional) the id of the new viewport (default: 0)
*/
- template <typename P1, typename P2> bool
- addArrow (const P1 &pt1, const P2 &pt2,
- double r_line, double g_line, double b_line,
- double r_text, double g_text, double b_text,
- const std::string &id = "arrow", int viewport = 0);
+ template <typename P1, typename P2> bool
+ addArrow (const P1 &pt1, const P2 &pt2,
+ double r_line, double g_line, double b_line,
+ double r_text, double g_text, double b_text,
+ const std::string &id = "arrow", int viewport = 0);
/** \brief Add a sphere shape from a point and a radius
int viewport = 0);
/** \brief Add a cone from a set of given model coefficients
- * \param[in] coefficients the model coefficients (point_on_axis, axis_direction, radiu)
+ * \param[in] coefficients the model coefficients (see \ref pcl::visualization::createCone)
* \param[in] id the cone id/name (default: "cone")
* \param[in] viewport (optional) the id of the new viewport (default: 0)
*/
int viewport = 0);
/** \brief Add a cube from a set of given model coefficients
- * \param[in] coefficients the model coefficients (Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth)
+ * \param[in] coefficients the model coefficients (see \ref pcl::visualization::createCube)
* \param[in] id the cube id/name (default: "cube")
* \param[in] viewport (optional) the id of the new viewport (default: 0)
*/
setSize (int xw, int yw);
/** \brief Use Vertex Buffer Objects renderers.
+ * This is an optimization for the obsolete OpenGL backend. Modern OpenGL2 backend (VTK ≥ 6.3) uses vertex
+ * buffer objects by default, transparently for the user.
* \param[in] use_vbos set to true to use VBOs
*/
void
setUseVbos (bool use_vbos);
+ /** \brief Set the ID of a cloud or shape to be used for LUT display
+ * \param[in] id The id of the cloud/shape look up table to be displayed
+ * The look up table is displayed by pressing 'u' in the PCLVisualizer */
+ void
+ setLookUpTableID (const std::string id);
+
/** \brief Create the internal Interactor object. */
void
createInteractor ();
using PointCloudColorHandler<PointT>::fields_;
};
+ //////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Label field handler class for colors. Paints the points according to their
+ * labels, assigning a unique color from a predefined color lookup table to each label.
+ * \author Sergey Alexandrov
+ * \ingroup visualization
+ */
+ template <typename PointT>
+ class PointCloudColorHandlerLabelField : public PointCloudColorHandler<PointT>
+ {
+ typedef typename PointCloudColorHandler<PointT>::PointCloud PointCloud;
+ typedef typename PointCloud::Ptr PointCloudPtr;
+ typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+ public:
+ typedef boost::shared_ptr<PointCloudColorHandlerLabelField<PointT> > Ptr;
+ typedef boost::shared_ptr<const PointCloudColorHandlerLabelField<PointT> > ConstPtr;
+
+ /** \brief Constructor.
+ * \param[in] static_mapping Use a static colormapping from label_id to color (default true) */
+ PointCloudColorHandlerLabelField (const bool static_mapping = true)
+ : PointCloudColorHandler<PointT> ()
+ {
+ capable_ = false;
+ static_mapping_ = static_mapping;
+ }
+
+ /** \brief Constructor.
+ * \param[in] static_mapping Use a static colormapping from label_id to color (default true) */
+ PointCloudColorHandlerLabelField (const PointCloudConstPtr &cloud,
+ const bool static_mapping = true)
+ : PointCloudColorHandler<PointT> (cloud)
+ {
+ setInputCloud (cloud);
+ static_mapping_ = static_mapping;
+ }
+
+ /** \brief Destructor. */
+ virtual ~PointCloudColorHandlerLabelField () {}
+
+ /** \brief Get the name of the field used. */
+ virtual std::string
+ getFieldName () const { return ("label"); }
+
+ /** \brief Obtain the actual color for the input dataset as vtk scalars.
+ * \param[out] scalars the output scalars containing the color for the dataset
+ * \return true if the operation was successful (the handler is capable and
+ * the input cloud was given as a valid pointer), false otherwise
+ */
+ virtual bool
+ getColor (vtkSmartPointer<vtkDataArray> &scalars) const;
+
+ /** \brief Set the input cloud to be used.
+ * \param[in] cloud the input cloud to be used by the handler
+ */
+ virtual void
+ setInputCloud (const PointCloudConstPtr &cloud);
+
+ protected:
+ /** \brief Class getName method. */
+ virtual std::string
+ getName () const { return ("PointCloudColorHandlerLabelField"); }
+
+ private:
+ // Members derived from the base class
+ using PointCloudColorHandler<PointT>::cloud_;
+ using PointCloudColorHandler<PointT>::capable_;
+ using PointCloudColorHandler<PointT>::field_idx_;
+ using PointCloudColorHandler<PointT>::fields_;
+ bool static_mapping_;
+ };
+
//////////////////////////////////////////////////////////////////////////////////////
/** \brief Base Handler class for PointCloud colors.
* \author Radu B. Rusu
virtual std::string
getFieldName () const { return ("rgba"); }
};
+
+ //////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Label field handler class for colors. Paints the points according to their
+ * labels, assigning a unique color from a predefined color lookup table to each label.
+ * \author Sergey Alexandrov
+ * \ingroup visualization
+ */
+ template <>
+ class PCL_EXPORTS PointCloudColorHandlerLabelField<pcl::PCLPointCloud2> : public PointCloudColorHandler<pcl::PCLPointCloud2>
+ {
+ typedef PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloud PointCloud;
+ typedef PointCloud::Ptr PointCloudPtr;
+ typedef PointCloud::ConstPtr PointCloudConstPtr;
+
+ public:
+ typedef boost::shared_ptr<PointCloudColorHandlerLabelField<PointCloud> > Ptr;
+ typedef boost::shared_ptr<const PointCloudColorHandlerLabelField<PointCloud> > ConstPtr;
+
+ /** \brief Constructor.
+ * \param[in] static_mapping Use a static colormapping from label_id to color (default true) */
+ PointCloudColorHandlerLabelField (const PointCloudConstPtr &cloud,
+ const bool static_mapping = true);
+
+ /** \brief Empty destructor */
+ virtual ~PointCloudColorHandlerLabelField () {}
+
+ /** \brief Obtain the actual color for the input dataset as vtk scalars.
+ * \param[out] scalars the output scalars containing the color for the dataset
+ * \return true if the operation was successful (the handler is capable and
+ * the input cloud was given as a valid pointer), false otherwise
+ */
+ virtual bool
+ getColor (vtkSmartPointer<vtkDataArray> &scalars) const;
+
+ protected:
+ /** \brief Get the name of the class. */
+ virtual std::string
+ getName () const { return ("PointCloudColorHandlerLabelField"); }
+
+ /** \brief Get the name of the field used. */
+ virtual std::string
+ getFieldName () const { return ("label"); }
+ private:
+ bool static_mapping_;
+ };
+
}
}
#endif
#endif
+#include <vtkVersion.h>
#include <vtkAppendPolyData.h>
#include <vtkAssemblyPath.h>
#include <vtkAxesActor.h>
#endif
#include <vtkSelection.h>
-#if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>4)
+#if VTK_MAJOR_VERSION>=6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>4)
#include <vtkHardwareSelector.h>
#else
#include <vtkVisibleCellSelector.h>
void setColors (unsigned char rgb[3]) { memcpy (colors, rgb, 3 * sizeof (unsigned char)); }
void setOpacity (double opacity) { SetOpacity (opacity); };
unsigned char colors[3];
- double opacity;
std::vector<float> params;
};
#include <vtkCocoaRenderWindowInteractor.h>
#include <vtkObjectFactory.h>
+#if ((VTK_MAJOR_VERSION < 6) || ((VTK_MAJOR_VERSION == 6) && (VTK_MINOR_VERSION < 2)))
+
//----------------------------------------------------------------------------
@interface vtkCocoaServerFix : NSObject
{
{
return (vtkCocoaRenderWindowInteractorFix::New ());
}
-
+#else
+//----------------------------------------------------------------------------
+vtkRenderWindowInteractor* vtkRenderWindowInteractorFixNew ()
+{
+ return (vtkCocoaRenderWindowInteractor::New ());
+}
+#endif
#include <pcl/point_types.h>
#include <pcl/visualization/common/common.h>
+#include <pcl/console/print.h>
#include <stdlib.h>
//////////////////////////////////////////////////////////////////////////////////////////////
return (fabsf (float (sum * 0.5f)));
}
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::visualization::getColormapLUT (LookUpTableRepresentationProperties colormap_type, vtkSmartPointer<vtkLookupTable> &table)
+{
+ table = vtkSmartPointer<vtkLookupTable>::New ();
+ switch (colormap_type)
+ {
+ case PCL_VISUALIZER_LUT_JET:
+ {
+ table->SetHueRange (0, 0.667);
+ table->SetSaturationRange (1, 1);
+ table->SetAlphaRange (1, 1);
+ break;
+ }
+
+ case PCL_VISUALIZER_LUT_JET_INVERSE:
+ {
+ table->SetHueRange (0.667, 0);
+ table->SetSaturationRange (1, 1);
+ table->SetAlphaRange (1, 1);
+ break;
+ }
+
+ case PCL_VISUALIZER_LUT_HSV:
+ {
+ table->SetHueRange (0, 1);
+ table->SetSaturationRange (1, 1);
+ table->SetAlphaRange (1, 1);
+ break;
+ }
+
+ case PCL_VISUALIZER_LUT_HSV_INVERSE:
+ {
+ table->SetHueRange (1, 0);
+ table->SetSaturationRange (1, 1);
+ table->SetAlphaRange (1, 1);
+ break;
+ }
+
+ case PCL_VISUALIZER_LUT_GREY:
+ {
+ table->SetValueRange (0, 1);
+ table->SetHueRange (0, 0);
+ table->SetSaturationRange (0, 0);
+ table->SetAlphaRange (1, 1);
+ break;
+ }
+
+ case PCL_VISUALIZER_LUT_BLUE2RED:
+ {
+ table->SetSaturationRange (1, 1);
+ table->SetAlphaRange (1, 1);
+ table->SetNumberOfTableValues (256);
+
+ double red[3] = {1.0, 0.0, 0.0};
+ double white[3] = {1.0, 1.0, 1.0};
+ double blue[3] = {0.0, 0.0, 1.0};
+
+ for (size_t i = 0; i < 128; i++)
+ {
+ double weight = static_cast<double>(i) / 128.0;
+ table->SetTableValue ( i,
+ white[0] * weight + blue[0] * (1 - weight),
+ white[1] * weight + blue[1] * (1 - weight),
+ white[2] * weight + blue[2] * (1 - weight) );
+ }
+
+ for (size_t i = 128; i < 256; i++)
+ {
+ double weight = (static_cast<double>(i) -128.0) / 128.0;
+ table->SetTableValue ( i,
+ red[0] * weight + white[0] * (1 - weight),
+ red[1] * weight + white[1] * (1 - weight),
+ red[2] * weight + white[2] * (1 - weight) );
+ }
+ break;
+ }
+ default:
+ PCL_WARN ("[pcl::visualization::getColormapLUT] Requested colormap type does not exist!\n");
+ return false;
+ }
+ table->Build ();
+ return true;
+}
+
/////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::visualization::Camera::computeViewMatrix (Eigen::Matrix4d &view_mat) const
#if VTK_MAJOR_VERSION < 6
map->SetInput (empty_image);
#else
+ #if ((VTK_MAJOR_VERSION == 6) && (VTK_MINOR_VERSION == 1))
+ empty_image->AllocateScalars (VTK_UNSIGNED_CHAR, 3);
+ algo_->SetInputData (empty_image);
+ map->SetInputConnection (algo_->GetOutputPort ());
+ #else
map->SetInputData (empty_image);
+ #endif
#endif
slice_->SetMapper (map);
ren_->AddViewProp (slice_);
+ ren_->GetActiveCamera ()->ParallelProjectionOn ();
interactor_->SetInteractorStyle (interactor_style_);
#endif
#else
algo_->SetInputData (image);
algo_->Update ();
+ slice_->GetMapper ()->SetInputConnection (algo_->GetOutputPort ());
+ ren_->ResetCamera ();
+ ren_->GetActiveCamera ()->SetParallelScale (0.5 * win_->GetSize ()[1]);
#endif
}
#else
algo_->SetInputData (image);
algo_->Update ();
+ slice_->GetMapper ()->SetInputConnection (algo_->GetOutputPort ());
+ ren_->ResetCamera ();
+ ren_->GetActiveCamera ()->SetParallelScale (0.5 * win_->GetSize ()[1]);
#endif
}
static_cast<unsigned char> (255.0 * g),
static_cast<unsigned char> (255.0 * b));
circle->setOpacity (opacity);
-#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 10))
+#if ((VTK_MAJOR_VERSION >= 6) || ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 7)))
circle->set (static_cast<float> (x), static_cast<float> (y), static_cast<float> (radius));
#else
circle->set (static_cast<float> (x), static_cast<float> (getSize ()[1] - y), static_cast<float> (radius));
static_cast<unsigned char> (255.0 * g),
static_cast<unsigned char> (255.0 * b));
rect->setOpacity (opacity);
-#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 10))
+#if ((VTK_MAJOR_VERSION >= 6) || (VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 7))
rect->set (static_cast<float> (x_min), static_cast<float> (y_min),
static_cast<float> (x_max - x_min), static_cast<float> (y_max - y_min));
#else
static_cast<unsigned char> (255.0 * g),
static_cast<unsigned char> (255.0 * b));
rect->setOpacity (opacity);
-#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 7))
+#if ((VTK_MAJOR_VERSION >= 6) || ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 7)))
rect->set (static_cast<float> (x_min), static_cast<float> (y_min),
static_cast<float> (x_max), static_cast<float> (y_max));
#else
static_cast<unsigned char> (255.0 * g),
static_cast<unsigned char> (255.0 * b));
rect->setOpacity (opacity);
-#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 7))
+#if ((VTK_MAJOR_VERSION >= 6) ||((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 7)))
rect->set (min_pt.x, min_pt.y, max_pt.x, max_pt.y);
#else
rect->set (min_pt.x, static_cast<float> (getSize ()[1]) - min_pt.y, max_pt.x, max_pt.y);
static_cast<unsigned char> (255.0 * g),
static_cast<unsigned char> (255.0 * b));
line->setOpacity (opacity);
-#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 10))
+#if ((VTK_MAJOR_VERSION >= 6) || ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 7)))
line->set (static_cast<float> (x_min), static_cast<float> (y_min),
static_cast<float> (x_max), static_cast<float> (y_max));
#else
static_cast<unsigned char> (255.0 * g),
static_cast<unsigned char> (255.0 * b));
text->setOpacity (opacity);
-#if ((VTK_MAJOR_VERSION == 5) && (VTKOR_VERSION > 10))
+#if ((VTK_MAJOR_VERSION >= 6) || ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 7)))
text->set (static_cast<float> (x), static_cast<float> (y), text_string);
#else
text->set (static_cast<float> (x), static_cast<float> (getSize ()[1] - y), text_string);
disk->setColors (bg_color[0], bg_color[1], bg_color[2]);
disk->setOpacity (opacity);
-#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 10))
+#if ((VTK_MAJOR_VERSION >= 6) || ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 7)))
point->set (static_cast<float> (u), static_cast<float> (v));
disk->set (static_cast<float> (u), static_cast<float> (v), static_cast<float> (radius));
#else
vtkSmartPointer<context_items::Markers> markers = vtkSmartPointer<context_items::Markers>::New ();
markers->setOpacity (opacity);
-#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 10))
+#if ((VTK_MAJOR_VERSION >= 6) || ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 7)))
markers->set (uv);
#else
// translate v which is on odd indices
#include <vtkPointPicker.h>
#include <vtkAreaPicker.h>
+#if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
#include <pcl/visualization/vtk/vtkVertexBufferObjectMapper.h>
+#endif
#define ORIENT_MODE 0
#define SELECT_MODE 1
// Create the image filter and PNG writer objects
wif_ = vtkSmartPointer<vtkWindowToImageFilter>::New ();
+ wif_->ReadFrontBufferOff ();
snapshot_writer_ = vtkSmartPointer<vtkPNGWriter>::New ();
snapshot_writer_->SetInputConnection (wif_->GetOutputPort ());
// Geometry ?
if (keymod)
{
- for (it = actors_->begin (); it != actors_->end (); ++it)
+ for (it = cloud_actors_->begin (); it != cloud_actors_->end (); ++it)
{
CloudActor *act = &(*it).second;
if (index >= static_cast<int> (act->geometry_handlers.size ()))
data->SetPoints (points);
data->SetVerts (vertices);
// Modify the mapper
+#if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
if (use_vbos_)
{
vtkVertexBufferObjectMapper* mapper = static_cast<vtkVertexBufferObjectMapper*>(act->actor->GetMapper ());
act->actor->SetMapper (mapper);
}
else
+#endif
{
vtkPolyDataMapper* mapper = static_cast<vtkPolyDataMapper*>(act->actor->GetMapper ());
#if VTK_MAJOR_VERSION < 6
}
else
{
- for (it = actors_->begin (); it != actors_->end (); ++it)
+ for (it = cloud_actors_->begin (); it != cloud_actors_->end (); ++it)
{
CloudActor *act = &(*it).second;
// Check for out of bounds
vtkPolyData *data = static_cast<vtkPolyData*>(act->actor->GetMapper ()->GetInput ());
data->GetPointData ()->SetScalars (scalars);
// Modify the mapper
+#if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
if (use_vbos_)
{
vtkVertexBufferObjectMapper* mapper = static_cast<vtkVertexBufferObjectMapper*>(act->actor->GetMapper ());
act->actor->SetMapper (mapper);
}
else
+#endif
{
vtkPolyDataMapper* mapper = static_cast<vtkPolyDataMapper*>(act->actor->GetMapper ());
mapper->SetScalarRange (minmax);
case 'l': case 'L':
{
// Iterate over the entire actors list and extract the geomotry/color handlers list
- for (CloudActorMap::iterator it = actors_->begin (); it != actors_->end (); ++it)
+ for (CloudActorMap::iterator it = cloud_actors_->begin (); it != cloud_actors_->end (); ++it)
{
std::list<std::string> geometry_handlers_list, color_handlers_list;
CloudActor *act = &(*it).second;
}
break;
}
+
+ // Switch representation to wireframe (override default behavior)
+ case 'w': case 'W':
+ {
+ vtkSmartPointer<vtkActorCollection> ac = CurrentRenderer->GetActors ();
+ vtkCollectionSimpleIterator ait;
+ for (ac->InitTraversal (ait); vtkActor* actor = ac->GetNextActor (ait); )
+ {
+ for (actor->InitPathTraversal (); vtkAssemblyPath* path = actor->GetNextPath (); )
+ {
+ vtkSmartPointer<vtkActor> apart = reinterpret_cast <vtkActor*> (path->GetLastNode ()->GetViewProp ());
+ apart->GetProperty ()->SetRepresentationToWireframe ();
+ apart->GetProperty ()->SetLighting (false);
+ }
+ }
+ break;
+ }
+
// Save a PNG snapshot with the current screen
case 'j': case 'J':
{
Interactor->Render ();
}
else
- Superclass::OnKeyDown ();
+ {
+ Superclass::OnKeyDown();
+ vtkSmartPointer<vtkActorCollection> ac = CurrentRenderer->GetActors();
+ vtkCollectionSimpleIterator ait;
+ for (ac->InitTraversal(ait); vtkActor* actor = ac->GetNextActor(ait);)
+ {
+ for (actor->InitPathTraversal(); vtkAssemblyPath* path = actor->GetNextPath();)
+ {
+ vtkSmartPointer<vtkActor> apart = reinterpret_cast<vtkActor*>(path->GetLastNode()->GetViewProp());
+ apart->GetProperty()->SetRepresentationToSurface();
+ apart->GetProperty()->SetLighting(true);
+ }
+ }
+ }
break;
}
// Display a LUT actor on screen
case 'u': case 'U':
{
- CloudActorMap::iterator it;
- for (it = actors_->begin (); it != actors_->end (); ++it)
- {
- CloudActor *act = &(*it).second;
-
- vtkScalarsToColors* lut = act->actor->GetMapper ()->GetLookupTable ();
- lut_actor_->SetLookupTable (lut);
- lut_actor_->Modified ();
- }
- if (!lut_enabled_)
- {
- CurrentRenderer->AddActor (lut_actor_);
- lut_actor_->SetVisibility (true);
- lut_enabled_ = true;
- }
- else
- {
- CurrentRenderer->RemoveActor (lut_actor_);
- lut_enabled_ = false;
- }
- CurrentRenderer->Render ();
+ updateLookUpTableDisplay (true);
break;
}
vtkSmartPointer<vtkCamera> cam = CurrentRenderer->GetActiveCamera ();
- static CloudActorMap::iterator it = actors_->begin ();
+ static CloudActorMap::iterator it = cloud_actors_->begin ();
// it might be that some actors don't have a valid transformation set -> we skip them to avoid a seg fault.
bool found_transformation = false;
- for (unsigned idx = 0; idx < actors_->size (); ++idx, ++it)
+ for (unsigned idx = 0; idx < cloud_actors_->size (); ++idx, ++it)
{
- if (it == actors_->end ())
- it = actors_->begin ();
+ if (it == cloud_actors_->end ())
+ it = cloud_actors_->begin ();
const CloudActor& actor = it->second;
if (actor.viewpoint_transformation_.GetPointer ())
}
// go to the next actor for the next key-press event.
- if (it != actors_->end ())
+ if (it != cloud_actors_->end ())
++it;
else
- it = actors_->begin ();
+ it = cloud_actors_->begin ();
CurrentRenderer->SetActiveCamera (cam);
CurrentRenderer->ResetCameraClippingRange ();
Interactor->Render ();
}
+//////////////////////////////////////////////////////////////////////////////////////////////
+// Update the look up table displayed when 'u' is pressed
+void
+pcl::visualization::PCLVisualizerInteractorStyle::updateLookUpTableDisplay (bool add_lut)
+{
+ CloudActorMap::iterator am_it;
+ ShapeActorMap::iterator sm_it;
+ bool actor_found = false;
+
+ if (!lut_enabled_ && !add_lut)
+ return;
+
+ if (lut_actor_id_ != "") // Search if provided actor id is in CloudActorMap or ShapeActorMap
+ {
+ am_it = cloud_actors_->find (lut_actor_id_);
+ if (am_it == cloud_actors_->end ())
+ {
+ sm_it = shape_actors_->find (lut_actor_id_);
+ if (sm_it == shape_actors_->end ())
+ {
+ PCL_WARN ("[updateLookUpTableDisplay] Could not find any actor with id <%s>!\n", lut_actor_id_.c_str ());
+ if (lut_enabled_)
+ { // Remove LUT and exit
+ CurrentRenderer->RemoveActor (lut_actor_);
+ lut_enabled_ = false;
+ }
+ return;
+ }
+
+ // ShapeActor found
+ vtkSmartPointer<vtkProp> *act = & (*sm_it).second;
+ vtkSmartPointer<vtkActor> actor = vtkActor::SafeDownCast (*act);
+ if (!actor || !actor->GetMapper ()->GetInput ()->GetPointData ()->GetScalars ())
+ {
+ PCL_WARN ("[updateLookUpTableDisplay] id <%s> does not hold any color information!\n", lut_actor_id_.c_str ());
+ if (lut_enabled_)
+ { // Remove LUT and exit
+ CurrentRenderer->RemoveActor (lut_actor_);
+ lut_enabled_ = false;
+ }
+ return;
+ }
+
+ lut_actor_->SetLookupTable (actor->GetMapper ()->GetLookupTable ());
+ lut_actor_->Modified ();
+ actor_found = true;
+ }
+ else
+ {
+ // CloudActor
+ CloudActor *act = & (*am_it).second;
+ if (!act->actor->GetMapper ()->GetLookupTable () && !act->actor->GetMapper ()->GetInput ()->GetPointData ()->GetScalars ())
+ {
+ PCL_WARN ("[updateLookUpTableDisplay] id <%s> does not hold any color information!\n", lut_actor_id_.c_str ());
+ if (lut_enabled_)
+ { // Remove LUT and exit
+ CurrentRenderer->RemoveActor (lut_actor_);
+ lut_enabled_ = false;
+ }
+ return;
+ }
+
+ vtkScalarsToColors* lut = act->actor->GetMapper ()->GetLookupTable ();
+ lut_actor_->SetLookupTable (lut);
+ lut_actor_->Modified ();
+ actor_found = true;
+ }
+ }
+ else // lut_actor_id_ == "", the user did not specify which cloud/shape LUT should be displayed
+ // Circling through all clouds/shapes and displaying first LUT found
+ {
+ for (am_it = cloud_actors_->begin (); am_it != cloud_actors_->end (); ++am_it)
+ {
+ CloudActor *act = & (*am_it).second;
+ if (!act->actor->GetMapper ()->GetLookupTable ())
+ continue;
+
+ if (!act->actor->GetMapper ()->GetInput ()->GetPointData ()->GetScalars ())
+ continue;
+
+ vtkScalarsToColors* lut = act->actor->GetMapper ()->GetLookupTable ();
+ lut_actor_->SetLookupTable (lut);
+ lut_actor_->Modified ();
+ actor_found = true;
+ break;
+ }
+
+ if (!actor_found)
+ {
+ for (sm_it = shape_actors_->begin (); sm_it != shape_actors_->end (); ++sm_it)
+ {
+ vtkSmartPointer<vtkProp> *act = & (*sm_it).second;
+ vtkSmartPointer<vtkActor> actor = vtkActor::SafeDownCast (*act);
+ if (!actor)
+ continue;
+
+ if (!actor->GetMapper ()->GetInput ()->GetPointData ()->GetScalars ()) // Check if actor has scalars
+ continue;
+ lut_actor_->SetLookupTable (actor->GetMapper ()->GetLookupTable ());
+ lut_actor_->Modified ();
+ actor_found = true;
+ break;
+ }
+ }
+ }
+
+ if ( (!actor_found && lut_enabled_) || (lut_enabled_ && add_lut)) // Remove actor
+ {
+ CurrentRenderer->RemoveActor (lut_actor_);
+ lut_enabled_ = false;
+ }
+ else if (!lut_enabled_ && add_lut && actor_found) // Add actor
+ {
+ CurrentRenderer->AddActor (lut_actor_);
+ lut_actor_->SetVisibility (true);
+ lut_enabled_ = true;
+ }
+ else if (lut_enabled_) // Update actor (if displayed)
+ {
+ CurrentRenderer->RemoveActor (lut_actor_);
+ CurrentRenderer->AddActor (lut_actor_);
+ }
+ else
+ return;
+
+ CurrentRenderer->Render ();
+ return;
+}
+
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::visualization::PCLVisualizerInteractorStyle::OnKeyUp ()
VTK_CREATE (vtkDoubleArray, varray_X);
varray_X->SetName ("X Axis");
- varray_X->SetArray (permanent_X, size, 1);
+ varray_X->SetArray (permanent_X, size, 1, vtkDoubleArray::VTK_DATA_ARRAY_DELETE);
table->AddColumn (varray_X);
VTK_CREATE (vtkDoubleArray, varray_Y);
varray_Y->SetName (name);
- varray_Y->SetArray (permanent_Y, size, 1);
+ varray_Y->SetArray (permanent_Y, size, 1, vtkDoubleArray::VTK_DATA_ARRAY_DELETE);
table->AddColumn (varray_Y);
//adding to chart
double min = data[0], max = data[0];
for (size_t i = 1; i < data.size (); i++)
{
- if (data[i] < min) min = data[i];
- if (data[i] > max) max = data[i];
+ if (pcl_isfinite (data[i]))
+ {
+ if (data[i] < min) min = data[i];
+ if (data[i] > max) max = data[i];
+ }
}
//finding the size of each bins
double size = (max - min) / nbins;
+ if (size == 0) size = 1.0;
//fill x values of each bins by bin center
for (int i = 0; i < nbins; i++)
//fill the freq for each data
for (size_t i = 0; i < data.size (); i++)
{
- int index = int (floor ((data[i] - min) / size));
- if (index == nbins) index = nbins - 1; //including right boundary
- histogram[index ].second++;
+ if (pcl_isfinite (data[i]))
+ {
+ unsigned int index = (unsigned int) (floor ((data[i] - min) / size));
+ if (index == nbins) index = nbins - 1; //including right boundary
+ histogram[index].second++;
+ }
}
}
#include <vtkMapper.h>
#include <vtkDataSetMapper.h>
-#if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>4)
+#if VTK_MAJOR_VERSION>=6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>4)
#include <vtkHardwareSelector.h>
#include <vtkSelectionNode.h>
#else
#include <vtkPointPicker.h>
#include <pcl/visualization/boost.h>
-#include <pcl/visualization/vtk/vtkVertexBufferObjectMapper.h>
#include <pcl/visualization/vtk/vtkRenderWindowInteractorFix.h>
+#if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
+#include <pcl/visualization/vtk/vtkVertexBufferObjectMapper.h>
+#endif
+
#include <vtkPolyLine.h>
#include <vtkPolyDataMapper.h>
#include <vtkRenderWindow.h>
#include <vtkPLYReader.h>
#include <vtkAxes.h>
#include <vtkTubeFilter.h>
-#include <vtkLineSource.h>
#include <vtkOrientationMarkerWidget.h>
#include <vtkAxesActor.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkAreaPicker.h>
#include <vtkXYPlotActor.h>
-#include <vtkOpenGLHardwareSupport.h>
#include <vtkOpenGLRenderWindow.h>
#include <vtkJPEGReader.h>
#include <vtkBMPReader.h>
#include <vtkPNMReader.h>
#include <vtkPNGReader.h>
#include <vtkTIFFReader.h>
+#include <vtkLookupTable.h>
+#include <vtkTextureUnitManager.h>
#include <pcl/visualization/common/shapes.h>
#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/visualization/common/common.h>
#include <pcl/common/time.h>
#include <boost/uuid/sha1.hpp>
#include <boost/filesystem.hpp>
win_->SetWindowName (name.c_str ());
// Get screen size
- int *scr_size = win_->GetScreenSize ();
+ int scr_size_x = win_->GetScreenSize ()[0];
+ int scr_size_y = win_->GetScreenSize ()[1];
// Set the window size as 1/2 of the screen size
- win_->SetSize (scr_size[0] / 2, scr_size[1] / 2);
+ win_->SetSize (scr_size_x / 2, scr_size_y / 2);
// By default, don't use vertex buffer objects
use_vbos_ = false;
style_->Initialize ();
style_->setRendererCollection (rens_);
style_->setCloudActorMap (cloud_actor_map_);
+ style_->setShapeActorMap (shape_actor_map_);
style_->UseTimersOn ();
style_->setUseVbos(use_vbos_);
update_fps_->pcl_visualizer = this;
update_fps_->decimated = false;
ren->AddActor (txt);
+ txt->SetInput("0 FPS");
// Create a RendererWindow
win_ = vtkSmartPointer<vtkRenderWindow>::New ();
style_->Initialize ();
style_->setRendererCollection (rens_);
style_->setCloudActorMap (cloud_actor_map_);
+ style_->setShapeActorMap (shape_actor_map_);
style_->UseTimersOn ();
// Get screen size
- int *scr_size = win_->GetScreenSize ();
+ int scr_size_x = win_->GetScreenSize ()[0];
+ int scr_size_y = win_->GetScreenSize ()[1];
// Set default camera parameters
initCameraParameters ();
// Set the window size as 1/2 of the screen size or the user given parameter
if (!camera_set_ && !camera_file_loaded_)
{
- win_->SetSize (scr_size[0]/2, scr_size[1]/2);
+ win_->SetSize (scr_size_x/2, scr_size_y/2);
win_->SetPosition (0, 0);
}
// Initialize and create timer, also create window
iren->Initialize ();
-#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 4))
-#else
- timer_id_ = iren->CreateRepeatingTimer (5000L);
-#endif
// Set a simple PointPicker
vtkSmartPointer<vtkPointPicker> pp = vtkSmartPointer<vtkPointPicker>::New ();
pp->SetTolerance (pp->GetTolerance () * 2);
iren->SetPicker (pp);
-
- exit_main_loop_timer_callback_ = vtkSmartPointer<ExitMainLoopTimerCallback>::New ();
- exit_main_loop_timer_callback_->pcl_visualizer = this;
- exit_main_loop_timer_callback_->right_timer_id = -1;
- iren->AddObserver (vtkCommand::TimerEvent, exit_main_loop_timer_callback_);
-
- exit_callback_ = vtkSmartPointer<ExitCallback>::New ();
- exit_callback_->pcl_visualizer = this;
- iren->AddObserver (vtkCommand::ExitEvent, exit_callback_);
-
- resetStoppedFlag ();
}
// Initialize and create timer, also create window
iren->Initialize ();
-#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 4))
-#else
- timer_id_ = iren->CreateRepeatingTimer (5000L);
-#endif
// Set a simple PointPicker
// vtkSmartPointer<vtkPointPicker> pp = vtkSmartPointer<vtkPointPicker>::New ();
// pp->SetTolerance (pp->GetTolerance () * 2);
// iren->SetPicker (pp);
-
- exit_main_loop_timer_callback_ = vtkSmartPointer<ExitMainLoopTimerCallback>::New ();
- exit_main_loop_timer_callback_->pcl_visualizer = this;
- exit_main_loop_timer_callback_->right_timer_id = -1;
- iren->AddObserver (vtkCommand::TimerEvent, exit_main_loop_timer_callback_);
-
- exit_callback_ = vtkSmartPointer<ExitCallback>::New ();
- exit_callback_->pcl_visualizer = this;
- iren->AddObserver (vtkCommand::ExitEvent, exit_callback_);
-
- resetStoppedFlag ();
}
/////////////////////////////////////////////////////////////////////////////////////////////
pcl::visualization::PCLVisualizer::spinOnce (int time, bool force_redraw)
{
resetStoppedFlag ();
+ #if (defined (__APPLE__)\
+ && ( (VTK_MAJOR_VERSION > 6) || ( (VTK_MAJOR_VERSION == 6) && (VTK_MINOR_VERSION >= 1))))
+ if (!win_->IsDrawable ())
+ {
+ close ();
+ return;
+ }
+ #endif
if (time <= 0)
time = 1;
{
if (removeActorFromRenderer (am_it->second, viewport))
{
+ bool update_LUT (true);
+ if (!style_->lut_actor_id_.empty() && am_it->first != style_->lut_actor_id_)
+ update_LUT = false;
shape_actor_map_->erase (am_it);
+ if (update_LUT)
+ style_->updateLookUpTableDisplay (false);
return (true);
}
}
{
if (removeActorFromRenderer (ca_it->second.actor, viewport))
{
+ bool update_LUT (true);
+ if (!style_->lut_actor_id_.empty() && ca_it->first != style_->lut_actor_id_)
+ update_LUT = false;
cloud_actor_map_->erase (ca_it);
+ if (update_LUT)
+ style_->updateLookUpTableDisplay (false);
return (true);
}
}
bool
pcl::visualization::PCLVisualizer::removeAllShapes (int viewport)
{
+ bool display_lut (style_->lut_enabled_);
+ style_->lut_enabled_ = false; // Temporally disable LUT to fasten shape removal
+
// Check to see if the given ID entry exists
ShapeActorMap::iterator am_it = shape_actor_map_->begin ();
while (am_it != shape_actor_map_->end ())
else
++am_it;
}
+
+ if (display_lut)
+ {
+ style_->lut_enabled_ = true;
+ style_->updateLookUpTableDisplay (true);
+ }
return (true);
}
/////////////////////////////////////////////////////////////////////////////////////////////
bool
-pcl::visualization::PCLVisualizer::addPointCloudPrincipalCurvatures (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud,
- const pcl::PointCloud<pcl::Normal>::ConstPtr &normals,
- const pcl::PointCloud<pcl::PrincipalCurvatures>::ConstPtr &pcs,
- int level, float scale,
- const std::string &id, int viewport)
+pcl::visualization::PCLVisualizer::removeAllCoordinateSystems (int viewport)
{
- if (pcs->points.size () != cloud->points.size () || normals->points.size () != cloud->points.size ())
- {
- pcl::console::print_error ("[addPointCloudPrincipalCurvatures] The number of points differs from the number of principal curvatures/normals!\n");
- return (false);
- }
- // Check to see if this ID entry already exists (has it been already added to the visualizer?)
- CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
-
- if (am_it != cloud_actor_map_->end ())
- {
- pcl::console::print_warn (stderr, "[addPointCloudPrincipalCurvatures] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
- return (false);
- }
-
- vtkSmartPointer<vtkAppendPolyData> polydata_1 = vtkSmartPointer<vtkAppendPolyData>::New ();
- vtkSmartPointer<vtkAppendPolyData> polydata_2 = vtkSmartPointer<vtkAppendPolyData>::New ();
-
- // Setup two colors - one for each line
- unsigned char green[3] = {0, 255, 0};
- unsigned char blue[3] = {0, 0, 255};
-
- // Setup the colors array
- vtkSmartPointer<vtkUnsignedCharArray> line_1_colors =vtkSmartPointer<vtkUnsignedCharArray>::New ();
- line_1_colors->SetNumberOfComponents (3);
- line_1_colors->SetName ("Colors");
- vtkSmartPointer<vtkUnsignedCharArray> line_2_colors =vtkSmartPointer<vtkUnsignedCharArray>::New ();
- line_2_colors->SetNumberOfComponents (3);
- line_2_colors->SetName ("Colors");
-
- // Create the first sets of lines
- for (size_t i = 0; i < cloud->points.size (); i+=level)
+ // Check to see if the given ID entry exists
+ CoordinateActorMap::iterator am_it = coordinate_actor_map_->begin ();
+ while (am_it != coordinate_actor_map_->end () )
{
- pcl::PointXYZ p = cloud->points[i];
- p.x += (pcs->points[i].pc1 * pcs->points[i].principal_curvature[0]) * scale;
- p.y += (pcs->points[i].pc1 * pcs->points[i].principal_curvature[1]) * scale;
- p.z += (pcs->points[i].pc1 * pcs->points[i].principal_curvature[2]) * scale;
-
- vtkSmartPointer<vtkLineSource> line_1 = vtkSmartPointer<vtkLineSource>::New ();
- line_1->SetPoint1 (cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
- line_1->SetPoint2 (p.x, p.y, p.z);
- line_1->Update ();
-#if VTK_MAJOR_VERSION < 6
- polydata_1->AddInput (line_1->GetOutput ());
-#else
- polydata_1->AddInputData (line_1->GetOutput ());
-#endif
- line_1_colors->InsertNextTupleValue (green);
- }
- polydata_1->Update ();
- vtkSmartPointer<vtkPolyData> line_1_data = polydata_1->GetOutput ();
- line_1_data->GetCellData ()->SetScalars (line_1_colors);
-
- // Create the second sets of lines
- for (size_t i = 0; i < cloud->points.size (); i += level)
- {
- Eigen::Vector3f pc (pcs->points[i].principal_curvature[0],
- pcs->points[i].principal_curvature[1],
- pcs->points[i].principal_curvature[2]);
- Eigen::Vector3f normal (normals->points[i].normal[0],
- normals->points[i].normal[1],
- normals->points[i].normal[2]);
- Eigen::Vector3f pc_c = pc.cross (normal);
-
- pcl::PointXYZ p = cloud->points[i];
- p.x += (pcs->points[i].pc2 * pc_c[0]) * scale;
- p.y += (pcs->points[i].pc2 * pc_c[1]) * scale;
- p.z += (pcs->points[i].pc2 * pc_c[2]) * scale;
-
- vtkSmartPointer<vtkLineSource> line_2 = vtkSmartPointer<vtkLineSource>::New ();
- line_2->SetPoint1 (cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
- line_2->SetPoint2 (p.x, p.y, p.z);
- line_2->Update ();
-#if VTK_MAJOR_VERSION < 6
- polydata_2->AddInput (line_2->GetOutput ());
-#else
- polydata_2->AddInputData (line_2->GetOutput ());
-#endif
-
- line_2_colors->InsertNextTupleValue (blue);
+ if (removeCoordinateSystem (am_it->first, viewport))
+ am_it = coordinate_actor_map_->begin ();
+ else
+ ++am_it;
}
- polydata_2->Update ();
- vtkSmartPointer<vtkPolyData> line_2_data = polydata_2->GetOutput ();
- line_2_data->GetCellData ()->SetScalars (line_2_colors);
-
- // Assemble the two sets of lines
- vtkSmartPointer<vtkAppendPolyData> alldata = vtkSmartPointer<vtkAppendPolyData>::New ();
-#if VTK_MAJOR_VERSION < 6
- alldata->AddInput (line_1_data);
- alldata->AddInput (line_2_data);
-#else
- alldata->AddInputData (line_1_data);
- alldata->AddInputData (line_2_data);
-#endif
-
- // Create an Actor
- vtkSmartPointer<vtkLODActor> actor;
- createActorFromVTKDataSet (alldata->GetOutput (), actor);
- actor->GetMapper ()->SetScalarModeToUseCellData ();
-
- // Add it to all renderers
- addActorToRenderer (actor, viewport);
-
- // Save the pointer/ID pair to the global actor map
- CloudActor act;
- act.actor = actor;
- (*cloud_actor_map_)[id] = act;
return (true);
}
int
getDefaultScalarInterpolationForDataSet (vtkDataSet* data)
{
- vtkPolyData* polyData = vtkPolyData::SafeDownCast (data);
+ vtkPolyData* polyData = vtkPolyData::SafeDownCast (data); // Check that polyData != NULL in case of segfault
return (polyData && polyData->GetNumberOfCells () != polyData->GetNumberOfVerts ());
}
if (!actor)
actor = vtkSmartPointer<vtkLODActor>::New ();
+#if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
if (use_vbos_)
{
vtkSmartPointer<vtkVertexBufferObjectMapper> mapper = vtkSmartPointer<vtkVertexBufferObjectMapper>::New ();
actor->SetMapper (mapper);
}
else
+#endif
{
vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
#if VTK_MAJOR_VERSION < 6
if (!actor)
actor = vtkSmartPointer<vtkActor>::New ();
+#if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
if (use_vbos_)
{
vtkSmartPointer<vtkVertexBufferObjectMapper> mapper = vtkSmartPointer<vtkVertexBufferObjectMapper>::New ();
actor->SetMapper (mapper);
}
else
+#endif
{
vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
#if VTK_MAJOR_VERSION < 6
}
// Get the actor pointer
vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second.actor);
+ if (!actor)
+ return (false);
switch (property)
{
case PCL_VISUALIZER_COLOR:
{
+ if (val1 > 1.0 || val2 > 1.0 || val3 > 1.0)
+ PCL_WARN ("[setPointCloudRenderingProperties] Colors go from 0.0 to 1.0!\n");
actor->GetProperty ()->SetColor (val1, val2, val3);
actor->GetMapper ()->ScalarVisibilityOff ();
actor->Modified ();
return (true);
}
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::visualization::PCLVisualizer::setPointCloudRenderingProperties (
+ int property, double val1, double val2, const std::string &id, int)
+{
+ // Check to see if this ID entry already exists (has it been already added to the visualizer?)
+ CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
+
+ if (am_it == cloud_actor_map_->end ())
+ {
+ pcl::console::print_error ("[setPointCloudRenderingProperties] Could not find any PointCloud datasets with id <%s>!\n", id.c_str ());
+ return (false);
+ }
+ // Get the actor pointer
+ vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second.actor);
+ if (!actor)
+ return (false);
+
+ switch (property)
+ {
+ case PCL_VISUALIZER_LUT_RANGE:
+ {
+ // Check if the mapper has scalars
+ if (!actor->GetMapper ()->GetInput ()->GetPointData ()->GetScalars ())
+ break;
+
+ // Check that scalars are not unisgned char (i.e. check if a LUT is used to colormap scalars assuming vtk ColorMode is Default)
+ if (actor->GetMapper ()->GetInput ()->GetPointData ()->GetScalars ()->IsA ("vtkUnsignedCharArray"))
+ break;
+
+ // Check that range values are correct
+ if (val1 >= val2)
+ {
+ PCL_WARN ("[setPointCloudRenderingProperties] Range max must be greater than range min!\n");
+ return (false);
+ }
+
+ // Update LUT
+ actor->GetMapper ()->GetLookupTable ()->SetRange (val1, val2);
+ actor->GetMapper()->UseLookupTableScalarRangeOn ();
+ style_->updateLookUpTableDisplay (false);
+ break;
+ }
+ default:
+ {
+ pcl::console::print_error ("[setPointCloudRenderingProperties] Unknown property (%d) specified!\n", property);
+ return (false);
+ }
+ }
+ return (true);
+}
+
/////////////////////////////////////////////////////////////////////////////////////////////
bool
pcl::visualization::PCLVisualizer::getPointCloudRenderingProperties (int property, double &value, const std::string &id)
return (false);
// Get the actor pointer
vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second.actor);
+ if (!actor)
+ return (false);
switch (property)
{
}
// Get the actor pointer
vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second.actor);
+ if (!actor)
+ return (false);
switch (property)
{
actor->Modified ();
break;
}
+ case PCL_VISUALIZER_LUT:
+ {
+ // Check if the mapper has scalars
+ if (!actor->GetMapper ()->GetInput ()->GetPointData ()->GetScalars ())
+ break;
+
+ // Check that scalars are not unisgned char (i.e. check if a LUT is used to colormap scalars assuming vtk ColorMode is Default)
+ if (actor->GetMapper ()->GetInput ()->GetPointData ()->GetScalars ()->IsA ("vtkUnsignedCharArray"))
+ break;
+
+ // Get range limits from existing LUT
+ double *range;
+ range = actor->GetMapper ()->GetLookupTable ()->GetRange ();
+
+ actor->GetMapper ()->ScalarVisibilityOn ();
+ actor->GetMapper ()->SetScalarRange (range[0], range[1]);
+ vtkSmartPointer<vtkLookupTable> table;
+ if (!pcl::visualization::getColormapLUT (static_cast<LookUpTableRepresentationProperties>(value), table))
+ break;
+ table->SetRange (range[0], range[1]);
+ actor->GetMapper ()->SetLookupTable (table);
+ style_->updateLookUpTableDisplay (false);
+ break;
+ }
+ case PCL_VISUALIZER_LUT_RANGE:
+ {
+ // Check if the mapper has scalars
+ if (!actor->GetMapper ()->GetInput ()->GetPointData ()->GetScalars ())
+ break;
+
+ // Check that scalars are not unisgned char (i.e. check if a LUT is used to colormap scalars assuming vtk ColorMode is Default)
+ if (actor->GetMapper ()->GetInput ()->GetPointData ()->GetScalars ()->IsA ("vtkUnsignedCharArray"))
+ break;
+
+ switch (int(value))
+ {
+ case PCL_VISUALIZER_LUT_RANGE_AUTO:
+ double range[2];
+ actor->GetMapper ()->GetInput ()->GetPointData ()->GetScalars ()->GetRange (range);
+ actor->GetMapper ()->GetLookupTable ()->SetRange (range[0], range[1]);
+ actor->GetMapper ()->UseLookupTableScalarRangeOn ();
+ style_->updateLookUpTableDisplay (false);
+ break;
+ }
+ break;
+ }
default:
{
pcl::console::print_error ("[setPointCloudRenderingProperties] Unknown property (%d) specified!\n", property);
}
// Get the actor pointer
vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second.actor);
+ if (!actor)
+ return (false);
if (selected)
{
}
// Get the actor pointer
vtkActor* actor = vtkActor::SafeDownCast (am_it->second);
+ if (!actor)
+ return (false);
switch (property)
{
case PCL_VISUALIZER_COLOR:
{
+ if (val1 > 1.0 || val2 > 1.0 || val3 > 1.0)
+ PCL_WARN ("[setShapeRenderingProperties] Colors go from 0.0 to 1.0!\n");
+
actor->GetMapper ()->ScalarVisibilityOff ();
actor->GetProperty ()->SetColor (val1, val2, val3);
actor->GetProperty ()->SetEdgeColor (val1, val2, val3);
actor->GetProperty ()->SetAmbient (0.8);
actor->GetProperty ()->SetDiffuse (0.8);
actor->GetProperty ()->SetSpecular (0.8);
-#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION >= 4))
- actor->GetProperty ()->SetLighting (0);
-#endif
actor->Modified ();
break;
}
}
// Get the actor pointer
vtkActor* actor = vtkActor::SafeDownCast (am_it->second);
+ if (!actor)
+ return (false);
switch (property)
{
case PCL_VISUALIZER_FONT_SIZE:
{
vtkTextActor* text_actor = vtkTextActor::SafeDownCast (am_it->second);
+ if (!text_actor)
+ return (false);
vtkSmartPointer<vtkTextProperty> tprop = text_actor->GetTextProperty ();
tprop->SetFontSize (int (value));
text_actor->Modified ();
actor->Modified ();
break;
}
+ case PCL_VISUALIZER_LUT:
+ {
+ // Check if the mapper has scalars
+ if (!actor->GetMapper ()->GetInput ()->GetPointData ()->GetScalars ())
+ break;
+
+ double range[2];
+ actor->GetMapper ()->GetInput ()->GetPointData ()->GetScalars ()->GetRange (range);
+ actor->GetMapper ()->ScalarVisibilityOn ();
+ actor->GetMapper ()->SetScalarRange (range[0], range[1]);
+ vtkSmartPointer<vtkLookupTable> table = vtkSmartPointer<vtkLookupTable>::New ();
+ getColormapLUT (static_cast<LookUpTableRepresentationProperties>(value), table);
+ table->SetRange (range[0], range[1]);
+ actor->GetMapper ()->SetLookupTable (table);
+ style_->updateLookUpTableDisplay (false);
+ break;
+ }
default:
{
pcl::console::print_error ("[setShapeRenderingProperties] Unknown property (%d) specified!\n", property);
// Set the camera field of view to about
camera_temp.fovy = 0.8575;
- int *scr_size = win_->GetScreenSize ();
- camera_temp.window_size[0] = scr_size[0] / 2;
- camera_temp.window_size[1] = scr_size[1] / 2;
+ int scr_size_x = win_->GetScreenSize ()[0];
+ int scr_size_y = win_->GetScreenSize ()[1];
+ camera_temp.window_size[0] = scr_size_x / 2;
+ camera_temp.window_size[1] = scr_size_y / 2;
setCameraParameters (camera_temp);
}
else
actor = vtkLODActor::SafeDownCast (am_it->second);
+ if (!actor)
+ return (false);
+
vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New ();
convertToVtkMatrix (pose.matrix (), matrix);
else
actor = vtkLODActor::SafeDownCast (am_it->second);
+ if (!actor)
+ return (false);
+
vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New ();
convertToVtkMatrix (pose.matrix (), matrix);
z_axis = (z_axis - pos).normalized ();
x_axis = y_axis.cross (z_axis).normalized ();
- /// TODO replace this ugly thing with matrix.block () = vector3f
- ret (0, 0) = static_cast<float> (x_axis[0]);
- ret (0, 1) = static_cast<float> (y_axis[0]);
- ret (0, 2) = static_cast<float> (z_axis[0]);
- ret (0, 3) = static_cast<float> (pos[0]);
-
- ret (1, 0) = static_cast<float> (x_axis[1]);
- ret (1, 1) = static_cast<float> (y_axis[1]);
- ret (1, 2) = static_cast<float> (z_axis[1]);
- ret (1, 3) = static_cast<float> (pos[1]);
-
- ret (2, 0) = static_cast<float> (x_axis[2]);
- ret (2, 1) = static_cast<float> (y_axis[2]);
- ret (2, 2) = static_cast<float> (z_axis[2]);
- ret (2, 3) = static_cast<float> (pos[2]);
+ ret.translation () = pos.cast<float> ();
+ ret.linear ().col (0) << x_axis.cast<float> ();
+ ret.linear ().col (1) << y_axis.cast<float> ();
+ ret.linear ().col (2) << z_axis.cast<float> ();
return ret;
}
return (false);
}
+ if (coefficients.values.size () != 7)
+ {
+ PCL_WARN ("[addCylinder] Coefficients size does not match expected size (expected 7).\n");
+ return (false);
+ }
+
vtkSmartPointer<vtkDataSet> data = createCylinder (coefficients);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
- actor->GetProperty ()->SetRepresentationToWireframe ();
- actor->GetProperty ()->SetLighting (false);
+ actor->GetProperty ()->SetRepresentationToSurface ();
addActorToRenderer (actor, viewport);
// Save the pointer/ID pair to the global actor map
return (false);
}
+ if (coefficients.values.size () != 10)
+ {
+ PCL_WARN ("[addCube] Coefficients size does not match expected size (expected 10).\n");
+ return (false);
+ }
+
vtkSmartPointer<vtkDataSet> data = createCube (coefficients);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
- actor->GetProperty ()->SetRepresentationToWireframe ();
- actor->GetProperty ()->SetLighting (false);
+ actor->GetProperty ()->SetRepresentationToSurface ();
addActorToRenderer (actor, viewport);
// Save the pointer/ID pair to the global actor map
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
- actor->GetProperty ()->SetRepresentationToWireframe ();
- actor->GetProperty ()->SetLighting (false);
+ actor->GetProperty ()->SetRepresentationToSurface ();
addActorToRenderer (actor, viewport);
// Save the pointer/ID pair to the global actor map
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
- actor->GetProperty ()->SetRepresentationToWireframe ();
- actor->GetProperty ()->SetLighting (false);
+ actor->GetProperty ()->SetRepresentationToSurface ();
actor->GetProperty ()->SetColor (r,g,b);
addActorToRenderer (actor, viewport);
return (false);
}
+ if (coefficients.values.size () != 4)
+ {
+ PCL_WARN ("[addSphere] Coefficients size does not match expected size (expected 4).\n");
+ return (false);
+ }
+
vtkSmartPointer<vtkDataSet> data = createSphere (coefficients);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
- actor->GetProperty ()->SetRepresentationToWireframe ();
- actor->GetProperty ()->SetLighting (false);
+ actor->GetProperty ()->SetRepresentationToSurface ();
addActorToRenderer (actor, viewport);
// Save the pointer/ID pair to the global actor map
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (polydata, actor);
- actor->GetProperty ()->SetRepresentationToWireframe ();
+ actor->GetProperty ()->SetRepresentationToSurface ();
addActorToRenderer (actor, viewport);
// Save the pointer/ID pair to the global actor map
// Create an Actor
vtkSmartPointer <vtkLODActor> actor;
createActorFromVTKDataSet (trans_filter->GetOutput (), actor);
- actor->GetProperty ()->SetRepresentationToWireframe ();
+ actor->GetProperty ()->SetRepresentationToSurface ();
addActorToRenderer (actor, viewport);
// Save the pointer/ID pair to the global actor map
return (true);
}
-
////////////////////////////////////////////////////////////////////////////////////////////
bool
pcl::visualization::PCLVisualizer::addModelFromPLYFile (const std::string &filename,
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (reader->GetOutput (), actor);
- actor->GetProperty ()->SetRepresentationToWireframe ();
+ actor->GetProperty ()->SetRepresentationToSurface ();
addActorToRenderer (actor, viewport);
// Save the pointer/ID pair to the global actor map
// Create an Actor
vtkSmartPointer <vtkLODActor> actor;
createActorFromVTKDataSet (trans_filter->GetOutput (), actor);
- actor->GetProperty ()->SetRepresentationToWireframe ();
+ actor->GetProperty ()->SetRepresentationToSurface ();
addActorToRenderer (actor, viewport);
// Save the pointer/ID pair to the global actor map
return (false);
}
+ if (coefficients.values.size () != 6)
+ {
+ PCL_WARN ("[addLine] Coefficients size does not match expected size (expected 6).\n");
+ return (false);
+ }
+
vtkSmartPointer<vtkDataSet> data = createLine (coefficients);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
- actor->GetProperty ()->SetRepresentationToWireframe ();
- actor->GetProperty ()->SetLighting (false);
+ actor->GetProperty ()->SetRepresentationToSurface ();
addActorToRenderer (actor, viewport);
// Save the pointer/ID pair to the global actor map
return (false);
}
+ if (coefficients.values.size () != 4)
+ {
+ PCL_WARN ("[addPlane] Coefficients size does not match expected size (expected 4).\n");
+ return (false);
+ }
+
vtkSmartPointer<vtkDataSet> data = createPlane (coefficients);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
-// actor->GetProperty ()->SetRepresentationToWireframe ();
actor->GetProperty ()->SetRepresentationToSurface ();
- actor->GetProperty ()->SetLighting (false);
addActorToRenderer (actor, viewport);
// Save the pointer/ID pair to the global actor map
return (false);
}
+ if (coefficients.values.size () != 4)
+ {
+ PCL_WARN ("[addPlane] Coefficients size does not match expected size (expected 4).\n");
+ return (false);
+ }
+
vtkSmartPointer<vtkDataSet> data = createPlane (coefficients, x, y, z);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
- actor->GetProperty ()->SetRepresentationToWireframe ();
- actor->GetProperty ()->SetLighting (false);
+ actor->GetProperty ()->SetRepresentationToSurface ();
addActorToRenderer (actor, viewport);
// Save the pointer/ID pair to the global actor map
return (false);
}
+ if (coefficients.values.size () != 3)
+ {
+ PCL_WARN ("[addCircle] Coefficients size does not match expected size (expected 3).\n");
+ return (false);
+ }
+
vtkSmartPointer<vtkDataSet> data = create2DCircle (coefficients);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
- actor->GetProperty ()->SetRepresentationToWireframe ();
- actor->GetProperty ()->SetLighting (false);
+ actor->GetProperty ()->SetRepresentationToSurface ();
addActorToRenderer (actor, viewport);
// Save the pointer/ID pair to the global actor map
return (false);
}
+ if (coefficients.values.size () != 7)
+ {
+ PCL_WARN ("[addCone] Coefficients size does not match expected size (expected 7).\n");
+ return (false);
+ }
+
vtkSmartPointer<vtkDataSet> data = createCone (coefficients);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
- actor->GetProperty ()->SetRepresentationToWireframe ();
- actor->GetProperty ()->SetLighting (false);
+ actor->GetProperty ()->SetRepresentationToSurface ();
addActorToRenderer (actor, viewport);
// Save the pointer/ID pair to the global actor map
// Retrieve the Actor
vtkTextActor* actor = vtkTextActor::SafeDownCast (am_it->second);
+ if (!actor)
+ return (false);
+
actor->SetPosition (xpos, ypos);
actor->SetInput (text.c_str ());
-
actor->Modified ();
-
return (true);
}
// Create the Actor
vtkTextActor* actor = vtkTextActor::SafeDownCast (am_it->second);
+ if (!actor)
+ return (false);
actor->SetPosition (xpos, ypos);
actor->SetInput (text.c_str ());
// Retrieve the Actor
vtkTextActor *actor = vtkTextActor::SafeDownCast (am_it->second);
+ if (!actor)
+ return (false);
actor->SetPosition (xpos, ypos);
actor->SetInput (text.c_str ());
vtkPolyData *data = static_cast<vtkPolyData*>(am_it->second.actor->GetMapper ()->GetInput ());
data->GetPointData ()->SetScalars (scalars);
// Modify the mapper
+#if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
if (use_vbos_)
{
vtkVertexBufferObjectMapper* mapper = static_cast<vtkVertexBufferObjectMapper*>(am_it->second.actor->GetMapper ());
//style_->setCloudActorMap (cloud_actor_map_);
}
else
+#endif
{
vtkPolyDataMapper* mapper = static_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ());
mapper->SetScalarRange (minmax);
poly_grid->SetPoints (poly_points);
createActorFromVTKDataSet (poly_grid, actor);
- actor->GetProperty ()->SetRepresentationToWireframe ();
+ actor->GetProperty ()->SetRepresentationToSurface ();
}
else
{
#endif
vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
- vtkOpenGLHardwareSupport* hardware = vtkOpenGLRenderWindow::SafeDownCast (win_)->GetHardwareSupport ();
- bool supported = hardware->GetSupportsMultiTexturing ();
+ vtkTextureUnitManager* tex_manager = vtkOpenGLRenderWindow::SafeDownCast (win_)->GetTextureUnitManager ();
+ if (!tex_manager)
+ return (false);
// Check if hardware support multi texture
- std::size_t texture_units (hardware->GetNumberOfFixedTextureUnits ());
- if ((mesh.tex_materials.size () > 1) && supported && (texture_units > 1))
+ int texture_units = tex_manager->GetNumberOfTextureUnits ();
+ if ((mesh.tex_materials.size () > 1) && (texture_units > 1))
{
if (texture_units < mesh.tex_materials.size ())
PCL_WARN ("[PCLVisualizer::addTextureMesh] GPU texture units %d < mesh textures %d!\n",
texture_units, mesh.tex_materials.size ());
// Load textures
- std::size_t last_tex_id = std::min (mesh.tex_materials.size (), texture_units);
+ std::size_t last_tex_id = std::min (static_cast<int> (mesh.tex_materials.size ()), texture_units);
int tu = vtkProperty::VTK_TEXTURE_UNIT_0;
std::size_t tex_id = 0;
while (tex_id < last_tex_id)
} // end of multi texturing
else
{
- if (!supported || texture_units < 2)
+ if ((mesh.tex_materials.size () > 1) && (texture_units < 2))
PCL_WARN ("[PCLVisualizer::addTextureMesh] Your GPU doesn't support multi texturing. "
"Will use first one only!\n");
while ((actor = actors->GetNextActor ()) != NULL)
{
actor->GetProperty ()->SetRepresentationToSurface ();
+ actor->GetProperty ()->SetLighting(true);
}
}
}
while ((actor = actors->GetNextActor ()) != NULL)
{
actor->GetProperty ()->SetRepresentationToWireframe ();
+ actor->GetProperty ()->SetLighting(false);
}
}
}
// Get camera positions
vtkPolyData *sphere = subdivide->GetOutput ();
- std::vector<Eigen::Vector3f> cam_positions;
+ std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > cam_positions;
if (!use_vertices)
{
vtkSmartPointer<vtkCellArray> cells_sphere = sphere->GetPolys ();
sphere->GetPoint (ptIds_com[2], p3_com);
vtkTriangle::TriangleCenter (p1_com, p2_com, p3_com, center);
cam_positions[i] = Eigen::Vector3f (float (center[0]), float (center[1]), float (center[2]));
+ cam_positions[i].normalize ();
i++;
}
double cam_pos[3];
sphere->GetPoint (i, cam_pos);
cam_positions[i] = Eigen::Vector3f (float (cam_pos[0]), float (cam_pos[1]), float (cam_pos[2]));
+ cam_positions[i].normalize ();
}
}
continue;
}
- vtkSmartPointer<vtkIdTypeArray> ids = vtkSmartPointer<vtkIdTypeArray>::New ();
+ vtkSmartPointer<vtkIdTypeArray> ids;
ids = vtkIdTypeArray::SafeDownCast(hdw_selection->GetNode(0)->GetSelectionList());
+ if (!ids)
+ return;
double visible_area = 0;
for (int sel_id = 0; sel_id < (ids->GetNumberOfTuples ()); sel_id++)
{
mat2 (i, j) = static_cast<float> (view_transform->Element[i][j]);
}
- mat1 = mat1.inverse ();
+ mat1 = mat1.inverse ().eval ();
int ptr = 0;
for (int y = 0; y < yres; ++y)
dheight * float (y) - 1.0f,
depth[ptr],
1.0f);
- world_coords = mat1 * world_coords;
+ world_coords = mat2 * mat1 * world_coords;
float w3 = 1.0f / world_coords[3];
world_coords[0] *= w3;
world_coords[1] *= -w3;
world_coords[2] *= -w3;
- world_coords = mat2 * world_coords;
pt.x = static_cast<float> (world_coords[0]);
pt.y = static_cast<float> (world_coords[1]);
pt.z = static_cast<float> (world_coords[2]);
pcl::visualization::PCLVisualizer::setPosition (int x, int y)
{
if (win_)
+ {
win_->SetPosition (x, y);
+ win_->Render ();
+ }
}
//////////////////////////////////////////////////////////////////////////////////////////////
pcl::visualization::PCLVisualizer::setSize (int xw, int yw)
{
if (win_)
+ {
win_->SetSize (xw, yw);
+ win_->Render ();
+ }
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::visualization::PCLVisualizer::setUseVbos (bool use_vbos)
{
+#if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
use_vbos_ = use_vbos;
style_->setUseVbos (use_vbos_);
+#else
+ PCL_WARN ("[PCLVisualizer::setUseVbos] Has no effect when OpenGL version is ≥ 2\n");
+#endif
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl::visualization::PCLVisualizer::setLookUpTableID (const std::string id)
+{
+ style_->lut_actor_id_ = id;
+ style_->updateLookUpTableDisplay (false);
}
//////////////////////////////////////////////////////////////////////////////////////////////
boost::filesystem::path path (argv[p_file_indices[i]]);
if (boost::filesystem::exists (path))
{
+#if BOOST_VERSION >= 104800
path = boost::filesystem::canonical (path);
+#endif
str = path.string ().c_str ();
sha1.process_bytes (str, std::strlen (str));
valid = true;
colors[cp * 3 + 1] = static_cast<unsigned char> (g_);
colors[cp * 3 + 2] = static_cast<unsigned char> (b_);
}
- reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0);
+ reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
return (true);
}
colors[cp * 3 + 1] = static_cast<unsigned char> (g_);
colors[cp * 3 + 2] = static_cast<unsigned char> (b_);
}
- reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0);
+ reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
return (true);
}
}
}
if (j != 0)
- reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, j, 0);
+ reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, j, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
else
reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (0);
//delete [] colors;
}
}
// Set array takes over allocation (Set save to 1 to keep the class from deleting the array when it cleans up or reallocates memory.)
- reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * j, 0);
+ reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * j, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
return (true);
}
j++;
}
}
- reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetArray (colors, j, 0);
+ reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetArray (colors, j, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
return (true);
}
}
}
if (j != 0)
- reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, j, 0);
+ reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, j, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
+ else
+ reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (0);
+ //delete [] colors;
+ return (true);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+pcl::visualization::PointCloudColorHandlerLabelField<pcl::PCLPointCloud2>::PointCloudColorHandlerLabelField (const pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloudConstPtr &cloud,
+ const bool static_mapping)
+: pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloudColorHandler (cloud)
+{
+ field_idx_ = pcl::getFieldIndex (*cloud, "label");
+ if (field_idx_ != -1)
+ capable_ = true;
+ else
+ capable_ = false;
+ static_mapping_ = static_mapping;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////
+bool
+pcl::visualization::PointCloudColorHandlerLabelField<pcl::PCLPointCloud2>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
+{
+ if (!capable_ || !cloud_)
+ return (false);
+
+ if (!scalars)
+ scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+ scalars->SetNumberOfComponents (3);
+
+ vtkIdType nr_points = cloud_->width * cloud_->height;
+ reinterpret_cast<vtkUnsignedCharArray*> (&(*scalars))->SetNumberOfTuples (nr_points);
+ // Allocate enough memory to hold all colors
+ unsigned char* colors = new unsigned char[nr_points * 3];
+
+ int j = 0;
+ int point_offset = cloud_->fields[field_idx_].offset;
+ const int field_size = pcl::getFieldSize (cloud_->fields[field_idx_].datatype);
+
+
+ std::map<uint32_t, pcl::RGB> colormap;
+ if (!static_mapping_)
+ {
+ std::set<uint32_t> labels;
+ // First pass: find unique labels
+ for (vtkIdType i = 0; i < nr_points; ++i, point_offset += cloud_->point_step)
+ {
+ uint32_t label;
+ memcpy (&label, &cloud_->data[point_offset], field_size);
+ labels.insert (label);
+ }
+
+ // Assign Glasbey colors in ascending order of labels
+ size_t color = 0;
+ for (std::set<uint32_t>::iterator iter = labels.begin (); iter != labels.end (); ++iter, ++color)
+ colormap[*iter] = GlasbeyLUT::at (color % GlasbeyLUT::size ());
+ }
+ // If XYZ present, check if the points are invalid
+ int x_idx = pcl::getFieldIndex (*cloud_, "x");
+ point_offset = cloud_->fields[field_idx_].offset;
+ if (x_idx != -1)
+ {
+ float x_data, y_data, z_data;
+ int x_point_offset = cloud_->fields[x_idx].offset;
+
+ // Color every point
+ for (vtkIdType cp = 0; cp < nr_points; ++cp,
+ point_offset += cloud_->point_step,
+ x_point_offset += cloud_->point_step)
+ {
+ uint32_t label;
+ memcpy (&label, &cloud_->data[point_offset], field_size);
+
+ memcpy (&x_data, &cloud_->data[x_point_offset], sizeof (float));
+ memcpy (&y_data, &cloud_->data[x_point_offset + sizeof (float)], sizeof (float));
+ memcpy (&z_data, &cloud_->data[x_point_offset + 2 * sizeof (float)], sizeof (float));
+
+ if (!pcl_isfinite (x_data) || !pcl_isfinite (y_data) || !pcl_isfinite (z_data))
+ continue;
+ const pcl::RGB& color = static_mapping_ ? GlasbeyLUT::at (label % GlasbeyLUT::size ()) : colormap[label];
+ colors[j ] = color.r;
+ colors[j + 1] = color.g;
+ colors[j + 2] = color.b;
+ j += 3;
+ }
+ }
+ // No XYZ data checks
+ else
+ {
+ // Color every point
+ for (vtkIdType cp = 0; cp < nr_points; ++cp, point_offset += cloud_->point_step)
+ {
+ uint32_t label;
+ memcpy (&label, &cloud_->data[point_offset], field_size);
+ const pcl::RGB& color = static_mapping_ ? GlasbeyLUT::at (label % GlasbeyLUT::size ()) : colormap[label];
+ colors[j ] = color.r;
+ colors[j + 1] = color.g;
+ colors[j + 2] = color.b;
+ j += 3;
+ }
+ }
+ if (j != 0)
+ reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, j, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
else
reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (0);
//delete [] colors;
// Instantiations of specific point types
#ifdef PCL_ONLY_CORE_POINT_TYPES
- PCL_INSTANTIATE(PointCloudGeometryHandlerXYZ, (pcl::PointSurfel)(pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)(pcl::PointNormal)(pcl::PointXYZRGBNormal)(pcl::PointXYZRGBL))
+ PCL_INSTANTIATE(PointCloudGeometryHandlerXYZ, (pcl::PointSurfel)(pcl::PointXYZ)(pcl::PointXYZL)(pcl::PointXYZI)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)(pcl::PointNormal)(pcl::PointXYZRGBNormal)(pcl::PointXYZRGBL)(pcl::PointWithRange))
PCL_INSTANTIATE(PointCloudGeometryHandlerSurfaceNormal, (pcl::Normal)(pcl::PointNormal)(pcl::PointXYZRGBNormal))
#else
PCL_INSTANTIATE(PointCloudGeometryHandlerXYZ, PCL_XYZ_POINT_TYPES)
bool
pcl::visualization::context_items::Circle::Paint (vtkContext2D *painter)
{
- painter->GetPen ()->SetColor (colors);
- painter->GetBrush ()->SetColor (colors);
+ painter->GetBrush ()->SetColor (colors[0], colors[1], colors[2], static_cast<unsigned char> ((255.0 * GetOpacity ())));
+ painter->GetPen ()->SetColor (colors[0], colors[1], colors[2], static_cast<unsigned char> ((255.0 * GetOpacity ())));
painter->DrawWedge (params[0], params[1], params[2], params[3], 0.0, 360.0);
return (true);
}
bool
pcl::visualization::context_items::Disk::Paint (vtkContext2D *painter)
{
- painter->GetBrush ()->SetColor (colors);
- painter->GetPen ()->SetColor (colors);
+ painter->GetBrush ()->SetColor (colors[0], colors[1], colors[2], static_cast<unsigned char> ((255.0 * GetOpacity ())));
+ painter->GetPen ()->SetColor (colors[0], colors[1], colors[2], static_cast<unsigned char> ((255.0 * GetOpacity ())));
painter->DrawEllipse (params[0], params[1], params[2], params[2]);
return (true);
}
bool
pcl::visualization::context_items::Rectangle::Paint (vtkContext2D *painter)
{
- painter->GetPen ()->SetColor (colors);
+ painter->GetPen ()->SetColor (colors[0], colors[1], colors[2], static_cast<unsigned char> ((255.0 * GetOpacity ())));
float p[] =
{
params[0], params[1],
bool
pcl::visualization::context_items::FilledRectangle::Paint (vtkContext2D *painter)
{
- painter->GetBrush ()->SetColor (colors);
- painter->GetPen ()->SetColor (colors);
+ painter->GetBrush ()->SetColor (colors[0], colors[1], colors[2], static_cast<unsigned char> ((255.0 * GetOpacity ())));
+ painter->GetPen ()->SetColor (colors[0], colors[1], colors[2], static_cast<unsigned char> ((255.0 * GetOpacity ())));
painter->DrawRect (params[0], params[1], params[2], params[3]);
return (true);
}
bool
pcl::visualization::context_items::Line::Paint (vtkContext2D *painter)
{
- painter->GetPen ()->SetColor (colors);
+ painter->GetPen ()->SetColor (colors[0], colors[1], colors[2], static_cast<unsigned char> ((255.0 * GetOpacity ())));
painter->DrawLine (params[0], params[1], params[2], params[3]);
return (true);
}
bool
pcl::visualization::context_items::Polygon::Paint (vtkContext2D *painter)
{
- painter->GetBrush ()->SetColor (colors);
- painter->GetPen ()->SetColor (colors);
+ painter->GetBrush ()->SetColor (colors[0], colors[1], colors[2], static_cast<unsigned char> ((255.0 * GetOpacity ())));
+ painter->GetPen ()->SetColor (colors[0], colors[1], colors[2], static_cast<unsigned char> ((255.0 * GetOpacity ())));
painter->DrawPolygon (¶ms[0], static_cast<int> (params.size () / 2));
return (true);
}
bool
pcl::visualization::context_items::Point::Paint (vtkContext2D *painter)
{
- painter->GetPen ()->SetColor (colors);
+ painter->GetPen ()->SetColor (colors[0], colors[1], colors[2], static_cast<unsigned char> ((255.0 * GetOpacity ())));
painter->DrawPoint (params[0], params[1]);
return (true);
}
bool
pcl::visualization::context_items::Points::Paint (vtkContext2D *painter)
{
- painter->GetPen ()->SetColor (colors);
+ painter->GetPen ()->SetColor (colors[0], colors[1], colors[2], static_cast<unsigned char> ((255.0 * GetOpacity ())));
painter->DrawPoints (¶ms[0], static_cast<int> (params.size () / 2));
return (true);
}
{
vtkTextProperty *text_property = painter->GetTextProp ();
text_property->SetColor (255.0 * colors[0], 255.0 * colors[1], 255.0 * colors[2]);
+ text_property->SetOpacity (GetOpacity ());
text_property->SetFontFamilyToArial ();
text_property->SetFontSize (10);
text_property->SetJustificationToLeft ();
size = 2.3 * painter->GetPen ()->GetWidth ();
painter->GetPen ()->SetWidth (size);
- painter->GetPen ()->SetColor (colors);
+ painter->GetPen ()->SetColor (colors[0], colors[1], colors[2], static_cast<unsigned char> ((255.0 * GetOpacity ())));
painter->DrawPointSprites (0, ¶ms[0], nb_points);
painter->GetPen ()->SetWidth (1);
- painter->GetPen ()->SetColor (point_colors);
+ painter->GetPen ()->SetColor (point_colors[0], point_colors[1], point_colors[2], static_cast<unsigned char> ((255.0 * GetOpacity ())));
painter->DrawPointSprites (0, ¶ms[0], nb_points);
return (true);
}
PCL_ADD_EXECUTABLE(pcl_hdl_viewer_simple ${SUBSYS_NAME} hdl_viewer_simple.cpp)
target_link_libraries(pcl_hdl_viewer_simple pcl_io pcl_common pcl_visualization)
-if(OPENNI_FOUND AND BUILD_OPENNI)
+PCL_ADD_EXECUTABLE(pcl_vlp_viewer ${SUBSYS_NAME} vlp_viewer.cpp)
+target_link_libraries(pcl_vlp_viewer pcl_io pcl_common pcl_visualization)
+
+if(WITH_OPENNI)
PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_pcd_grabber_viewer ${SUBSYS_NAME} pcd_grabber_viewer.cpp)
target_link_libraries(pcl_pcd_grabber_viewer pcl_common pcl_io pcl_kdtree pcl_visualization)
target_link_libraries(pcl_openni_image pcl_common pcl_io pcl_kdtree pcl_visualization)
endif()
-if(OPENNI2_FOUND AND BUILD_OPENNI2)
+if(WITH_OPENNI2)
PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni2_viewer ${SUBSYS_NAME} openni2_viewer.cpp)
target_link_libraries(pcl_openni2_viewer pcl_common pcl_io pcl_kdtree pcl_visualization)
endif()
+
+if(WITH_ENSENSO)
+ PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ensenso_viewer ${SUBSYS_NAME} ensenso_viewer.cpp)
+ target_link_libraries(pcl_ensenso_viewer pcl_common pcl_io pcl_visualization)
+endif()
+
+if(WITH_DAVIDSDK)
+ PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_davidsdk_viewer ${SUBSYS_NAME} davidsdk_viewer.cpp)
+ target_link_libraries(pcl_davidsdk_viewer pcl_common pcl_io pcl_visualization)
+endif()
+
+if(WITH_DSSDK)
+ PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_depth_sense_viewer ${SUBSYS_NAME} depth_sense_viewer.cpp)
+ target_link_libraries(pcl_depth_sense_viewer pcl_common pcl_io pcl_visualization)
+endif()
+
+if(WITH_RSSDK)
+ PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_real_sense_viewer ${SUBSYS_NAME} real_sense_viewer.cpp)
+ target_link_libraries(pcl_real_sense_viewer pcl_common pcl_io pcl_visualization)
+endif()
+
--- /dev/null
+/*\r
+ * Software License Agreement (BSD License)\r
+ *\r
+ * Point Cloud Library (PCL) - www.pointclouds.org\r
+ * Copyright (c) 2014-, Open Perception, Inc.\r
+ *\r
+ * All rights reserved.\r
+ *\r
+ * Redistribution and use in source and binary forms, with or without\r
+ * modification, are permitted provided that the following conditions\r
+ * are met:\r
+ *\r
+ * * Redistributions of source code must retain the above copyright\r
+ * notice, this list of conditions and the following disclaimer.\r
+ * * Redistributions in binary form must reproduce the above\r
+ * copyright notice, this list of conditions and the following\r
+ * disclaimer in the documentation and/or other materials provided\r
+ * with the distribution.\r
+ * * Neither the name of the copyright holder(s) nor the names of its\r
+ * contributors may be used to endorse or promote products derived\r
+ * from this software without specific prior written permission.\r
+ *\r
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\r
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\r
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\r
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\r
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\r
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\r
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\r
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\r
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\r
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\r
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\r
+ * POSSIBILITY OF SUCH DAMAGE.\r
+ *\r
+ * Author: Victor Lamoine (victor.lamoine@gmail.com)\r
+ */\r
+\r
+#include <iostream>\r
+#include <pcl/io/davidsdk_grabber.h>\r
+#include <pcl/visualization/cloud_viewer.h>\r
+\r
+/** @brief Convenience typedef */\r
+typedef pcl::visualization::CloudViewer CloudViewer;\r
+\r
+/** @brief Convenience typedef for XYZ point clouds */\r
+typedef pcl::PointCloud<pcl::PointXYZ> PointCloudXYZ;\r
+\r
+/** @brief CloudViewer pointer */\r
+boost::shared_ptr<CloudViewer> viewer_ptr;\r
+\r
+/** @brief PCL DavidSDK object pointer */\r
+pcl::DavidSDKGrabber::Ptr davidsdk_ptr;\r
+\r
+/** @brief Process and/or display DavidSDK grabber clouds\r
+ * @param[in] cloud DavidSDK cloud */\r
+void\r
+grabberCallback (const PointCloudXYZ::Ptr& cloud)\r
+{\r
+ if (!viewer_ptr->wasStopped ())\r
+ viewer_ptr->showCloud (cloud);\r
+}\r
+\r
+/** @brief Main function\r
+ * @param[in] argc\r
+ * @param[in] argv\r
+ * @return Exit status */\r
+int\r
+main (int argc,\r
+ char *argv[])\r
+{\r
+ if (argc != 2)\r
+ {\r
+ PCL_ERROR ("Usage:\n%s 192.168.100.65\n", argv[0]);\r
+ return (-1);\r
+ }\r
+\r
+ viewer_ptr.reset (new CloudViewer ("davidSDK 3D cloud viewer"));\r
+ davidsdk_ptr.reset (new pcl::DavidSDKGrabber);\r
+ davidsdk_ptr->connect (argv[1]);\r
+\r
+ if (!davidsdk_ptr->isConnected ())\r
+ return (-1);\r
+ PCL_WARN ("davidSDK connected\n");\r
+\r
+#ifndef _WIN32// || _WIN64\r
+ PCL_WARN ("Linux / Mac OSX detected, setting local_path_ to /var/tmp/davidsdk/ and remote_path_ to \\\\m6700\\davidsdk\\\n");\r
+ davidsdk_ptr->setLocalAndRemotePaths ("/var/tmp/davidsdk/", "\\\\m6700\\davidsdk\\");\r
+#endif\r
+\r
+ //davidsdk_ptr->setFileFormatToPLY();\r
+ std::cout << "Using " << davidsdk_ptr->getFileFormat () << " file format" << std::endl;\r
+\r
+ boost::function<void\r
+ (const PointCloudXYZ::Ptr&)> f = boost::bind (&grabberCallback, _1);\r
+ davidsdk_ptr->registerCallback (f);\r
+ davidsdk_ptr->start ();\r
+\r
+ while (!viewer_ptr->wasStopped ())\r
+ {\r
+ boost::this_thread::sleep (boost::posix_time::seconds (20));\r
+ std::cout << "FPS: " << davidsdk_ptr->getFramesPerSecond () << std::endl;\r
+ }\r
+\r
+ davidsdk_ptr->stop ();\r
+ return (0);\r
+}\r
+\r
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2014-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <iostream>
+
+#include <boost/thread/mutex.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/format.hpp>
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/common/time.h>
+#include <pcl/console/print.h>
+#include <pcl/console/parse.h>
+#include <pcl/io/io_exception.h>
+#include <pcl/io/depth_sense_grabber.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+using namespace pcl::console;
+
+void
+printHelp (int, char **argv)
+{
+ std::cout << std::endl;
+ std::cout << "****************************************************************************" << std::endl;
+ std::cout << "* *" << std::endl;
+ std::cout << "* DEPTH SENSE VIEWER - Usage Guide *" << std::endl;
+ std::cout << "* *" << std::endl;
+ std::cout << "****************************************************************************" << std::endl;
+ std::cout << std::endl;
+ std::cout << "Usage: " << argv[0] << " [Options] device_id" << std::endl;
+ std::cout << std::endl;
+ std::cout << "Options:" << std::endl;
+ std::cout << std::endl;
+ std::cout << " --help, -h : Show this help" << std::endl;
+ std::cout << " --list, -l : List connected DepthSense devices" << std::endl;
+ std::cout << " --xyz : View XYZ-only clouds" << std::endl;
+ std::cout << std::endl;
+ std::cout << "Keyboard commands:" << std::endl;
+ std::cout << std::endl;
+ std::cout << " When the focus is on the viewer window, the following keyboard commands" << std::endl;
+ std::cout << " are available:" << std::endl;
+ std::cout << " * t/T : increase or decrease depth data confidence threshold" << std::endl;
+ std::cout << " * k : enable next temporal filtering method" << std::endl;
+ std::cout << " * s : save the last grabbed cloud to disk" << std::endl;
+ std::cout << " * h : print the list of standard PCL viewer commands" << std::endl;
+ std::cout << std::endl;
+ std::cout << "Notes:" << std::endl;
+ std::cout << std::endl;
+ std::cout << " The device to grab data from is selected using device_id argument. It" << std::endl;
+ std::cout << " could be either:" << std::endl;
+ std::cout << " * serial number (e.g. YZVF0780239000261D)" << std::endl;
+ std::cout << " * device index (e.g. #2 for the second connected device)" << std::endl;
+ std::cout << std::endl;
+ std::cout << " If device_id is not given, then the first available device will be used." << std::endl;
+ std::cout << std::endl;
+}
+
+void
+printDeviceList ()
+{
+ typedef boost::shared_ptr<pcl::DepthSenseGrabber> DepthSenseGrabberPtr;
+ std::vector<DepthSenseGrabberPtr> grabbers;
+ std::cout << "Connected devices: ";
+ boost::format fmt ("\n #%i %s");
+ while (true)
+ {
+ try
+ {
+ grabbers.push_back (DepthSenseGrabberPtr (new pcl::DepthSenseGrabber));
+ std::cout << boost::str (fmt % grabbers.size () % grabbers.back ()->getDeviceSerialNumber ());
+ }
+ catch (pcl::io::IOException& e)
+ {
+ break;
+ }
+ }
+ if (grabbers.size ())
+ std::cout << std::endl;
+ else
+ std::cout << "none" << std::endl;
+}
+
+template <typename PointT>
+class DepthSenseViewer
+{
+
+ public:
+
+ typedef pcl::PointCloud<PointT> PointCloudT;
+
+ DepthSenseViewer (pcl::DepthSenseGrabber& grabber)
+ : grabber_ (grabber)
+ , viewer_ ("DepthSense Viewer")
+ , threshold_ (50)
+ , window_ (5)
+ , temporal_filtering_ (pcl::DepthSenseGrabber::DepthSense_None)
+ {
+ viewer_.registerKeyboardCallback (&DepthSenseViewer::keyboardCallback, *this);
+ }
+
+ ~DepthSenseViewer ()
+ {
+ connection_.disconnect ();
+ }
+
+ void
+ run ()
+ {
+ boost::function<void (const typename PointCloudT::ConstPtr&)> f = boost::bind (&DepthSenseViewer::cloudCallback, this, _1);
+ connection_ = grabber_.registerCallback (f);
+ grabber_.start ();
+ while (!viewer_.wasStopped ())
+ {
+ if (new_cloud_)
+ {
+ boost::mutex::scoped_lock lock (new_cloud_mutex_);
+ if (!viewer_.updatePointCloud (new_cloud_, "cloud"))
+ {
+ viewer_.addPointCloud (new_cloud_, "cloud");
+ viewer_.resetCamera ();
+ }
+ displaySettings ();
+ last_cloud_ = new_cloud_;
+ new_cloud_.reset ();
+ }
+ viewer_.spinOnce (1, true);
+ }
+ grabber_.stop ();
+ }
+
+ private:
+
+ void
+ cloudCallback (typename PointCloudT::ConstPtr cloud)
+ {
+ if (!viewer_.wasStopped ())
+ {
+ boost::mutex::scoped_lock lock (new_cloud_mutex_);
+ new_cloud_ = cloud;
+ }
+ }
+
+ void
+ keyboardCallback (const pcl::visualization::KeyboardEvent& event, void*)
+ {
+ if (event.keyDown ())
+ {
+ if (event.getKeyCode () == 'w' || event.getKeyCode () == 'W')
+ {
+ window_ += event.getKeyCode () == 'w' ? 1 : -1;
+ if (window_ < 1)
+ window_ = 1;
+ pcl::console::print_info ("Temporal filtering window size: ");
+ pcl::console::print_value ("%i\n", window_);
+ grabber_.enableTemporalFiltering (temporal_filtering_, window_);
+ }
+ if (event.getKeyCode () == 't' || event.getKeyCode () == 'T')
+ {
+ threshold_ += event.getKeyCode () == 't' ? 10 : -10;
+ if (threshold_ < 0)
+ threshold_ = 0;
+ pcl::console::print_info ("Confidence threshold: ");
+ pcl::console::print_value ("%i\n", threshold_);
+ grabber_.setConfidenceThreshold (threshold_);
+ }
+ if (event.getKeyCode () == 'k')
+ {
+ pcl::console::print_info ("Temporal filtering: ");
+ switch (temporal_filtering_)
+ {
+ case pcl::DepthSenseGrabber::DepthSense_None:
+ {
+ temporal_filtering_ = pcl::DepthSenseGrabber::DepthSense_Median;
+ pcl::console::print_value ("median\n");
+ break;
+ }
+ case pcl::DepthSenseGrabber::DepthSense_Median:
+ {
+ temporal_filtering_ = pcl::DepthSenseGrabber::DepthSense_Average;
+ pcl::console::print_value ("average\n");
+ break;
+ }
+ case pcl::DepthSenseGrabber::DepthSense_Average:
+ {
+ temporal_filtering_ = pcl::DepthSenseGrabber::DepthSense_None;
+ pcl::console::print_value ("none\n");
+ break;
+ }
+ }
+ grabber_.enableTemporalFiltering (temporal_filtering_, window_);
+ }
+ if (event.getKeyCode () == 's')
+ {
+ boost::format fmt ("DS_%s_%u.pcd");
+ std::string fn = boost::str (fmt % grabber_.getDeviceSerialNumber ().c_str () % last_cloud_->header.stamp);
+ pcl::io::savePCDFileBinaryCompressed (fn, *last_cloud_);
+ pcl::console::print_info ("Saved point cloud: ");
+ pcl::console::print_value (fn.c_str ());
+ pcl::console::print_info ("\n");
+ }
+ displaySettings ();
+ }
+ }
+
+ void displaySettings ()
+ {
+ const int dx = 5;
+ const int dy = 14;
+ const int fs = 10;
+ boost::format name_fmt ("text%i");
+ const char* TF[] = {"off", "median", "average"};
+ std::vector<boost::format> entries;
+ // Framerate
+ entries.push_back(boost::format("framerate: %.1f") % grabber_.getFramesPerSecond());
+ // Confidence threshold
+ entries.push_back (boost::format ("confidence threshold: %i") % threshold_);
+ // Temporal filter settings
+ std::string tfs = boost::str (boost::format (", window size %i") % window_);
+ entries.push_back (boost::format ("temporal filtering: %s%s") % TF[temporal_filtering_] % (temporal_filtering_ == pcl::DepthSenseGrabber::DepthSense_None ? "" : tfs));
+ for (size_t i = 0; i < entries.size (); ++i)
+ {
+ std::string name = boost::str (name_fmt % i);
+ std::string entry = boost::str (entries[i]);
+ if (!viewer_.updateText (entry, dx, dy + i * (fs + 2), fs, 1.0, 1.0, 1.0, name))
+ viewer_.addText (entry, dx, dy + i * (fs + 2), fs, 1.0, 1.0, 1.0, name);
+ }
+ }
+
+ pcl::DepthSenseGrabber& grabber_;
+ pcl::visualization::PCLVisualizer viewer_;
+ boost::signals2::connection connection_;
+
+ int threshold_;
+ int window_;
+ pcl::DepthSenseGrabber::TemporalFilteringType temporal_filtering_;
+
+ mutable boost::mutex new_cloud_mutex_;
+ typename PointCloudT::ConstPtr new_cloud_;
+ typename PointCloudT::ConstPtr last_cloud_;
+
+};
+
+
+int
+main (int argc, char** argv)
+{
+ print_info ("Viewer for DepthSense devices (run with --help for more information)\n", argv[0]);
+
+ if (find_switch (argc, argv, "--help") || find_switch (argc, argv, "-h"))
+ {
+ printHelp (argc, argv);
+ return (0);
+ }
+
+ if (find_switch (argc, argv, "--list") || find_switch (argc, argv, "-l"))
+ {
+ printDeviceList ();
+ return (0);
+ }
+
+ bool xyz_only = find_switch (argc, argv, "--xyz");
+
+ std::string device_id;
+
+ if (argc == 1 || // no arguments
+ (argc == 2 && xyz_only)) // single argument, and it is --xyz
+ {
+ device_id = "";
+ print_info ("Creating a grabber for the first available device\n");
+ }
+ else
+ {
+ device_id = argv[argc - 1];
+ print_info ("Creating a grabber for device \"%s\"\n", device_id.c_str ());
+ }
+
+ try
+ {
+ pcl::DepthSenseGrabber grabber (device_id);
+ if (xyz_only)
+ {
+ DepthSenseViewer<pcl::PointXYZ> viewer (grabber);
+ viewer.run ();
+ }
+ else
+ {
+ DepthSenseViewer<pcl::PointXYZRGBA> viewer (grabber);
+ viewer.run ();
+ }
+ }
+ catch (pcl::io::IOException& e)
+ {
+ print_error ("Failed to create a grabber: %s\n", e.what ());
+ return (1);
+ }
+
+ return (0);
+}
+
--- /dev/null
+/*\r
+ * Software License Agreement (BSD License)\r
+ *\r
+ * Point Cloud Library (PCL) - www.pointclouds.org\r
+ * Copyright (c) 2014-, Open Perception, Inc.\r
+ *\r
+ * All rights reserved.\r
+ *\r
+ * Redistribution and use in source and binary forms, with or without\r
+ * modification, are permitted provided that the following conditions\r
+ * are met:\r
+ *\r
+ * * Redistributions of source code must retain the above copyright\r
+ * notice, this list of conditions and the following disclaimer.\r
+ * * Redistributions in binary form must reproduce the above\r
+ * copyright notice, this list of conditions and the following\r
+ * disclaimer in the documentation and/or other materials provided\r
+ * with the distribution.\r
+ * * Neither the name of the copyright holder(s) nor the names of its\r
+ * contributors may be used to endorse or promote products derived\r
+ * from this software without specific prior written permission.\r
+ *\r
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\r
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\r
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\r
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\r
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\r
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\r
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\r
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\r
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\r
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\r
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\r
+ * POSSIBILITY OF SUCH DAMAGE.\r
+ *\r
+ * Author: Victor Lamoine (victor.lamoine@gmail.com)\r
+ */\r
+\r
+#include <iostream>\r
+#include <pcl/io/ensenso_grabber.h>\r
+#include <pcl/visualization/cloud_viewer.h>\r
+\r
+/** @brief Convenience typedef */\r
+typedef pcl::visualization::CloudViewer CloudViewer;\r
+\r
+/** @brief Convenience typedef for XYZ point clouds */\r
+typedef pcl::PointCloud<pcl::PointXYZ> PointCloudXYZ;\r
+\r
+/** @brief CloudViewer pointer */\r
+boost::shared_ptr<CloudViewer> viewer_ptr;\r
+\r
+/** @brief PCL Ensenso object pointer */\r
+pcl::EnsensoGrabber::Ptr ensenso_ptr;\r
+\r
+/** @brief Process and/or display Ensenso grabber clouds\r
+ * @param[in] cloud Ensenso cloud */\r
+void\r
+grabberCallback (const PointCloudXYZ::Ptr& cloud)\r
+{\r
+ if (!viewer_ptr->wasStopped ())\r
+ viewer_ptr->showCloud (cloud);\r
+}\r
+\r
+/** @brief Main function\r
+ * @return Exit status */\r
+int\r
+main (void)\r
+{\r
+ viewer_ptr.reset (new CloudViewer ("Ensenso 3D cloud viewer"));\r
+ ensenso_ptr.reset (new pcl::EnsensoGrabber);\r
+ ensenso_ptr->openTcpPort ();\r
+ ensenso_ptr->openDevice ();\r
+\r
+ boost::function<void\r
+ (const PointCloudXYZ::Ptr&)> f = boost::bind (&grabberCallback, _1);\r
+ ensenso_ptr->registerCallback (f);\r
+ ensenso_ptr->start ();\r
+\r
+ while (!viewer_ptr->wasStopped ())\r
+ {\r
+ boost::this_thread::sleep (boost::posix_time::milliseconds (1000));\r
+ std::cout << "FPS: " << ensenso_ptr->getFramesPerSecond () << std::endl;\r
+ }\r
+\r
+ ensenso_ptr->closeDevice ();\r
+ return (0);\r
+}\r
+\r
print_info ("\t-dir directory_path = directory path to image or pclzf file(s) to be read from\n");
print_info ("\t-fps frequency = frames per second\n");
print_info ("\t-pclzf = Load pclzf files instead\n");
- print_info ("\t-repeat = optional parameter that tells wheter the TIFF file(s) should be \"grabbed\" in a endless loop.\n");
+ print_info ("\t-repeat = optional parameter that tells whether the TIFF file(s) should be \"grabbed\" in a endless loop.\n");
print_info ("\n");
print_info ("\t-cam (*) = use given camera settings as initial view\n");
print_info (stderr, " (*) [Clipping Range / Focal Point / Position / ViewUp / Distance / Window Size / Window Pos] or use a <filename.cam> that contains the same information.\n");
*
*/
+#ifndef Q_MOC_RUN
#include <boost/thread/thread.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
+#endif
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/time.h> //fps calculations
print_info (" -file file_name = PCD file to be read from\n");
print_info (" -dir directory_path = directory path to PCD file(s) to be read from\n");
print_info (" -fps frequency = frames per second\n");
- print_info (" -repeat = optional parameter that tells wheter the PCD file(s) should be \"grabbed\" in a endless loop.\n");
+ print_info (" -repeat = optional parameter that tells whether the PCD file(s) should be \"grabbed\" in a endless loop.\n");
print_info ("\n");
print_info (" -cam (*) = use given camera settings as initial view\n");
print_info (stderr, " (*) [Clipping Range / Focal Point / Position / ViewUp / Distance / Window Size / Window Pos] or use a <filename.cam> that contains the same information.\n");
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/image_viewer.h>
#include <pcl/visualization/histogram_visualizer.h>
-#if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
+#if VTK_MAJOR_VERSION>=6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
#include <pcl/visualization/pcl_plotter.h>
#endif
#include <pcl/visualization/point_picking_event.h>
print_info (" -ps X = point size ("); print_value ("1..64"); print_info (") \n");
print_info (" -opaque X = rendered point cloud opacity ("); print_value ("0..1"); print_info (")\n");
print_info (" -shading X = rendered surface shading ("); print_value ("'flat' (default), 'gouraud', 'phong'"); print_info (")\n");
+ print_info (" -position x,y,z = absolute point cloud position in metres\n");
+ print_info (" -orientation r,p,y = absolute point cloud orientation (roll, pitch, yaw) in radians\n");
print_info (" -ax "); print_value ("n"); print_info (" = enable on-screen display of ");
print_color (stdout, TT_BRIGHT, TT_RED, "X"); print_color (stdout, TT_BRIGHT, TT_GREEN, "Y"); print_color (stdout, TT_BRIGHT, TT_BLUE, "Z");
print_info ("\n");
print_info (" -use_point_picking = enable the usage of picking points on screen (default "); print_value ("disabled"); print_info (")\n");
print_info ("\n");
+ print_info (" -optimal_label_colors = maps existing labels to the optimal sequential glasbey colors, label_ids will not be mapped to fixed colors (default "); print_value ("disabled"); print_info (")\n");
+ print_info ("\n");
- print_info ("\n(Note: for multiple .pcd files, provide multiple -{fc,ps,opaque} parameters; they will be automatically assigned to the right file)\n");
+ print_info ("\n(Note: for multiple .pcd files, provide multiple -{fc,ps,opaque,position,orientation} parameters; they will be automatically assigned to the right file)\n");
}
// Global visualizer object
-#if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
+#if VTK_MAJOR_VERSION>=6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
pcl::visualization::PCLPlotter ph_global;
#endif
boost::shared_ptr<pcl::visualization::PCLVisualizer> p;
if (!isMultiDimensionalFeatureField (cloud->fields[i]))
continue;
PCL_INFO ("Multidimensional field found: %s\n", cloud->fields[i].name.c_str ());
-#if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
+#if VTK_MAJOR_VERSION>=6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
ph_global.addFeatureHistogram (*cloud, cloud->fields[i].name, idx, ss.str ());
ph_global.renderOnce ();
#endif
std::vector<double> fcolor_r, fcolor_b, fcolor_g;
bool fcolorparam = pcl::console::parse_multiple_3x_arguments (argc, argv, "-fc", fcolor_r, fcolor_g, fcolor_b);
+ std::vector<double> pose_x, pose_y, pose_z, pose_roll, pose_pitch, pose_yaw;
+ bool poseparam = pcl::console::parse_multiple_3x_arguments (argc, argv, "-position", pose_x, pose_y, pose_z);
+ poseparam &= pcl::console::parse_multiple_3x_arguments (argc, argv, "-orientation", pose_roll, pose_pitch, pose_yaw);
+
std::vector<int> psize;
pcl::console::parse_multiple_arguments (argc, argv, "-ps", psize);
if (use_pp)
print_highlight ("Point picking enabled.\n");
+ bool use_optimal_l_colors = pcl::console::find_switch (argc, argv, "-optimal_label_colors");
+ if (use_optimal_l_colors)
+ print_highlight ("Optimal glasbey colors are being assigned to existing labels.\nNote: No static mapping between label ids and colors\n");
+
// If VBOs are not enabled, then try to use immediate rendering
bool use_immediate_rendering = false;
if (!use_vbos)
shadings.push_back ("flat");
// Create the PCLVisualizer object
-#if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
+#if VTK_MAJOR_VERSION>=6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
boost::shared_ptr<pcl::visualization::PCLPlotter> ph;
#endif
// Using min_p, max_p to set the global Y min/max range for the histogram
if (pcd.read (argv[p_file_indices.at (i)], *cloud, origin, orientation, version) < 0)
return (-1);
+ // Calculate transform if available.
+ if (pose_x.size () > i && pose_y.size () > i && pose_z.size () > i &&
+ pose_roll.size () > i && pose_pitch.size () > i && pose_yaw.size () > i)
+ {
+ Eigen::Affine3f pose =
+ Eigen::Translation3f (Eigen::Vector3f (pose_x[i], pose_y[i], pose_z[i])) *
+ Eigen::AngleAxisf (pose_yaw[i], Eigen::Vector3f::UnitZ ()) *
+ Eigen::AngleAxisf (pose_pitch[i], Eigen::Vector3f::UnitY ()) *
+ Eigen::AngleAxisf (pose_roll[i], Eigen::Vector3f::UnitX ());
+ orientation = pose.rotation () * orientation;
+ origin.block<3, 1> (0, 0) = (pose * Eigen::Translation3f (origin.block<3, 1> (0, 0))).translation ();
+ }
+
std::stringstream cloud_name;
// ---[ Special check for 1-point multi-dimension histograms
{
cloud_name << argv[p_file_indices.at (i)];
-#if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
+#if VTK_MAJOR_VERSION>=6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
if (!ph)
ph.reset (new pcl::visualization::PCLPlotter);
#endif
pcl::getMinMax (*cloud, 0, cloud->fields[0].name, min_p, max_p);
-#if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
+#if VTK_MAJOR_VERSION>=6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
ph->addFeatureHistogram (*cloud, cloud->fields[0].name, cloud_name.str ());
#endif
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud->fields[0].count); print_info (" points]\n");
p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, cloud_name_normals_pc.str ());
p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc.str ());
cloud_name_normals_pc << "-pc";
- p->addPointCloudPrincipalCurvatures (cloud_xyz, cloud_normals, cloud_pc, factor, pc_scale, cloud_name_normals_pc.str (), viewport);
+ p->addPointCloudPrincipalCurvatures<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, cloud_pc, factor, pc_scale, cloud_name_normals_pc.str (), viewport);
p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc.str ());
}
// Add every dimension as a possible color
if (!fcolorparam)
{
- int idx = 0;
+ int rgb_idx = 0;
+ int label_idx = 0;
for (size_t f = 0; f < cloud->fields.size (); ++f)
{
if (cloud->fields[f].name == "rgb" || cloud->fields[f].name == "rgba")
{
- idx = f + 1;
+ rgb_idx = f + 1;
color_handler.reset (new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PCLPointCloud2> (cloud));
}
+ else if (cloud->fields[f].name == "label")
+ {
+ label_idx = f + 1;
+ color_handler.reset (new pcl::visualization::PointCloudColorHandlerLabelField<pcl::PCLPointCloud2> (cloud, !use_optimal_l_colors));
+ }
else
{
if (!isValidFieldName (cloud->fields[f].name))
//p->addPointCloud<pcl::PointXYZ> (cloud_xyz, color_handler, cloud_name.str (), viewport);
p->addPointCloud (cloud, color_handler, origin, orientation, cloud_name.str (), viewport);
}
- // Set RGB color handler as default
- p->updateColorHandlerIndex (cloud_name.str (), idx);
+ // Set RGB color handler or label handler as default
+ p->updateColorHandlerIndex (cloud_name.str (), (rgb_idx ? rgb_idx : label_idx));
}
// Additionally, add normals as a handler
bool stopped = false;
do
{
-#if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
+#if VTK_MAJOR_VERSION>=6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
if (ph) ph->spinOnce ();
#endif
else
{
// If no images, continue
-#if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
+#if VTK_MAJOR_VERSION>=6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
if (ph)
{
//print_highlight ("Setting the global Y range for all histograms to: "); print_value ("%f -> %f\n", min_p, max_p);
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2015-, Open Perception, Inc.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder(s) nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <iostream>
+
+#include <boost/format.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/thread/mutex.hpp>
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/common/time.h>
+#include <pcl/console/print.h>
+#include <pcl/console/parse.h>
+#include <pcl/io/io_exception.h>
+#include <pcl/io/real_sense_grabber.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+
+using namespace pcl::console;
+
+void
+printHelp (int, char **argv)
+{
+ std::cout << std::endl;
+ std::cout << "****************************************************************************" << std::endl;
+ std::cout << "* *" << std::endl;
+ std::cout << "* REAL SENSE VIEWER - Usage Guide *" << std::endl;
+ std::cout << "* *" << std::endl;
+ std::cout << "****************************************************************************" << std::endl;
+ std::cout << std::endl;
+ std::cout << "Usage: " << argv[0] << " [Options] device_id" << std::endl;
+ std::cout << std::endl;
+ std::cout << "Options:" << std::endl;
+ std::cout << std::endl;
+ std::cout << " --help, -h : Show this help" << std::endl;
+ std::cout << " --list, -l : List connected RealSense devices and supported modes" << std::endl;
+ std::cout << " --mode <id> : Use capture mode <id> from the list of supported modes" << std::endl;
+ std::cout << std::endl;
+ std::cout << "Keyboard commands:" << std::endl;
+ std::cout << std::endl;
+ std::cout << " When the focus is on the viewer window, the following keyboard commands" << std::endl;
+ std::cout << " are available:" << std::endl;
+ std::cout << " * t/T : increase or decrease depth data confidence threshold" << std::endl;
+ std::cout << " * k : enable next temporal filtering method" << std::endl;
+ std::cout << " * s : save the last grabbed cloud to disk" << std::endl;
+ std::cout << " * h : print the list of standard PCL viewer commands" << std::endl;
+ std::cout << std::endl;
+ std::cout << "Notes:" << std::endl;
+ std::cout << std::endl;
+ std::cout << " The device to grab data from is selected using device_id argument. It" << std::endl;
+ std::cout << " could be either:" << std::endl;
+ std::cout << " * serial number (e.g. 231400041-03)" << std::endl;
+ std::cout << " * device index (e.g. #2 for the second connected device)" << std::endl;
+ std::cout << std::endl;
+ std::cout << " If device_id is not given, then the first available device will be used." << std::endl;
+ std::cout << std::endl;
+ std::cout << " If capture mode is not given, then the grabber will try to enable both" << std::endl;
+ std::cout << " depth and color streams at VGA resolution and 30 Hz framerate. If this" << std::endl;
+ std::cout << " particular mode is not available, the one that most closely matches this" << std::endl;
+ std::cout << " specification will be chosen." << std::endl;
+ std::cout << std::endl;
+}
+
+void
+printDeviceList ()
+{
+ typedef boost::shared_ptr<pcl::RealSenseGrabber> RealSenseGrabberPtr;
+ std::vector<RealSenseGrabberPtr> grabbers;
+ std::cout << "Connected devices: ";
+ boost::format fmt ("\n #%i %s");
+ boost::format fmt_dm ("\n %2i) %d Hz %dx%d Depth");
+ boost::format fmt_dcm ("\n %2i) %d Hz %dx%d Depth %dx%d Color");
+ while (true)
+ {
+ try
+ {
+ grabbers.push_back (RealSenseGrabberPtr (new pcl::RealSenseGrabber));
+ std::cout << boost::str (fmt % grabbers.size () % grabbers.back ()->getDeviceSerialNumber ());
+ std::vector<pcl::RealSenseGrabber::Mode> xyz_modes = grabbers.back ()->getAvailableModes (true);
+ std::cout << "\n Depth modes:";
+ if (xyz_modes.size ())
+ for (size_t i = 0; i < xyz_modes.size (); ++i)
+ std::cout << boost::str (fmt_dm % (i + 1) % xyz_modes[i].fps % xyz_modes[i].depth_width % xyz_modes[i].depth_height);
+ else
+ {
+ std::cout << " none";
+ }
+ std::vector<pcl::RealSenseGrabber::Mode> xyzrgba_modes = grabbers.back ()->getAvailableModes (false);
+ std::cout << "\n Depth + color modes:";
+ if (xyz_modes.size ())
+ for (size_t i = 0; i < xyzrgba_modes.size (); ++i)
+ {
+ const pcl::RealSenseGrabber::Mode& m = xyzrgba_modes[i];
+ std::cout << boost::str (fmt_dcm % (i + xyz_modes.size () + 1) % m.fps % m.depth_width % m.depth_height % m.color_width % m.color_height);
+ }
+ else
+ std::cout << " none";
+ }
+ catch (pcl::io::IOException& e)
+ {
+ break;
+ }
+ }
+ if (grabbers.size ())
+ std::cout << std::endl;
+ else
+ std::cout << "none" << std::endl;
+}
+
+template <typename PointT>
+class RealSenseViewer
+{
+
+ public:
+
+ typedef pcl::PointCloud<PointT> PointCloudT;
+
+ RealSenseViewer (pcl::RealSenseGrabber& grabber)
+ : grabber_ (grabber)
+ , viewer_ ("RealSense Viewer")
+ , window_ (3)
+ , threshold_ (6)
+ , temporal_filtering_ (pcl::RealSenseGrabber::RealSense_None)
+ {
+ viewer_.setCameraFieldOfView (0.785398); // approximately 45 degrees
+ viewer_.setCameraPosition (0, 0, 0, 0, 0, 1, 0, 1, 0);
+ viewer_.registerKeyboardCallback (&RealSenseViewer::keyboardCallback, *this);
+ viewer_.registerPointPickingCallback (&RealSenseViewer::pointPickingCallback, *this);
+ }
+
+ ~RealSenseViewer ()
+ {
+ connection_.disconnect ();
+ }
+
+ void
+ run ()
+ {
+ boost::function<void (const typename PointCloudT::ConstPtr&)> f = boost::bind (&RealSenseViewer::cloudCallback, this, _1);
+ connection_ = grabber_.registerCallback (f);
+ grabber_.start ();
+ printMode (grabber_.getMode ());
+ viewer_.setSize (grabber_.getMode ().depth_width, grabber_.getMode ().depth_height);
+ while (!viewer_.wasStopped ())
+ {
+ if (new_cloud_)
+ {
+ boost::mutex::scoped_lock lock (new_cloud_mutex_);
+ if (!viewer_.updatePointCloud (new_cloud_, "cloud"))
+ {
+ viewer_.addPointCloud (new_cloud_, "cloud");
+ viewer_.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");
+ }
+ displaySettings ();
+ last_cloud_ = new_cloud_;
+ new_cloud_.reset ();
+ }
+ viewer_.spinOnce (1, true);
+ }
+ grabber_.stop ();
+ }
+
+ private:
+
+ void
+ cloudCallback (typename PointCloudT::ConstPtr cloud)
+ {
+ if (!viewer_.wasStopped ())
+ {
+ boost::mutex::scoped_lock lock (new_cloud_mutex_);
+ new_cloud_ = cloud;
+ }
+ }
+
+ void
+ keyboardCallback (const pcl::visualization::KeyboardEvent& event, void*)
+ {
+ if (event.keyDown ())
+ {
+ if (event.getKeyCode () == 'w' || event.getKeyCode () == 'W')
+ {
+ window_ += event.getKeyCode () == 'w' ? 1 : -1;
+ if (window_ < 1)
+ window_ = 1;
+ pcl::console::print_info ("Temporal filtering window size: ");
+ pcl::console::print_value ("%i\n", window_);
+ grabber_.enableTemporalFiltering (temporal_filtering_, window_);
+ }
+ if (event.getKeyCode () == 't' || event.getKeyCode () == 'T')
+ {
+ threshold_ += event.getKeyCode () == 't' ? 1 : -1;
+ if (threshold_ < 0)
+ threshold_ = 0;
+ if (threshold_ > 15)
+ threshold_ = 15;
+ pcl::console::print_info ("Confidence threshold: ");
+ pcl::console::print_value ("%i\n", threshold_);
+ grabber_.setConfidenceThreshold (threshold_);
+ }
+ if (event.getKeyCode () == 'k')
+ {
+ pcl::console::print_info ("Temporal filtering: ");
+ switch (temporal_filtering_)
+ {
+ case pcl::RealSenseGrabber::RealSense_None:
+ {
+ temporal_filtering_ = pcl::RealSenseGrabber::RealSense_Median;
+ pcl::console::print_value ("median\n");
+ break;
+ }
+ case pcl::RealSenseGrabber::RealSense_Median:
+ {
+ temporal_filtering_ = pcl::RealSenseGrabber::RealSense_Average;
+ pcl::console::print_value ("average\n");
+ break;
+ }
+ case pcl::RealSenseGrabber::RealSense_Average:
+ {
+ temporal_filtering_ = pcl::RealSenseGrabber::RealSense_None;
+ pcl::console::print_value ("none\n");
+ break;
+ }
+ }
+ grabber_.enableTemporalFiltering (temporal_filtering_, window_);
+ }
+ if (event.getKeyCode () == 's')
+ {
+ boost::format fmt ("RS_%s_%u.pcd");
+ std::string fn = boost::str (fmt % grabber_.getDeviceSerialNumber ().c_str () % last_cloud_->header.stamp);
+ pcl::io::savePCDFileBinaryCompressed (fn, *last_cloud_);
+ pcl::console::print_info ("Saved point cloud: ");
+ pcl::console::print_value (fn.c_str ());
+ pcl::console::print_info ("\n");
+ }
+ displaySettings ();
+ }
+ }
+
+ void
+ pointPickingCallback (const pcl::visualization::PointPickingEvent& event, void*)
+ {
+ float x, y, z;
+ event.getPoint (x, y, z);
+ pcl::console::print_info ("Picked point at ");
+ pcl::console::print_value ("%.3f", x); pcl::console::print_info (", ");
+ pcl::console::print_value ("%.3f", y); pcl::console::print_info (", ");
+ pcl::console::print_value ("%.3f\n", z);
+ }
+
+ void
+ displaySettings ()
+ {
+ const int dx = 5;
+ const int dy = 14;
+ const int fs = 10;
+ boost::format name_fmt ("text%i");
+ const char* TF[] = {"off", "median", "average"};
+ std::vector<boost::format> entries;
+ // Framerate
+ entries.push_back (boost::format ("framerate: %.1f") % grabber_.getFramesPerSecond ());
+ // Confidence threshold
+ entries.push_back (boost::format ("confidence threshold: %i") % threshold_);
+ // Temporal filter settings
+ std::string tfs = boost::str (boost::format (", window size %i") % window_);
+ entries.push_back (boost::format ("temporal filtering: %s%s") % TF[temporal_filtering_] % (temporal_filtering_ == pcl::RealSenseGrabber::RealSense_None ? "" : tfs));
+ for (size_t i = 0; i < entries.size (); ++i)
+ {
+ std::string name = boost::str (name_fmt % i);
+ std::string entry = boost::str (entries[i]);
+ if (!viewer_.updateText (entry, dx, dy + i * (fs + 2), fs, 1.0, 1.0, 1.0, name))
+ viewer_.addText (entry, dx, dy + i * (fs + 2), fs, 1.0, 1.0, 1.0, name);
+ }
+ }
+
+ void
+ printMode (const pcl::RealSenseGrabber::Mode& mode)
+ {
+ print_info ("Capturing mode: ");
+ print_value ("%i", mode.fps);
+ print_info (" Hz ");
+ print_value ("%dx%d ", mode.depth_width, mode.depth_height);
+ print_info ("Depth");
+ if (pcl::traits::has_color<PointT>::value)
+ {
+ print_value (" %dx%d ", mode.color_width, mode.color_height);
+ print_info ("Color");
+ }
+ print_value ("\n");
+ }
+
+ pcl::RealSenseGrabber& grabber_;
+ pcl::visualization::PCLVisualizer viewer_;
+ boost::signals2::connection connection_;
+
+ int window_;
+ int threshold_;
+ pcl::RealSenseGrabber::TemporalFilteringType temporal_filtering_;
+
+ mutable boost::mutex new_cloud_mutex_;
+ typename PointCloudT::ConstPtr new_cloud_;
+ typename PointCloudT::ConstPtr last_cloud_;
+
+};
+
+int
+main (int argc, char** argv)
+{
+ print_info ("Viewer for RealSense devices (run with --help for more information)\n", argv[0]);
+
+ if (find_switch (argc, argv, "--help") || find_switch (argc, argv, "-h"))
+ {
+ printHelp (argc, argv);
+ return (0);
+ }
+
+ if (find_switch (argc, argv, "--list") || find_switch (argc, argv, "-l"))
+ {
+ printDeviceList ();
+ return (0);
+ }
+
+ unsigned int mode_id = 0;
+ bool with_mode = find_argument(argc, argv, "--mode") != -1;
+ parse_argument(argc, argv, "--mode", mode_id);
+
+ std::string device_id;
+
+ if (argc == 1 || // no arguments
+ (argc == 3 && with_mode)) // single argument, and it is --mode <id>
+ {
+ device_id = "";
+ print_info ("Creating a grabber for the first available device\n");
+ }
+ else
+ {
+ device_id = argv[argc - 1];
+ print_info ("Creating a grabber for device "); print_value ("%s\n", device_id.c_str ());
+ }
+
+ try
+ {
+ pcl::RealSenseGrabber grabber (device_id);
+ std::vector<pcl::RealSenseGrabber::Mode> xyz_modes = grabber.getAvailableModes (true);
+ std::vector<pcl::RealSenseGrabber::Mode> xyzrgba_modes = grabber.getAvailableModes (false);
+ if (mode_id == 0)
+ {
+ RealSenseViewer<pcl::PointXYZRGBA> viewer (grabber);
+ viewer.run ();
+ }
+ else if (mode_id <= xyz_modes.size ())
+ {
+ grabber.setMode (xyz_modes[mode_id - 1], true);
+ RealSenseViewer<pcl::PointXYZ> viewer (grabber);
+ viewer.run ();
+ }
+ else if (mode_id <= xyz_modes.size () + xyzrgba_modes.size ())
+ {
+ grabber.setMode (xyzrgba_modes[mode_id - xyz_modes.size () - 1], true);
+ RealSenseViewer<pcl::PointXYZRGBA> viewer (grabber);
+ viewer.run ();
+ }
+ else
+ {
+ print_error ("Requested a mode (%i) that is not in the list of supported by this device\n", mode_id);
+ return (1);
+ }
+ }
+ catch (pcl::io::IOException& e)
+ {
+ print_error ("Failed to create a grabber: %s\n", e.what ());
+ return (1);
+ }
+
+ return (0);
+}
+
--- /dev/null
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Point Cloud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2015-, Open Perception, Inc.
+ * Copyright (c) 2015, The MITRE Corporation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Keven Ring <keven@mitre.org>
+ */
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/common/time.h> //fps calculations
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/hdl_grabber.h>
+#include <pcl/io/vlp_grabber.h>
+#include <pcl/visualization/point_cloud_color_handlers.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/visualization/image_viewer.h>
+#include <pcl/io/openni_camera/openni_driver.h>
+#include <pcl/console/parse.h>
+#include <pcl/visualization/boost.h>
+#include <pcl/visualization/mouse_event.h>
+#include <vector>
+#include <string>
+#include <boost/algorithm/string.hpp>
+#include <typeinfo>
+
+using namespace std;
+using namespace pcl;
+using namespace pcl::console;
+using namespace pcl::visualization;
+
+#define SHOW_FPS 0
+#if SHOW_FPS
+#define FPS_CALC(_WHAT_) \
+do \
+{ \
+ static unsigned count = 0;\
+ static double last = getTime ();\
+ double now = getTime (); \
+ ++count; \
+ if (now - last >= 1.0) \
+ { \
+ std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
+ count = 0; \
+ last = now; \
+ } \
+}while(false)
+#else
+#define FPS_CALC(_WHAT_) \
+do \
+{ \
+}while(false)
+#endif
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointType>
+class SimpleVLPViewer
+{
+ public:
+ typedef PointCloud<PointType> Cloud;
+ typedef typename Cloud::ConstPtr CloudConstPtr;
+ typedef typename Cloud::Ptr CloudPtr;
+
+ SimpleVLPViewer (Grabber& grabber,
+ PointCloudColorHandler<PointType> *handler) :
+ cloud_viewer_ (new PCLVisualizer ("PCL VLP Cloud")),
+ grabber_ (grabber),
+ handler_ (handler)
+ {
+ }
+
+ void
+ cloud_callback (const CloudConstPtr& cloud)
+ {
+ FPS_CALC("cloud callback");
+ boost::mutex::scoped_lock lock (cloud_mutex_);
+ cloud_ = cloud;
+ }
+
+ void
+ keyboard_callback (const KeyboardEvent& event,
+ void* /*cookie*/)
+ {
+ if (event.keyUp ())
+ {
+ switch (event.getKeyCode ())
+ {
+ case '0':
+ delete handler_;
+ handler_ = new PointCloudColorHandlerCustom<PointXYZI> (255, 255, 255);
+ break;
+ case '1':
+ delete handler_;
+ handler_ = new PointCloudColorHandlerGenericField<PointXYZI> ("x");
+ break;
+ case '2':
+ delete handler_;
+ handler_ = new PointCloudColorHandlerGenericField<PointXYZI> ("y");
+ break;
+ case '3':
+ delete handler_;
+ handler_ = new PointCloudColorHandlerGenericField<PointXYZI> ("z");
+ break;
+ case '4':
+ delete handler_;
+ handler_ = new PointCloudColorHandlerGenericField<PointXYZI> ("intensity");
+ break;
+ case 'a':
+ cloud_viewer_->removeAllCoordinateSystems ();
+ cloud_viewer_->addCoordinateSystem (1.0, "global");
+ break;
+ case 'A':
+ cloud_viewer_->removeAllCoordinateSystems ();
+ break;
+ }
+ }
+ }
+
+ void
+ run ()
+ {
+ cloud_viewer_->addCoordinateSystem (1.0, "global");
+ cloud_viewer_->setBackgroundColor (0, 0, 0);
+ cloud_viewer_->initCameraParameters ();
+ cloud_viewer_->setCameraPosition (0.0, 0.0, 30.0, 0.0, 1.0, 0.0, 0);
+ cloud_viewer_->setCameraClipDistances (0.0, 50.0);
+ cloud_viewer_->registerKeyboardCallback (&SimpleVLPViewer::keyboard_callback, *this);
+
+ boost::function<void
+ (const CloudConstPtr&)> cloud_cb = boost::bind (&SimpleVLPViewer::cloud_callback, this, _1);
+ boost::signals2::connection cloud_connection = grabber_.registerCallback (cloud_cb);
+
+ grabber_.start ();
+
+ while (!cloud_viewer_->wasStopped ())
+ {
+ CloudConstPtr tmp, cloud;
+
+ if (cloud_mutex_.try_lock ())
+ {
+ cloud_.swap (cloud);
+ cloud_mutex_.unlock ();
+ }
+
+ if (cloud)
+ {
+ FPS_CALC("drawing cloud");
+ handler_->setInputCloud (cloud);
+ if (!cloud_viewer_->updatePointCloud (cloud, *handler_, "VLP"))
+ cloud_viewer_->addPointCloud (cloud, *handler_, "VLP");
+
+ cloud_viewer_->spinOnce ();
+ }
+
+ if (!grabber_.isRunning ())
+ cloud_viewer_->spin ();
+
+ boost::this_thread::sleep (boost::posix_time::microseconds (100));
+ }
+
+ grabber_.stop ();
+
+ cloud_connection.disconnect ();
+ }
+
+ boost::shared_ptr<PCLVisualizer> cloud_viewer_;
+ boost::shared_ptr<ImageViewer> image_viewer_;
+
+ Grabber& grabber_;
+ boost::mutex cloud_mutex_;
+ boost::mutex image_mutex_;
+
+ CloudConstPtr cloud_;
+ PointCloudColorHandler<PointType> *handler_;
+};
+
+void
+usage (char ** argv)
+{
+ cout << "usage: " << argv[0] << " [-pcapFile <path-to-pcap-file>] [-h | --help]" << endl;
+ cout << argv[0] << " -h | --help : shows this help" << endl;
+ return;
+}
+
+int
+main (int argc,
+ char ** argv)
+{
+ std::string pcapFile;
+
+ if (find_switch (argc, argv, "-h") || find_switch (argc, argv, "--help"))
+ {
+ usage (argv);
+ return (0);
+ }
+
+ parse_argument (argc, argv, "-pcapFile", pcapFile);
+
+ VLPGrabber grabber (pcapFile);
+
+ PointCloudColorHandlerGenericField<PointXYZI> *color_handler = new PointCloudColorHandlerGenericField<PointXYZI> ("intensity");
+
+ SimpleVLPViewer<PointXYZI> v (grabber, color_handler);
+ v.run ();
+
+ return (0);
+}
+
projects / pcl.git / commitdiff
![doc/tutorials/content/images/creative_camera.jpg](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=doc/tutorials/content/images/creative_camera.jpg;h=fa0fa6d059134de6bf609166e7b14b11731d1699;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}
![doc/tutorials/content/images/davidsdk/david.png](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=doc/tutorials/content/images/davidsdk/david.png;h=a09443b8a2da02ca5da1a8fd34a9dbe4e1474e0b;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}
![doc/tutorials/content/images/davidsdk/davidsdk_viewer.jpg](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=doc/tutorials/content/images/davidsdk/davidsdk_viewer.jpg;h=82da909a8849905b9860f8b1747b5def190034c0;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}
![doc/tutorials/content/images/ensenso/ensenso_viewer.jpg](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=doc/tutorials/content/images/ensenso/ensenso_viewer.jpg;h=f7c05b23cf8dfa84d775c1a7c4115651acd667dc;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}
![doc/tutorials/content/images/ensenso/ids.png](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=doc/tutorials/content/images/ensenso/ids.png;h=c55bf6e04644070f616a4e900f0e4549f3a461bd;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}
![doc/tutorials/content/images/global_hypothesis_verification/multiple.png](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=doc/tutorials/content/images/global_hypothesis_verification/multiple.png;h=1c989ec9b4429993f6050c355033a4993d6e5e7c;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}
![doc/tutorials/content/images/global_hypothesis_verification/original.png](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=doc/tutorials/content/images/global_hypothesis_verification/original.png;h=aea55efdb613ea08649588768bf2ca4f53dbfefd;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}
![doc/tutorials/content/images/global_hypothesis_verification/single.png](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=doc/tutorials/content/images/global_hypothesis_verification/single.png;h=4d07c1b12e040c2d08f5d5e71f377400dd783cbb;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}
![doc/tutorials/content/images/posix_building_pcl/boost_logo.png](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=doc/tutorials/content/images/posix_building_pcl/boost_logo.png;h=0a2ffd571cfa4079024e9fa8902c0ed5f265e82e;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}
![doc/tutorials/content/images/posix_building_pcl/cuda_logo.png](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=doc/tutorials/content/images/posix_building_pcl/cuda_logo.png;h=3ae451c416541f5cd24f90fb89c17c925f8542dd;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}
![doc/tutorials/content/images/posix_building_pcl/eigen_logo.png](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=doc/tutorials/content/images/posix_building_pcl/eigen_logo.png;h=ed86079a5ab0f4e61977795717be936e4fa66be1;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}
![doc/tutorials/content/images/posix_building_pcl/flann_logo.png](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=doc/tutorials/content/images/posix_building_pcl/flann_logo.png;h=ebc23cd5e2c1c97ee73b30bf0358390547acfcdb;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}
![doc/tutorials/content/images/posix_building_pcl/openni_logo.png](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=doc/tutorials/content/images/posix_building_pcl/openni_logo.png;h=7c05b5c418a2ff7e59e0fe99ad23b5cf3d43194c;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}
![doc/tutorials/content/images/posix_building_pcl/qhull_logo.png](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=doc/tutorials/content/images/posix_building_pcl/qhull_logo.png;h=ccbd326e9b6997721399df2e26e4e2537a187c1c;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}
![doc/tutorials/content/images/posix_building_pcl/vtk_logo.png](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=doc/tutorials/content/images/posix_building_pcl/vtk_logo.png;h=83a638d3ef2f28768eb5d0e39c3f0c08ccec28dd;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}
![doc/tutorials/content/images/qt_colorize_cloud/colorize_cloud.gif](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=doc/tutorials/content/images/qt_colorize_cloud/colorize_cloud.gif;h=037f2e58ad17aec495917ac06ecb5c6c7f087c26;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}
![doc/tutorials/content/images/qt_colorize_cloud/ui.png](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=doc/tutorials/content/images/qt_colorize_cloud/ui.png;h=ac71ad502dbd4b22cecc2ba537b36e32ce81e1d3;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}
![test/2d/gauss_smooth.png](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=test/2d/gauss_smooth.png;h=7568c6da588dbcc0df01d6e59ac3181c5924c3cf;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}
![test/2d/lena-grayscale.png](/"http:/www.example.com/cgi/"https:/www.github.com/"bookmarks://"{url}?p=pcl.git;a=blob;f=test/2d/lena-grayscale.png;h=9ac73d825dcaed20283001e3bb28a9d5a26fae31;hb=cff328462e82392bce32c3c549a259a22c105ae6){kind=link}